int read_mat_matrix(Matrix* M, const char* file_name, const char* varname)
{
MATFile* matf = NULL;
mxArray* Mx = NULL;
int res;
unsigned int i,j;
double* data;
assert(file_name);
assert(M);
assert(varname);
/* let's open the file*/
matf = matOpen(file_name, "r" );
if(!matf) {
return ERROR_IO;
}
/* let's read the variable varname */
Mx = matGetVariable(matf, varname);
if (!Mx) {
matClose(matf);
return ERROR_IO_MAT;
}
/* let's close the file */
res = matClose(matf);
assert(!res);
/* check: the variable must be a 2D matrix!! */
if(mxGetNumberOfDimensions(Mx) > 2) {
matClose(matf);
return ERROR_IO_MAT;
}
/* let's create the Matrix */
M->n_rows = (unsigned int) mxGetM(Mx);
M->n_cols = (unsigned int) mxGetN(Mx);
res = create_matrix(M, M->n_rows, M->n_cols);
if (res) {
mxDestroyArray(Mx);
return res;
}
/* copy the data from the mxArray. Remember that matlab save in column-order!! */
data = mxGetPr(Mx);
for (j=0; j < M->n_cols; ++j) {
for (i=0; i < M->n_rows; ++i) {
matrix_set(M, i, j, (float) *data);
++data;
}
}
/* destroy mxArray*/
mxDestroyArray(Mx);
return 0;
}
void Transmission::LoadTransmission() {
MATFile *matFileHandle;
matFileHandle = matOpen(transmissionFileName, "r");
int numberOfDirectories;
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
std::vector<double> transmissionDataEntry;
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
transmissionDataEntry.push_back(matrixEntry);
}
transmissionData.push_back(transmissionDataEntry);
}
gearRatios = transmissionData[0];
gearEfficiency = transmissionData[1];
gearInertias = transmissionData[2];
//std::cout<<"Transmission data successfully loaded"<<std::endl;
if (matClose(matFileHandle) != 0) {
std::cout<<"Error closing file"<<'\n';
}
}
int test()
{
string str_file_name = "E:\\research\\working\\test_data\\test_gk_means.mat";
SMatrix<double> matZ;
Vector<SMatrix<double> > vecmatDictionary;
SMatrix<CodeType> mat_target;
int num_dic;
int is_initialize;
int num_grouped;
{
MATFile* fp = matOpen(str_file_name.c_str(), "r");
SMART_ASSERT(fp).Exit();
mexConvert(matGetVariable(fp, "Z"), matZ);
mexConvert(matGetVariable(fp, "all_D"), vecmatDictionary);
mexConvert(matGetVariable(fp, "num_sub_dic_each_partition"), num_dic);
mexConvert(matGetVariable(fp, "mat_compact_B"), mat_target);
mxArray * para_encode = matGetVariable(fp, "para_encode");
mexConvert(mxGetField(para_encode, 0, "is_initialize"), is_initialize);
mexConvert(mxGetField(para_encode, 0, "num_grouped"), num_grouped);
matClose(fp);
}
IndexEncoding ie;
ie.SetIsInitialize(is_initialize);
ie.SetNumberGroup(num_grouped);
ie.SetEncodingType(Type_gk_means);
ie.SetEncodingType(Type_additive_quantization);
SMatrix<CodeType> matRepresentation;
matRepresentation.AllocateSpace(mat_target.Rows(), mat_target.Cols());
int num_selected_rows = 10;
matRepresentation.AllocateSpace(num_selected_rows, mat_target.Cols());
ie.Solve(SMatrix<double>(matZ.Ptr(), num_selected_rows, matZ.Cols()),
vecmatDictionary,
num_dic,
matRepresentation);
PRINT << "good\n";
for (int i = 0; i < matRepresentation.Rows(); i++)
{
for (int j = 0; j < matRepresentation.Cols(); j++)
{
cout << (int)(matRepresentation[i][j]) << "\t";
}
cout << "\n";
}
//for (int i = 0; i < mat_target.Rows(); i++)
//{
// for (int j = 0; j < mat_target.Cols(); j++)
// {
// SMART_ASSERT(matRepresentation[i][j] == mat_target[i][j]).Exit();
// }
//}
return 0;
}
////////////////////////////////////////////////////////////////////////
//
// create a MAT file from a double passed in
//
int createMAT(const char *file,
double *data,
int datalen)
{
MATFile *pmat;
mxArray *pa1;
printf("Creating a .MAT file %s...\n", file);
pmat = matOpen(file, "w");
if (pmat == NULL)
{
printf("Error creating file %s\n", file);
printf("(do you have write permission in this directory?)\n");
return(1);
}
pa1 = mxCreateDoubleMatrix(1,datalen,mxREAL);
mxSetName(pa1, "iroro_debug_variable");
memcpy((char *)(mxGetPr(pa1)),(char *)data, 1*datalen*sizeof(double));
matPutArray(pmat, pa1);
// clean up
mxDestroyArray(pa1);
if (matClose(pmat) != 0) {
printf("Error closing file %s\n",file);
return(1);
}
printf("Done\n");
return(0);
}
pair<double, double> ProbabilisticWvmClassifier::loadSigmoidParamsFromMatlab(const string& thresholdsFilename)
{
// Load sigmoid stuff:
double logisticA, logisticB;
MATFile *pmatfile;
mxArray *pmxarray; // =mat
pmatfile = matOpen(thresholdsFilename.c_str(), "r");
if (pmatfile == 0) {
throw invalid_argument("ProbabilisticWvmClassifier: Unable to open the thresholds-file: " + thresholdsFilename);
}
//TODO is there a case (when svm+wvm from same trainingdata) when there exists only a posterior_svm, and I should use this here?
pmxarray = matGetVariable(pmatfile, "posterior_wrvm");
if (pmxarray == 0) {
throw runtime_error("ProbabilisticWvmClassifier: Unable to find the vector posterior_wrvm. If you don't want probabilistic output, don't use a probabilistic classifier.");
//logisticA = logisticB = 0; // TODO: Does it make sense to continue?
} else {
double* matdata = mxGetPr(pmxarray);
const mwSize *dim = mxGetDimensions(pmxarray);
if (dim[1] != 2) {
throw runtime_error("ProbabilisticWvmClassifier: Size of vector posterior_wrvm !=2. If you don't want probabilistic output, don't use a probabilistic classifier.");
//logisticA = logisticB = 0; // TODO: Does it make sense to continue?
} else {
logisticB = matdata[0];
logisticA = matdata[1];
}
// Todo: delete *matdata and *dim??
mxDestroyArray(pmxarray);
}
matClose(pmatfile);
return make_pair(logisticA, logisticB);
}
void IO_MLabWriteDoubleImg(char *fileName, char *nameImg, double *img, int rows, int cols)
{
MATFile *fp;
int i, j, dims[2];
double *ptr;
mxArray *mxArr;
dims[0] = rows;
dims[1] = cols;
fp = matOpen(fileName, "u");
mxArr = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
ptr = mxGetPr(mxArr);
for(j=0; j<cols; j++)
for(i=0; i<rows; i++)
*(ptr ++) = img[i*cols + j];
/*mxSetName(mxArr, nameImg);
matPutArray(fp, mxArr);*/
matPutVariable(fp, nameImg, mxArr);
matClose(fp);
mxDestroyArray(mxArr);
}
void IO_MLabCreateFile(char *fileName)
{
MATFile *fp;
fp = matOpen(fileName, "w");
matClose(fp);
}
void MatlabWriter::done(PointTableRef table)
{
int result = matClose(m_matfile);
if (result != 0)
{
std::stringstream ss;
ss << "Unsuccessful write: " << m_filename;
throw pdal_error(ss.str());
}
}
void oz::matfile_write( const std::map<std::string, cpu_image> vars, const std::string& path ) {
MATFile *pmat = 0;
mxArray *pa = 0;
try {
pmat = matOpen(path.c_str(), "w");
if (!pmat)
OZ_X() << "Creating file '" << path << "' failed!";
for (std::map<std::string, cpu_image>::const_iterator i = vars.begin(); i != vars.end(); ++i) {
std::string name = i->first;
std::replace(name.begin(), name.end(), '-', '_');
const oz::cpu_image& img = i->second;
if (img.format() != FMT_FLOAT) OZ_X() << "Invalid format!";
pa = mxCreateNumericMatrix(img.w(), img.h(), mxSINGLE_CLASS, mxREAL);
if (!pa)
OZ_X() << "Creation of matrix for '" << name << "'failed!";
float *p = (float*)mxGetData(pa);
for (unsigned i = 0; i < img.w(); ++i) {
float *q = img.ptr<float>() + i;
for (unsigned j = 0; j < img.h(); ++j) {
*p++ = *q;
q += img.pitch() / sizeof(float);
}
}
if (matPutVariable(pmat, name.c_str(), pa) != 0)
OZ_X() << "Putting variable '" << name << "'failed!";
mxDestroyArray(pa);
}
matClose(pmat);
}
catch (std::exception&) {
if (pmat) matClose(pmat);
if (pa) mxDestroyArray(pa);
throw;
}
}
PetscErrorCode PetscViewerDestroy_Matlab(PetscViewer v)
{
PetscErrorCode ierr;
PetscViewer_Matlab *vf = (PetscViewer_Matlab*)v->data;
PetscFunctionBegin;
if (vf->ep) matClose(vf->ep);
ierr = PetscFree(vf);
CHKERRQ(ierr);
PetscFunctionReturn(0);
}
void CMATLAB::matPut(string filename,const ArrayXd &X,string Xname)
{
MATFile *pmat=matOpen(filename,string("w"));
if (!pmat) throw "CMATLAB::exportTo error: matOpen failed";
mxArray *pa=mxCreateDoubleMatrix((int)X.size(),1);
if (!pa) throw "CMATLAB::exportTo error: mxCreateDoubleMatrix failed";
memcpy((void *)(mxGetPr(pa)), (void *)X.data(), X.size()*sizeof(double));
if (!matPutVariable(pmat,string("X"),pa)) throw "CMATLAB::exportTo error: matlab.matPutVariable failed";
mxDestroyArray(pa);
if (!matClose(pmat)) throw "CMATLAB::exportTo error: matlab.matClose failed";
}
/*-------------------------------------------------------------------*/
void m2pvme_upkarray_rest(int nlhs, mxArray *plhs[], int nrhs, mxArrayIn *prhs[]) {
/*-------------------------------------------------------------------*/
MATFile *pmat;
char fname[256];
mxArray *array_ptr;
plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
if((mxGetPr(plhs[1]))[0] = (double) pvme_upkarray_rest(&plhs[0],(char*)0)==1) {
/* the data contains Matlab user defined objects! */
/* save and load this mxArray so as it will be recognized as an object */
/*
object passing could be done by getting a default object from the workspace
and then constructing the object in the workspace directly by using
mxPutArray with each struct for each level...
*/
/*
array_ptr = mexGetArray("def","base");
mxSetName(array_ptr, "defcopy");
mexPutArray(array_ptr, "base");
mxDestroyArray(array_ptr);
*/
sprintf(fname,"%s/dp_%d.mat",TMP_LOC,pvm_mytid());
/*printf(fname,"%s/dp_%d.mat",TMP_LOC,pvm_mytid());*/
pmat = matOpen(fname, "w");
if (pmat == NULL) {
mexErrMsgTxt("m2libpvme:Error creating file for transferring object\n");
}
mxSetName(plhs[0], "objsl");
if (matPutArray(pmat, plhs[0]) != 0) {
mexErrMsgTxt("m2libpvme:m2pvme_upkarray_rest: Error saving temporary intermediate file for objects\n");
}
if (matClose(pmat) != 0) {
mexErrMsgTxt("m2libpvme:m2pvme_upkarray_rest:Error temporary intermediate file for object transfer\n");
}
/* printf("class::::%s\n", mxGetClassName(plhs[0]));*/
if ( !( plhs[0] = mxCreateString(fname) ) )
mexErrMsgTxt("m2pvme_upkarray_rest(): mxCreateString() failed.\n");
}
return;
}
void utility::writeMatlabFile(mat armaMatrix, const char *varname, const char *filename)
{
MATFile *pmat;
mxArray *matlabMatrix;
matlabMatrix = mxCreateDoubleMatrix(armaMatrix.n_rows, armaMatrix.n_cols, mxREAL);
armadillo2matlabMatrix(&armaMatrix, matlabMatrix, armaMatrix.n_elem);
pmat=matOpen(filename, "w");
matPutVariable(pmat, varname, matlabMatrix);
matClose(pmat);
}
void CMATLAB::matGet(string filename,ArrayXd &X,string Xname)
{
MATFile *pmat=matOpen(filename,string("r"));
if (!pmat) throw "CMATLAB::importFrom error: matlab.matOpen failed";
mxArray *pa=matGetVariable(pmat,Xname);
if (!pa) throw "CMATLAB::importFrom error: matlab.matGetVariable failed";
int N=mxGetNumberOfElements(pa);
if (N<=0) throw "CMATLAB::importFrom error: matlab.mxGetNumberOfElements failed";
X.resize(N);
memcpy((void *)(mxGetPr(pa)), (void *)X.data(), sizeof((double *)X.data()));
mxDestroyArray(pa);
if (!matClose(pmat)) throw "CMATLAB::importFrom error: matlab.matClose failed";
}
pair<double, double> ProbabilisticSvmClassifier::loadSigmoidParamsFromMatlab(const string& logisticFilename)
{
Logger logger = Loggers->getLogger("classification");
#ifdef WITH_MATLAB_CLASSIFIER
// Load sigmoid stuff:
double logisticA = 0;
double logisticB = 0;
MATFile *pmatfile;
mxArray *pmxarray; // =mat
pmatfile = matOpen(logisticFilename.c_str(), "r");
if (pmatfile == 0) {
throw runtime_error("ProbabilisticSvmClassifier: Unable to open the logistic file to read the logistic parameters (wrong format?):" + logisticFilename);
}
else {
//read posterior_wrvm parameter for probabilistic WRVM output
//TODO 2012: is there a case (when svm+wvm from same trainingdata) when there exists only a posterior_svm, and I should use this here?
//TODO new2013: the posterior_svm is the same for all thresholds file, so the thresholds file is not needed for SVMs. The posterior_svm should go into the classifier.mat!
pmxarray = matGetVariable(pmatfile, "posterior_svm");
if (pmxarray == 0) {
std::cout << "[DetSVM] WARNING: Unable to find the vector posterior_svm, disable prob. SVM output;" << std::endl;
// TODO prob. output cannot be disabled in the new version of this lib -> throw exception or log info message or something
logisticA = logisticB = 0;
}
else {
double* matdata = mxGetPr(pmxarray);
const mwSize *dim = mxGetDimensions(pmxarray);
if (dim[1] != 2) {
std::cout << "[DetSVM] WARNING: Size of vector posterior_svm !=2, disable prob. SVM output;" << std::endl;
// TODO same as previous TODO
logisticA = logisticB = 0;
}
else {
logisticB = matdata[0];
logisticA = matdata[1];
}
// TODO delete *matdata and *dim? -> No I don't think so, no 'new'
mxDestroyArray(pmxarray);
}
matClose(pmatfile);
}
return make_pair(logisticA, logisticB);
#else
string errorMessage("ProbabilisticSvmClassifier: Cannot load a Matlab classifier, library compiled without support for Matlab. Please re-run CMake with WITH_MATLAB_CLASSIFIER enabled.");
logger.error(errorMessage);
throw std::runtime_error(errorMessage);
#endif
}
PetscErrorCode PetscViewerFileSetName_Matlab(PetscViewer viewer,const char name[])
{
PetscViewer_Matlab *vmatlab = (PetscViewer_Matlab*)viewer->data;
PetscFileMode type = vmatlab->btype;
PetscFunctionBegin;
if (type == (PetscFileMode) -1) SETERRQ(PETSC_COMM_SELF,PETSC_ERR_ORDER,"Must call PetscViewerFileSetMode() before PetscViewerFileSetName()");
if (vmatlab->ep) matClose(vmatlab->ep);
/* only first processor opens file */
if (!vmatlab->rank) {
if (type == FILE_MODE_READ) vmatlab->ep = matOpen(name,"r");
else if (type == FILE_MODE_WRITE || type == FILE_MODE_WRITE) vmatlab->ep = matOpen(name,"w");
else SETERRQ(PETSC_COMM_SELF,PETSC_ERR_ARG_OUTOFRANGE,"Unknown file type");
}
PetscFunctionReturn(0);
}
void utility::writeMatlabFile(mxArray *matlabMatrix, const char *varname, const char *filename)
{
MATFile *pmat;
cout << "Opening matlab file...";
pmat=matOpen(filename, "w");
if (pmat == NULL) {
cerr << "could not open matlab file for writing!" << endl;
exit(EXIT_FAILURE);
}
cout << "done" << endl;
cout << "Put variable into file...";
matPutVariable(pmat, varname, matlabMatrix);
cout << "Closing file...";
matClose(pmat);
cout << "done" << endl;
}
void CCE::export_mat_file()
{/*{{{*/
#ifdef HAS_MATLAB
cout << "begin post_treatement ... storing cce_data to file: " << _result_filename << endl;
MATFile *mFile = matOpen(_result_filename.c_str(), "w");
for(int i=0; i<_max_order; ++i)
{
char i_str [10];
sprintf(i_str, "%d", i);
string idx_str = i_str;
string label = "CCE" + idx_str;
string label1 = "CCE" + idx_str+"_tilder";
size_t nClst = _spin_clusters.getClusterNum(i);
mxArray *pArray = mxCreateDoubleMatrix(_nTime, nClst, mxREAL);
mxArray *pArray1 = mxCreateDoubleMatrix(_nTime, nClst, mxREAL);
size_t length= _nTime * nClst;
memcpy((void *)(mxGetPr(pArray)), (void *) _cce_evovle_result[i].memptr(), length*sizeof(double));
memcpy((void *)(mxGetPr(pArray1)), (void *) _cce_evovle_result_tilder[i].memptr(), length*sizeof(double));
matPutVariableAsGlobal(mFile, label.c_str(), pArray);
matPutVariableAsGlobal(mFile, label1.c_str(), pArray1);
mxDestroyArray(pArray);
mxDestroyArray(pArray1);
}
mxArray *pRes = mxCreateDoubleMatrix(_nTime, _max_order, mxREAL);
mxArray *pRes1 = mxCreateDoubleMatrix(_nTime, _max_order, mxREAL);
mxArray *pTime = mxCreateDoubleMatrix(_nTime, 1, mxREAL);
size_t length= _nTime*_max_order;
memcpy((void *)(mxGetPr(pRes)), (void *) _final_result_each_order.memptr(), length*sizeof(double));
memcpy((void *)(mxGetPr(pRes1)), (void *) _final_result.memptr(), length*sizeof(double));
memcpy((void *)(mxGetPr(pTime)), (void *) _time_list.memptr(), _nTime*sizeof(double));
matPutVariableAsGlobal(mFile, "final_result_each_order", pRes);
matPutVariableAsGlobal(mFile, "final_result", pRes1);
matPutVariableAsGlobal(mFile, "time_list", pTime);
mxDestroyArray(pRes);
mxDestroyArray(pRes1);
mxDestroyArray(pTime);
matClose(mFile);
#endif
}/*}}}*/
int write_mat_matrix(const Matrix* M, const char* file_name, const char* varname)
{
MATFile* matf = NULL;
mxArray* Mx = NULL;
unsigned int i, j;
double* data;
int res;
assert(file_name);
assert(M);
assert(varname);
/* create mxArray */
Mx = mxCreateDoubleMatrix(M->n_rows, M->n_cols, mxREAL);
if (!Mx) {
exit(ERROR_MEM); /* TODO */
return ERROR_MEM;
}
/* copy data in column order! */
data = mxGetPr(Mx);
for (j=0; j < M->n_cols; ++j) {
for (i=0; i < M->n_rows; ++i) {
*data = (double) matrix_get(M, i, j);
++data;
}
}
/* let's open the file */
matf = matOpen(file_name, "wz" );
if(!matf) {
return ERROR_IO;
}
/* put the variable in the .mat */
res = matPutVariable(matf, varname, Mx);
if (res) {
return ERROR_IO_MAT;
}
/* close the file and delete mxArray */
res = matClose(matf);
assert(!res);
mxDestroyArray(Mx);
return 0;
}
void Buffer::LoadBuffer() {
MATFile *matFileHandle;
matFileHandle = matOpen(bufferFileName, "r");
int numberOfDirectories;
if(matFileHandle==NULL) {
std::cout<<"ERROR IN FILE OPENING"<<std::endl;
}
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
bufferData.push_back(matrixEntry);
}
}
maximumBufferLevel = bufferData[0];
//std::cout<<"Max. buffer level "<<maximumBufferLevel<<std::endl;
minimumBufferLevel = bufferData[1];
//std::cout<<"Min. buffer level "<<minimumBufferLevel<<std::endl;
maximumStateOfBuffer = bufferData[2];
//std::cout<<"Max. state of buffer "<<maximumStateOfBuffer<<std::endl;
referenceStateOfBuffer = bufferData[3];
// A dummy reference is given to all buffers, Fuel Tanks and Batteries. For batteries,
// this value is updated eat each step. It remains constant for fuel tanks.
//std::cout<<"Min. allowed state of buffer "<<referenceStateOfBuffer<<std::endl;
openCircuitVoltage = bufferData[4];
//std::cout<<"openCircuitVoltage "<<openCircuitVoltage<<std::endl;
instantaneousBufferLevel=maximumStateOfBuffer*maximumBufferLevel;
//bufferLevelOverMission.push_back(instantaneousBufferLevel);
//std::cout<<"Inst. buffer level "<<instantaneousBufferLevel<<std::endl;
//std::cout<<"Buffer data successfully loaded"<<std::endl;
if (matClose(matFileHandle) != 0) {
std::cout<<"Error closing file"<<'\n';
}
}
static void save2matfile(mxArray *A,string file,string varname){
//mxArray*mxA=array2mxArray(scores);
//setDimensions(mxA,m,n);
//save2matfile(mxA,"C:\\tim\\temp\\trash\\1.mat","A");
//matOpen doesn't work under windows unless some library is included
#if defined(_WIN32)
assert(0);
#else
MATFile *pmat = matOpen(file.c_str(), "w");
if (pmat == NULL)
assert(0);//mexErrMsgTxt("error accessing mat file\n");
int status = matPutVariable(pmat, varname.c_str(), A);
if (status != 0)
assert(0);//mexErrMsgTxt("error writing mat file\n");
if (matClose(pmat) != 0)
mexPrintf("Error closing file %s\n",file.c_str());
else
mexPrintf("saved file %s\n",file.c_str());
#endif
}
void writeMxArrayToSigFile(mxArray* mxTrial, mxArray* mxMeta, const SignalFileInfo *pSigFileInfo)
{
int error;
// open the file
MATFile* pmat = matOpen(pSigFileInfo->fileName, "w");
if(pmat == NULL)
diep("Error opening MAT file");
// put variable in file
error = matPutVariable(pmat, "trial", mxTrial);
if(error)
diep("Error putting variable in MAT file");
error = matPutVariable(pmat, "meta", mxMeta);
if(error)
diep("Error putting variable in MAT file");
// close the file
matClose(pmat);
}
bool CPPMaster::SavePolicyToMat(std::vector< std::vector<double> > policy, int tau){
// Command = force | pi | time
// tau should also be saved, but it's included in the loop.
// save a file:
// File should also be in .mat format, if possible
// name: Policy-%d.dat, where %d is tau
// Should include BVP, Theta, and the tau associated.
std::vector<double> forces = policy.at(0);
std::vector<double> phi = policy.at(1);
std::vector<double> time = policy.at(2);
double* d_forces = forces.data();
double* d_phi = forces.data();
double* d_time = forces.data();
// The below code is a modification from an example given by MATLAB:
// http://bit.ly/1dvUEGF
MATFile *pmat;
mxArray *pa_forces, *pa_phi, *pa_time;
std::string name = "Policy-"+std::to_string(tau)+".mat";
const char *file = name.c_str();
pmat = matOpen(file, "w");
int status; // This is just for debugging purposes.
pa_forces = mxCreateDoubleMatrix(1, forces.size(), mxREAL);
pa_phi = mxCreateDoubleMatrix(1, phi.size(), mxREAL);
pa_time = mxCreateDoubleMatrix(1, time.size(), mxREAL);
memcpy((void *)(mxGetPr(pa_forces)), (void *)d_forces, sizeof(d_forces));
memcpy((void *)(mxGetPr(pa_phi)), (void *)d_phi, sizeof(d_phi));
memcpy((void *)(mxGetPr(pa_time)), (void *)d_time, sizeof(d_time));
status = matPutVariable(pmat, "forces", pa_forces);
status = matPutVariable(pmat, "phi", pa_phi);
status = matPutVariable(pmat, "time", pa_time);
mxDestroyArray(pa_forces);
mxDestroyArray(pa_phi);
mxDestroyArray(pa_time);
matClose(pmat);
return true;
}
int main(int argc, char** argv){
double delta = (double)pow(10,-5);
int max_iter = 100;
size_t size;
//load Series from .mat file
MATFile *pmat;
const char* file =argv[1];
const char* varname="Series";
mxArray* Series_mat;
pmat = matOpen(file, "r");
if(pmat == NULL){
printf("Error reopening file%s\n", file);
return(NULL);
}
Series_mat = matGetVariable(pmat, varname);
if(Series_mat == NULL){
printf("Error reading in file%s\n", file);
return(NULL);
}
matClose(pmat);
mwSize row, col; // mwSize is int
mwSize nRow = mxGetM(Series_mat);
mwSize nCol = mxGetN(Series_mat);
int T = nCol;
int N = nRow;
double *Series_Pr = mxGetPr(Series_mat);
Matrix Series;
Series.width = nCol;
Series.height = nRow;
size = Series.width*Series.height*sizeof(double);
Series.elements = (double*)malloc(size);
for(row = 0; row < nRow; row++) {
for(col = 0; col < nCol; col++) {
Series.elements[row*Series.width+col] = Series_Pr[nRow * col + row];// this needs testing
}
}
int i,j;
Matrix A;
//create matrix A
A.width = N;
A.height = N;
size = A.width*A.height*sizeof(double);
A.elements = (double*)malloc(size);
//time counter
double tstart,tstop,ttime;
tstart =(double)clock()/CLOCKS_PER_SEC;
int counter = 0;
while(counter<10){
//initialize A
for(i =0; i < A.height; i++){
for(j = 0; j < A.width; j++){
A.elements[i*A.width+j] = 0;
}
}
// Gradient_descent
Gradient_descent(A,Series,max_iter,delta,N,T);
counter++;
}
tstop = (double)clock()/CLOCKS_PER_SEC;
ttime=tstop-tstart;
printf("time:%fs\n",ttime);
free(A.elements);
free(Series.elements);
return 0;
}
int matFiles::readMatFile(const char *file, std::vector <std::vector<double> >& earSignals, double *fsHz) {
MATFile *pmat;
const char **dir;
const char *name;
int ndir;
int i;
mxArray *pa;
mxArray *var;
const size_t *dims;
size_t ndims;
double *data;
/*
* Open file to get directory
*/
pmat = matOpen(file, "r");
if (pmat == NULL) {
printf("Error opening file %s\n", file);
return(1);
}
/*
* get directory of MAT-file
*/
dir = (const char **)matGetDir(pmat, &ndir);
if (dir == NULL) {
printf("Error reading directory of file %s\n", file);
return(1);
}
mxFree(dir);
/* In order to use matGetNextXXX correctly, reopen file to read in headers. */
if (matClose(pmat) != 0) {
printf("Error closing file %s\n",file);
return(1);
}
pmat = matOpen(file, "r");
if (pmat == NULL) {
printf("Error reopening file %s\n", file);
return(1);
}
/* Get headers of all variables */
/* Examining the header for each variable */
for (i=0; i < ndir; i++) {
pa = matGetNextVariableInfo(pmat, &name);
var = matGetVariable(pmat, name);
data = mxGetPr(var);
if (pa == NULL) {
printf("Error reading in file %s\n", file);
return(1);
}
if ( strcmp(name,"earSignals") == 0 ) {
ndims = mxGetNumberOfDimensions(pa);
dims = mxGetDimensions(pa);
earSignals.resize(ndims);
for ( size_t ii = 0 ; ii < ndims ; ++ii )
earSignals[ii].resize(dims[0],0);
size_t ii, iii;
for ( ii = 0 ; ii < dims[0] ; ++ii )
earSignals[0][ii] = data[ii];
for ( ii = dims[0], iii = 0 ; ii < dims[0] * 2 ; ++ii, ++iii )
earSignals[1][iii] = data[ii];
} else if ( strcmp(name,"fsHz") == 0 ) {
ndims = mxGetNumberOfDimensions(pa);
dims = mxGetDimensions(pa);
assert( ndims == 2 );
assert( dims[0] == 1 );
*fsHz = data[0];
}
mxDestroyArray(pa);
}
if (matClose(pmat) != 0) {
printf("Error closing file %s\n",file);
return(1);
}
return(0);
}
int main(int argc, char *argv[])
{
MATFile *mat;
const char* name = NULL;
const mwSize* dims;
if (argc != 2) {
fprintf(stderr, "Usage: %s file.mat\n", argv[0]);
exit(1);
}
if (NULL == (mat = matOpen(argv[1], "r")))
exit(1);
mxArray* ar;
while (NULL != (ar = matGetNextVariable(mat, &name))) {
int ndim = (int)mxGetNumberOfDimensions(ar);
dims = mxGetDimensions(ar);
bool cmp = mxIsComplex(ar);
bool dbl = mxIsDouble(ar);
printf("%s: [ ", name);
if ((!cmp) || (!dbl)) {
printf("not complex double\n");
mxDestroyArray(ar);
continue;
}
long ldims[ndim];
for (int i = 0; i < ndim; i++)
ldims[i] = dims[i];
for (int i = 0; i < ndim; i++)
printf("%ld ", ldims[i]);
char outname[256];
snprintf(outname, 256, "%s_%s", strtok(argv[1], "."), name);
complex float* buf = create_cfl(outname, ndim, ldims);
double* re = mxGetPr(ar);
double* im = mxGetPi(ar);
size_t size = md_calc_size(ndim, ldims);
for (unsigned long i = 0; i < size; i++)
buf[i] = re[i] + 1.i * im[i];
printf("] -> %s\n", outname);
unmap_cfl(ndim, ldims, buf);
mxDestroyArray(ar);
}
matClose(mat);
}
MVT_3D_Object::MVT_3D_Object(ENUM_OBJECT_CATEGORY object_category, const char* filepath_3dobject_model)
{
m_object_category = object_category;
MATFile* mat = matOpen(filepath_3dobject_model,"r");
if( mat != NULL )
{
mxArray* pmxCad = matGetVariable(mat, "cad");
pmxCad = mxGetCell(pmxCad,0);
/*
* Names of parts
*/
mxArray* pmxNames = mxGetField(pmxCad, 0, "pnames");
m_num_of_partsNroots = mxGetNumberOfElements(pmxNames);
for( unsigned int i=0; i<m_num_of_partsNroots; i++)
{
mxArray* pmxName = mxGetCell(pmxNames, i);
m_names_partsNroots.push_back(MxArray(pmxName).toString());
}
/*
* Parts2d_Front
*/
mxArray* pmxPars2d_front = mxGetField(pmxCad, 0, "parts2d_front");
for( unsigned int i=0; i<m_num_of_partsNroots; i++)
{
MVT_2D_Part_Front front;
front.width = MxArray(mxGetField(pmxPars2d_front, i, "width" )).toDouble();
front.height = MxArray(mxGetField(pmxPars2d_front, i, "height" )).toDouble();
front.distance = MxArray(mxGetField(pmxPars2d_front, i, "distance")).toDouble();
cv::Mat mat_tmp = MxArray(mxGetField(pmxPars2d_front, i, "vertices")).toMat();
unsigned int n_row = mat_tmp.rows;
for( unsigned int r=0; r<n_row ; r++)
{
front.vertices.push_back(cv::Point2d(mat_tmp.at<double>(r,0), mat_tmp.at<double>(r,1)));
}
front.center = MxArray(mxGetField(pmxPars2d_front, i, "center")).toPoint_<double>();
front.viewport = MxArray(mxGetField(pmxPars2d_front, i, "viewport")).toDouble();
front.name = MxArray(mxGetField(pmxPars2d_front, i, "pname")).toString();
m_2dparts_front.push_back(front);
}
/*
* Part
*/
mxArray* pmxParts = mxGetField(pmxCad, 0, "parts");
m_num_of_parts = mxGetNumberOfElements(pmxParts);
for( unsigned int i=0 ; i<m_num_of_parts; i++ )
{
MVT_3D_Part part;
/* Vertices */
mxArray* mxVertices = mxGetField(pmxParts, i, "vertices");
cv::Mat matVertices = MxArray(mxVertices).toMat();
unsigned int n_rows = matVertices.rows;
for( unsigned int r=0; r<n_rows; r++)
{
cv::Mat matTmp = matVertices.row(r);
part.vertices.push_back
(
cv::Point3d( matTmp.at<double>(0),
matTmp.at<double>(1),
matTmp.at<double>(2) )
);
}
/* plane */
MxArray MxPlane(mxGetField(pmxParts, i, "plane"));
for( int j=0 ; j<4; j++ )
{
part.plane[j] = MxPlane.at<double>(j);
}
/* center */
MxArray MxCenter(mxGetField(pmxParts, i, "center"));
part.center.x = MxCenter.at<double>(0);
part.center.y = MxCenter.at<double>(1);
part.center.z = MxCenter.at<double>(2);
/* xaxis */
MxArray MxXAxis(mxGetField(pmxParts, i, "xaxis"));
part.xaxis.x = MxXAxis.at<double>(0);
part.xaxis.y = MxXAxis.at<double>(1);
part.xaxis.z = MxXAxis.at<double>(2);
/* yaxis */
MxArray MxYAxis(mxGetField(pmxParts, i, "yaxis"));
part.yaxis.x = MxYAxis.at<double>(0);
part.yaxis.y = MxYAxis.at<double>(1);
part.yaxis.z = MxYAxis.at<double>(2);
m_3dparts.push_back(part);
/*
* Occlusion Information
*/
mxArray* pmxAzimuth = mxGetField(pmxCad, 0, "azimuth" );
m_disc_azimuth = MxArray(pmxAzimuth).toVector<MVT_AZIMUTH>();
mxArray* pmxElevation = mxGetField(pmxCad, 0, "elevation" );
m_disc_elevation = MxArray(pmxElevation).toVector<MVT_ELEVATION>();
mxArray* pmxDistance = mxGetField(pmxCad, 0, "distance" );
m_disc_distance = MxArray(pmxDistance).toVector<MVT_DISTANCE>();
mxArray* pmxParts2d = mxGetField(pmxCad, 0, "parts2d");
m_num_of_disc_azimuth = m_disc_azimuth.size();
m_num_of_disc_elevation = m_disc_elevation.size();
m_num_of_disc_distance = m_disc_distance.size();
m_is_occluded = new bool***[m_num_of_disc_azimuth];
for( unsigned int a=0; a<m_num_of_disc_azimuth; a++ )
{
m_is_occluded[a] = new bool**[m_num_of_disc_elevation];
for( unsigned int e=0; e<m_num_of_disc_elevation; e++ )
{
m_is_occluded[a][e] = new bool*[m_num_of_disc_distance];
for( unsigned int d=0; d<m_num_of_disc_distance; d++ )
{
m_is_occluded[a][e][d] = new bool[m_num_of_partsNroots];
unsigned int idx = m_num_of_disc_distance*m_num_of_disc_elevation*a +
m_num_of_disc_distance*e +
d;
for( unsigned int p=0; p<m_num_of_partsNroots; p++)
{
mxArray* pmxPart = mxGetField(pmxParts2d, idx, m_names_partsNroots[p].c_str());
m_is_occluded[a][e][d][p] = mxIsEmpty(pmxPart);
}
}
}
}
}
mxDestroyArray(pmxCad);
matClose(mat);
}
}
int main (int argc, char**argv) {
if (argc < 3) {
printf ("Usage:\n\nmih <infile> <outfile> [options]\n\n");
printf ("Options:\n");
printf (" -nMs <n1 n2 n3 ...> Set an array of multiples of 'one million items' to experiment with\n");
printf (" -nM <number> Set the index from the nMs array to be used for this run (1 is the first)\n");
printf (" -Q <number> Set the number of query points to use from <infile>, default all\n");
printf (" -B <number> Set the number of bits per code, default autodetect\n");
printf (" -m <number> Set the number of chunks to use, default 1\n");
printf ("\n");
return 0;
}
char *infile = argv[1];
char *outfile = argv[2];
UINT32 N = 0;
int B = 0;
int m = 1;
UINT32 K = 100;
int nM = 0;
int NQ = 0;
double *nMs = NULL;
int nnMs = 0;
for (int argnum = 3; argnum < argc; argnum++) {
if (argv[argnum][0] == '-') {
switch (argv[argnum][1]) {
case 'B':
B = atoi(argv[++argnum]);
break;
case 'K':
K = atoi(argv[++argnum]);
break;
case 'n':
if (!strcmp(argv[argnum], "-nMs")) {
nMs = new double[100];
while (++argnum < argc)
if (argv[argnum][0] != '-') {
nMs[nnMs++] = atof(argv[argnum]);
} else {
argnum--;
break;
}
} else if (!strcmp(argv[argnum], "-nM")) {
nM = atoi(argv[++argnum]);
}
break;
case 'Q':
NQ = atoi(argv[++argnum]);
break;
case 'm':
m = atoi(argv[++argnum]);
break;
default:
printf ("Unrecognized Option or Missing Parameter when parsing: %s\n", argv[argnum]);
return EXIT_FAILURE;
}
} else {
printf ("Invalid Argument: %s\n", argv[argnum]);
return EXIT_FAILURE;
}
}
MATFile *ifp = NULL;
mxArray *mxnMs = NULL;
mxArray *mxret = NULL;
/* Opening output file to read "ret" and "nMs" if available */
ifp = matOpen(outfile, "r");
if (ifp) {
mxnMs = matGetVariable(ifp, "nMs");
mxret = matGetVariable(ifp, "ret");
double *nMs2 = (double*)mxGetPr(mxnMs);
int nnMs2 = mxGetN(mxnMs);
if (nMs != NULL) {
if (nnMs != nnMs2) {
printf("#nMs is different from the #nMs read from the output file %d vs. %d.\n", nnMs, nnMs2);
return EXIT_FAILURE;
}
for (int i=0; i<nnMs; i++)
if (int(nMs[i]*1.0e6) != int(nMs2[i] * 1.0e6)) {
printf("nMs are different from the nMs read from the output file.\n");
return EXIT_FAILURE;
}
delete[] nMs;
}
nnMs = nnMs2;
nMs = nMs2;
matClose (ifp);
} else {
mxnMs = mxCreateNumericMatrix(1, nnMs, mxDOUBLE_CLASS, mxREAL);
double *nMs2 = (double*)mxGetPr(mxnMs);
for (int i=0; i<nnMs; i++)
nMs2[i] = nMs[i];
delete[] nMs;
nMs = nMs2;
}
if (mxret == NULL) {
const char* ab[] = {"res", "nres", "stat", "wt", "cput", "vm", "rss", "m"};
mxret = mxCreateStructMatrix(1000, nnMs, 8, ab);
}
/* Done with initializing mxnMs and mxret and sanity checks */
/* Loading the codes and queries from the input file */
ifp = matOpen (infile, "r");
if (!ifp) {
printf ("Failed to open input file. Aborting.\n");
return EXIT_FAILURE;
}
printf ("Loading codes... ");
fflush (stdout);
mxArray *mxcodes = matGetVariable (ifp, "B");
printf ("done.\n");
printf ("Loading queries... ");
fflush (stdout);
mxArray *mxqueries = matGetVariable (ifp, "Q");
printf ("done.\n");
matClose (ifp);
/* Done with the inputs */
int dim1codes = mxGetM(mxcodes);
int dim1queries = mxGetM(mxqueries);
if (!B)
B = mxGetM(mxcodes)*8;
if (B % 8 != 0) {
printf ("Non-multiple of 8 code lengths are not currently supported.\n");
return EXIT_FAILURE;
}
N = 1.0e6 * nMs[nM-1];
if (N)
N = std::min ( (UINT32)N, (UINT32)mxGetN(mxcodes) );
if (NQ > 0)
NQ = std::min( NQ, (int)mxGetN(mxqueries) );
else
NQ = mxGetN (mxqueries);
printf("nM = %d |", nM);
printf(" N = %.0e |", (double)N);
printf(" NQ = %d |", NQ);
printf(" B = %d |", B);
printf(" m = %d", m);
printf("\n");
/* Run multi-index hashing for 1,10,100,1000 NN and store the required stats */
int mold = -1;
mihasher* MIH = NULL;
clock_t start0, end0;
time_t start1, end1;
qstat *stats = (qstat*) new qstat[NQ];
for (K = 1; K<=1000; K *= 10) {
mxArray *mxresults = mxCreateNumericMatrix (K, NQ, mxUINT32_CLASS, mxREAL);
mxArray *mxnumres = mxCreateNumericMatrix (B+1, NQ, mxUINT32_CLASS, mxREAL);
mxArray *mxctime = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxwtime = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxvm = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxrss = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxstats = mxCreateNumericMatrix (6, NQ, mxDOUBLE_CLASS, mxREAL);
UINT32 *numres = (UINT32*) mxGetPr (mxnumres);
UINT32 *results = (UINT32*) mxGetPr (mxresults);
double *ctime = (double*) mxGetPr (mxctime);
double *wtime = (double*) mxGetPr (mxwtime);
double *vm = (double*) mxGetPr (mxvm);
double *rss = (double*) mxGetPr (mxrss);
double *stats_d = (double*) mxGetPr (mxstats);
/* if m changes from K to K */
if (mold != m) {
if (MIH != NULL) {
printf ("Clearing up... ");
fflush (stdout);
delete MIH;
printf ("done. \r");
fflush (stdout);
}
MIH = new mihasher(B, m);
printf ("Populating %d hashtables with %.0e entries...\n", m, (double)N);
fflush (stdout);
start1 = time(NULL);
start0 = clock();
MIH->populate ( (UINT8*) mxGetPr(mxcodes), N, dim1codes);
end0 = clock();
end1 = time(NULL);
double ct = (double)(end0-start0) / (CLOCKS_PER_SEC);
double wt = (double)(end1-start1);
printf ("done. | cpu %.0fm%.0fs | wall %.0fm%.0fs\n", ct/60, ct-60*int(ct/60), wt/60, wt-60*int(wt/60));
// printf ("done. \r");
// fflush (stdout);
}
printf(" m = %2d |", m);
printf(" K = %4d |", K);
printf(" ");
MIH->setK(K);
printf("query... ");
fflush (stdout);
start1 = time(NULL);
start0 = clock();
MIH->batchquery (results, numres, stats, (UINT8*) mxGetPr(mxqueries), NQ, dim1queries);
end0 = clock();
end1 = time(NULL);
*ctime = (double)(end0-start0) / (CLOCKS_PER_SEC) / NQ;
*wtime = (double)(end1-start1) / NQ;
process_mem_usage(vm, rss);
*vm /= double(1024*1024);
*rss /= double(1024*1024);
printf ("done | cpu %.3fs | wall %.3fs | VM %.1fgb | RSS %.1fgb \n", *ctime, *wtime, *vm, *rss);
int ind = 1000*(nM-1) + K-1;
double *pstats_d = stats_d;
for (int i=0; i<NQ; i++) {
pstats_d[0] = stats[i].numres;
pstats_d[1] = stats[i].numcand;
pstats_d[2] = stats[i].numdups;
pstats_d[3] = stats[i].numlookups;
pstats_d[4] = stats[i].maxrho;
pstats_d[5] = (double) stats[i].ticks / CLOCKS_PER_SEC;
pstats_d += 6;
}
mxSetFieldByNumber(mxret, ind, 0, mxresults);
mxSetFieldByNumber(mxret, ind, 1, mxnumres);
mxSetFieldByNumber(mxret, ind, 2, mxstats);
mxSetFieldByNumber(mxret, ind, 3, mxwtime);
mxSetFieldByNumber(mxret, ind, 4, mxctime);
mxSetFieldByNumber(mxret, ind, 5, mxvm);
mxSetFieldByNumber(mxret, ind, 6, mxrss);
mxSetFieldByNumber(mxret, ind, 7, mxCreateDoubleScalar(m));
mold = m;
}
printf ("Clearing up... ");
fflush (stdout);
delete MIH;
printf ("done. \r");
fflush (stdout);
/* Done with mulit-index hashing and storing the stats */
/* Opening the output file for writing the results */
MATFile *ofp = matOpen (outfile, "w");
if (!ofp) {
printf ("Failed to create/open output file. Aborting.\n");
return EXIT_FAILURE;
}
printf("Writing results to file %s... ", outfile);
fflush(stdout);
matPutVariable(ofp, "nMs", mxnMs);
matPutVariable(ofp, "ret", mxret);
printf("done.\n");
matClose(ofp);
/* Done with the output file */
delete[] stats;
mxDestroyArray (mxnMs);
mxDestroyArray (mxret);
mxDestroyArray (mxcodes);
mxDestroyArray (mxqueries);
/* skip deleting nMs if it is initialized by new double[] */
return 0;
}
void
mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
MATFile *ph;
char *matfile;
char *varnamelist[100];
int counter=0;
(void) nlhs; /* unused parameters */
(void) plhs;
if(nrhs==0) {
/*
* no arguments; so load by default from 'matlab.mat'
*/
matfile=(char *)mxCalloc(11,sizeof(char));
matfile=strcat(matfile,"matlab.mat");
}
else {
/*
* parse the argument list for the
* filename and any variable names
*
* note: the first argument must be a filename.
* therefore you can't request specific
* variables from 'matlab.mat' without
* passing the filename in
*/
/*
* we're adding 5 to the size of the string.
* 1 for '\0' and 4 in case the '.mat' was
* forgotten
*/
mwSize buflen=mxGetN(prhs[0])+5;
int index;
/* filename */
matfile=(char *)mxCalloc(buflen,sizeof(char));
mxGetString(prhs[0],matfile,buflen);
/*
* make sure '.mat' suffix exists
*/
{
const char *lastdot = strrchr( matfile, '.' );
const char *lastslash = strrchr( matfile, '/' );
const char *lastbslash = strrchr( lastslash ? lastslash : matfile, '\\');
lastslash = lastbslash ? lastbslash : lastslash;
if(!lastdot || (!lastslash || lastdot < lastslash))
matfile=strcat(matfile,".mat");
}
/* variable name list */
for(index=1;index<nrhs;index++) {
mwSize varlength=mxGetN(prhs[index])+1;
varnamelist[counter]=(char *)mxCalloc(varlength,sizeof(char));
mxGetString(prhs[index],varnamelist[counter],varlength);
counter++;
}
}
/* open the MAT-File to read from */
ph=matOpen(matfile,"r");
if(ph<(MATFile *)3) {
char errmsg[]="Failed to open '";
strcat(errmsg,matfile);
strcat(errmsg,"'");
mxFree(matfile);
mexErrMsgTxt(errmsg);
}
if(counter>0) {
/* there where variables in the argument list */
mxArray *var;
int index;
for(index=0;index<counter;index++) {
var=matGetVariable(ph,varnamelist[index]);
mexPutVariable("caller", varnamelist[index],var);
mxFree(varnamelist[index]);
}
}
else {
/*
* the variable argument list
* was empty so get everything
*/
mxArray *var;
const char *var_name;
while((var=matGetNextVariable(ph, &var_name))!=NULL) {
mexPutVariable("caller", var_name, var);
}
}
matClose(ph);
mxFree(matfile);
}
/** Loads the data from a .mat file. */
int ArmModel::loadData(bool onlyReset) {
char filename[20];
sprintf(filename, "T%d.mat",inputFileNr);
// Open MAT-File
MATFile *mf = matOpen(filename, "r");
if (mf == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find input file %s\n",filename);
return -1;
}
if (!onlyReset) {
/* *********************************************************
READ ALL DATA FROM FILE
********************************************************* */
// Read Xdest and store it
mxArray *mxXdest = matGetVariable(mf, "Xdest");
if (mxXdest == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find Xdest in input file %s\n",filename);
return -1;
}
TIMESTEPS = mxGetN(mxXdest);
csimPrintf("Read data for %d timesteps!\n", TIMESTEPS);
Xdest = (double *) realloc(Xdest, TIMESTEPS*50*sizeof(double));
memcpy(Xdest, mxGetPr(mxXdest), TIMESTEPS*50*sizeof(double));
// Read Ydest and store it
mxArray *mxYdest = matGetVariable(mf, "Ydest");
if (mxYdest == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find Ydest in input file %s\n",filename);
return -1;
}
Ydest = (double *) realloc(Ydest, TIMESTEPS*50*sizeof(double));
memcpy(Ydest, mxGetPr(mxYdest), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxYdest);
// Read Theta1 and store it
mxArray *mxTheta1 = matGetVariable(mf, "theta1");
if (mxTheta1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find theta1 in input file %s\n",filename);
return -1;
}
theta1 = (double *) realloc(theta1, TIMESTEPS*50*sizeof(double));
memcpy(theta1, mxGetPr(mxTheta1), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxTheta1);
// Read Theta2 and store it
mxArray *mxTheta2 = matGetVariable(mf, "theta2");
if (mxTheta2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find theta2 in input file %s\n",filename);
return -1;
}
theta2 = (double *) realloc(theta2, TIMESTEPS*50*sizeof(double));
memcpy(theta2, mxGetPr(mxTheta2), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxTheta2);
// Read nu1 and store it
mxArray *mxNu1 = matGetVariable(mf, "nu1");
if (mxNu1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find nu1 in input file %s\n",filename);
return -1;
}
nu1 = (double *) realloc(nu1, TIMESTEPS*50*sizeof(double));
memcpy(nu1, mxGetPr(mxNu1), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxNu1);
// Read nu2 and store it
mxArray *mxNu2 = matGetVariable(mf, "nu2");
if (mxNu2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find nu2 in input file %s\n",filename);
return -1;
}
nu2 = (double *) realloc(nu2, TIMESTEPS*50*sizeof(double));
memcpy(nu2, mxGetPr(mxNu2), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxNu2);
// Read c_theta1 and store it
mxArray *mxCTheta1 = matGetVariable(mf, "c_theta1");
if (mxCTheta1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_theta1 in input file %s\n",filename);
return -1;
}
c_theta1 = (double *) realloc(c_theta1, TIMESTEPS*sizeof(double));
memcpy(c_theta1, mxGetPr(mxCTheta1), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCTheta1);
// Read c_theta2 and store it
mxArray *mxCTheta2 = matGetVariable(mf, "c_theta2");
if (mxCTheta2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_theta2 in input file %s\n",filename);
return -1;
}
c_theta2 = (double *) realloc(c_theta2, TIMESTEPS*sizeof(double));
memcpy(c_theta2, mxGetPr(mxCTheta2), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCTheta2);
// Read c_u1 and store it
mxArray *mxCU1 = matGetVariable(mf, "c_u1");
if (mxCU1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_u1 in input file %s\n",filename);
return -1;
}
c_u1 = (double *) realloc(c_u1, TIMESTEPS*sizeof(double));
memcpy(c_u1, mxGetPr(mxCU1), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCU1);
// Read c_u2 and store it
mxArray *mxCU2 = matGetVariable(mf, "c_u2");
if (mxCU2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_u2 in input file %s\n",filename);
return -1;
}
c_u2 = (double *) realloc(c_u2, TIMESTEPS*sizeof(double));
memcpy(c_u2, mxGetPr(mxCU2), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCU2);
// Get the values which will be added as baselines.
int pos = 0;
for (int i=0; i<TIMESTEPS; i++) {
for (int j=0; j<50; j++) {
if (pos == 0) {
mintheta1 = theta1[0];
mintheta2 = theta2[0];
minU1 = nu1[0];
minU2 = nu2[0];
// HERE THE GET_ACT_VAL AND MAXU1, MAXU2 ARE MISSING, DO WE NEED IT?
}
else {
if (theta1[pos] < mintheta1)
mintheta1 = theta1[pos];
if (theta2[pos] < mintheta2)
mintheta2 = theta2[pos];
if (nu1[pos] < minU1)
minU1 = nu1[pos];
if (nu2[pos] < minU2)
minU2 = nu2[pos];
}
pos++;
}
}
mintheta1 = fabs(mintheta1);
mintheta2 = fabs(mintheta2);
minU1 = fabs(minU1);
minU2 = fabs(minU2);
}
// Do the rest for a simple reset
/* *********************************************************
INITIALIZE THE OUTPUTS OF THE MODEL
********************************************************* */
for (int i=0; i<50; i++) {
out_buffer[i][0] = Xdest[i];
out_buffer[i+50][0] = Ydest[i];
out_buffer[i+100][0] = theta1[i];
if (out_buffer[i+100][0] != 0)
out_buffer[i+100][0] += mintheta1;
out_buffer[i+150][0] = theta2[i];
if (out_buffer[i+150][0] != 0)
out_buffer[i+150][0] += mintheta2;
out_buffer[i+200][0] = nu1[i];
if (out_buffer[i+200][0] != 0)
out_buffer[i+200][0] += minU1;
out_buffer[i+250][0] = nu2[i];
if (out_buffer[i+250][0] != 0)
out_buffer[i+250][0] += minU2;
}
buffer_position = 0;
/* *********************************************************
Calculate initial values for u, t and w
********************************************************* */
// Read u1(1) and store it
mxArray *mxU1 = matGetVariable(mf, "u1");
if (mxU1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find u1 in input file %s\n",filename);
return -1;
}
u1 = *(mxGetPr(mxU1));
mxDestroyArray(mxU1);
// Read u2(1) and store it
mxArray *mxU2 = matGetVariable(mf, "u2");
if (mxU2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find u2 in input file %s\n",filename);
return -1;
}
u2 = *(mxGetPr(mxU2));
mxDestroyArray(mxU2);
// Read t1(1) and store it
mxArray *mxT1 = matGetVariable(mf, "t1");
if (mxT1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find t1 in input file %s\n",filename);
return -1;
}
t1 = *(mxGetPr(mxT1));
mxDestroyArray(mxT1);
// Read t2(1) and store it
mxArray *mxT2 = matGetVariable(mf, "t2");
if (mxT2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find t2 in input file %s\n",filename);
return -1;
}
t2 = *(mxGetPr(mxT2));
mxDestroyArray(mxT2);
// Read dt1(1) and store it
mxArray *mxDT1 = matGetVariable(mf, "dt1");
if (mxDT1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find dt1 in input file %s\n",filename);
return -1;
}
w1 = *(mxGetPr(mxDT1));
mxDestroyArray(mxDT1);
// Read dt2(1) and store it
mxArray *mxDT2 = matGetVariable(mf, "dt2");
if (mxDT2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find dt2 in input file %s\n",filename);
return -1;
}
w2 = *(mxGetPr(mxDT2));
mxDestroyArray(mxDT2);
// Close the input file
if (matClose(mf) == EOF) {
TheCsimError.add("ArmModel::loadData Error while closing the input file!\n");
return -1;
}
return 0;
}
