- Added C runtime for reading the .mat format

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@7963 f25d12d1-65f4-0310-ae8a-bbce733d8d8e

c_runtime/Makefile.common

@@ -16,6 +16,7 @@ biglag.o  ddassl.o  dogleg.o  fdjac1.o  hybrj.o   newuoa.o   qrfac.o   trsapp.o
 daux.o    dlamch.o  dpmpar.o  hybrd.o   lsame.o   newuob.o   r1mpyq.o  update.o division.o\
 java_interface.o meta_modelica.o meta_modelica_builtin.o meta_modelica_real.o \
 meta_modelica_string_lit.o rtclock.o ModelicaUtilities.o simulation_varinfo.o \
+read_matlab4.o \
 $(EXTRA_OBJS)
 
 SIMOBJS = $(FOBJS) simulation_runtime.o simulation_init.o simulation_input.o simulation_events.o linearize.o \
@@ -47,6 +48,7 @@ meta_modelica.h \
 modelica.h \
 modelica_string.h \
 ModelicaUtilities.h \
+read_matlab4.h \
 read_write.h \
 real_array.h \
 ringbuffer.h \

c_runtime/read_matlab4.c

@@ -0,0 +1,308 @@
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include "read_matlab4.h"
+
+const int size_omc_mat_Aclass = 45;
+const char *omc_mat_Aclass = "A1 bt. ir1 na  Tj  re  ac  nt  so   r   y   ";
+
+int omc_matlab4_comp_var(const void *a, const void *b)
+{
+  char *as = ((ModelicaMatVariable_t*)a)->name;
+  char *bs = ((ModelicaMatVariable_t*)b)->name;
+  return strcmp(as,bs);
+}
+
+int mat_element_length(int type)
+{
+  int m = (type/1000);
+  int o = (type%1000)/100;
+  int p = (type%100)/10;
+  int t = (type%10);
+  if (m) return -1; // We require IEEE Little Endian for now
+  if (o) return -1; // Reserved number; forced 0
+  if (t == 1 && p != 5) return -1; // Text matrix? Force element length=1
+  if (t == 2) return -1; // Sparse matrix fails
+  switch (p) {
+    case 0: return 8;
+    case 1: return 4;
+    case 2: return 4;
+    case 3: return 2;
+    case 4: return 2;
+    case 5: return 1;
+    default: return -1;
+  }
+}
+
+// Do not double-free this :)
+void omc_free_matlab4_reader(ModelicaMatReader *reader)
+{
+  int i;
+  fclose(reader->file);
+  free(reader->fileName);
+  for (i=0; i<reader->nall; i++)
+    free(reader->allInfo[i].name);
+  free(reader->allInfo);
+  free(reader->params);
+  for (i=0; i<reader->nvar; i++)
+    if (reader->vars[i]) free(reader->vars[i]);
+  free(reader->vars);
+}
+
+// Returns 0 on success; the error message on error
+const char* omc_new_matlab4_reader(const char *filename, ModelicaMatReader *reader)
+{
+  typedef const char *_string;
+  const int nMatrix=6;
+  _string matrixNames[6]={"Aclass","name","description","dataInfo","data_1","data_2"};
+  const int matrixTypes[6]={51,51,51,0,0,0};
+  int i;
+  reader->file = fopen(filename, "rb");
+  if (!reader->file) return strerror(errno);
+  reader->fileName = strdup(filename);
+  for (i=0; i<nMatrix;i++) {
+    MHeader_t hdr;
+    int nr = fread(&hdr,sizeof(MHeader_t),1,reader->file);
+    int matrix_length,element_length;
+    if (nr != 1) return "Corrupt header";
+    // fprintf(stderr, "Found matrix type=%04d mrows=%d ncols=%d imagf=%d namelen=%d\n", hdr.type, hdr.mrows, hdr.ncols, hdr.imagf, hdr.namelen);
+    if (hdr.type != matrixTypes[i]) return "Matrix type mismatch";
+    if (hdr.imagf > 1) return "Matrix uses imaginary numbers";
+    if ((element_length = mat_element_length(hdr.type)) == -1) return "Could not determine size of matrix elements";
+    char name[hdr.namelen];
+    nr = fread(name,hdr.namelen,1,reader->file);
+    if (nr != 1) return "Corrupt header";
+    if (name[hdr.namelen-1]) return "Corrupt header";
+    // fprintf(stderr, "  Name of matrix: %s\n", name);
+    matrix_length = hdr.mrows*hdr.ncols*(1+hdr.imagf)*element_length;
+    if (0 != strcmp(name,matrixNames[i])) return "Matrix name mismatch";
+    switch (i) {
+    case 0: {
+      char tmp[sizeof(omc_mat_Aclass)];
+      if (fread(tmp,sizeof(omc_mat_Aclass)-1,1,reader->file) != 1) return "Corrupt header: Aclass matrix";
+      tmp[sizeof(omc_mat_Aclass)-1] = '\0';
+      if (0 != strncmp(tmp,omc_mat_Aclass,sizeof(omc_mat_Aclass))) return "Aclass matrix does not match the magic number";
+      break;
+    }
+    case 1: { // "names"
+      int i;
+      reader->nall = hdr.ncols;
+      reader->allInfo = (ModelicaMatVariable_t*) malloc(sizeof(ModelicaMatVariable_t)*reader->nall);
+      for (i=0; i<hdr.ncols; i++) {
+        reader->allInfo[i].name = malloc(hdr.mrows+1);
+        if (fread(reader->allInfo[i].name,hdr.mrows,1,reader->file) != 1) return "Corrupt header: names matrix";
+        reader->allInfo[i].name[hdr.mrows] = '\0';
+        reader->allInfo[i].isParam = -1;
+        reader->allInfo[i].index = -1;
+        // fprintf(stderr, "    Adding variable %s\n", reader->allInfo[i].name);
+      }
+      break;
+    }
+    case 2: {
+      if (-1==fseek(reader->file,matrix_length,SEEK_CUR)) return "Corrupt header: description matrix";
+      break;
+    }
+    case 3: { // "dataInfo"
+      int i;
+      double tmp[hdr.ncols][hdr.mrows];
+      if (1 != fread(tmp,sizeof(double)*hdr.ncols*hdr.mrows,1,reader->file)) return "Corrupt header: dataInfo matrix";
+      for (i=0; i<hdr.ncols; i++) {
+        reader->allInfo[i].isParam = tmp[i][0] == 1.0;
+        reader->allInfo[i].index = (int) tmp[i][1] - 1;
+        // fprintf(stderr, "    Variable %s isParam=%d index=%d\n", reader->allInfo[i].name, reader->allInfo[i].isParam, reader->allInfo[i].index);
+      }
+      // Sort the variables so we can do faster lookup
+      qsort(reader->allInfo,reader->nall,sizeof(ModelicaMatVariable_t),omc_matlab4_comp_var);
+      break;
+    }
+    case 4: { // "data_1"
+      int i;
+      if (hdr.ncols == 0) return "data_1 matrix does not contain at least 1 variable";
+      if (hdr.mrows != 2) return "data_1 matrix does not have 2 rows";
+      reader->nparam = hdr.ncols;
+      reader->params = (double*) malloc(hdr.mrows*hdr.ncols*sizeof(double));
+      if (1 != fread(reader->params,matrix_length,1,reader->file)) return "Corrupt header: data_1 matrix";
+      // fprintf(stderr, "    startTime = %.6g\n", reader->params[0]);
+      // fprintf(stderr, "    stopTime = %.6g\n", reader->params[1]);
+      for (i=1; i<hdr.ncols; i++) {
+        if (reader->params[i] != reader->params[i+reader->nparam]) return "data_1 matrix contained parameter that changed between start and stop-time";
+        // fprintf(stderr, "    Parameter[%d] = %.6g\n", i, reader->params[i]);
+      }
+      break;
+    }
+    case 5: { // "data_2"
+      reader->nrows = hdr.ncols;
+      reader->nvar = hdr.mrows;
+      if (reader->nrows < 2) return "Too few rows in data_2 matrix";
+      reader->var_offset = ftell(reader->file);
+      reader->vars = calloc(reader->nvar,sizeof(double*));
+      if (-1==fseek(reader->file,matrix_length,SEEK_CUR)) return "Corrupt header: data_2 matrix";
+      break;
+    }
+    default:
+      return "Implementation error: Unknown case";
+    }
+  };
+  return 0;
+}
+
+ModelicaMatVariable_t *omc_matlab4_find_var(ModelicaMatReader *reader, char *varName)
+{
+  ModelicaMatVariable_t key;
+  key.name = varName;
+  return bsearch(&key,reader->allInfo,reader->nall,sizeof(ModelicaMatVariable_t),omc_matlab4_comp_var);
+}
+
+// Writes the number of values in the returned array if nvals is non-NULL
+double* omc_matlab4_read_vals(int *nvals, ModelicaMatReader *reader, int varIndex)
+{
+  if (!reader->vars[varIndex]) {
+    int i;
+    double *tmp = malloc(reader->nrows*sizeof(double));
+    for (i=0; i<reader->nrows; i++) {
+      fseek(reader->file,reader->var_offset + sizeof(double)*(i*reader->nvar + varIndex), SEEK_SET);
+      if (1 != fread(&tmp[i], sizeof(double), 1, reader->file)) {
+        free(tmp);
+        return NULL;
+      }
+      // fprintf(stderr, "tmp[%d]=%g\n", i, tmp[i]);
+    }
+    reader->vars[varIndex] = tmp;
+  }
+  if (nvals) *nvals = reader->nrows;
+  return reader->vars[varIndex];
+}
+
+double omc_matlab4_read_single_val(double *res, ModelicaMatReader *reader, int varIndex, int timeIndex)
+{
+  if (reader->vars[varIndex]) {
+    *res = reader->vars[varIndex][timeIndex];
+    return 0;
+  }
+  fseek(reader->file,reader->var_offset + sizeof(double)*(timeIndex*reader->nvar + varIndex), SEEK_SET);
+  if (1 != fread(res, sizeof(double), 1, reader->file))
+    return 1;
+  return 0;
+}
+
+void find_closest_points(double key, double *vec, int nelem, int *index1, double *weight1, int *index2, double *weight2)
+{
+  int min = 0;
+  int max = nelem-1;
+  int mid;
+  // fprintf(stderr, "search closest: %g in %d elem\n", key, nelem);
+  do {
+    mid = min + (max-min)/2;
+    if (key == vec[mid]) {
+      *index1 = mid;
+      *weight1 = 1.0;
+      *index2 = -1;
+      *weight2 = 0.0;
+      return;
+    } else if (key > vec[mid]) {
+      min = mid + 1;
+    } else {
+      max = mid - 1;
+    }
+  } while (max > min);
+  if (key > vec[max])
+    max++;
+  else
+    min--;
+  *index1 = max;
+  *index2 = min;
+  // fprintf(stderr, "closest: %g = (%d,%g),(%d,%g)\n", key, min, vec[min], max, vec[max]);
+  *weight1 = (key - vec[min]) / (vec[max]-vec[min]);
+  *weight2 = 1.0 - *weight1;
+}
+
+// Returns 0 on success
+int omc_matlab4_val(double *res, ModelicaMatReader *reader, ModelicaMatVariable_t *var, double time)
+{
+  if (var->isParam) {
+    *res = reader->params[var->index];
+  } else {
+    if (time > reader->params[reader->nparam]) return 1; // time > stopTime
+    if (time < reader->params[0]) return 1; // time < startTime
+    if (!omc_matlab4_read_vals(NULL,reader,0)) return 1;
+    double w1,w2,y1,y2;
+    int i1,i2;
+    find_closest_points(time, reader->vars[0], reader->nrows, &i1, &w1, &i2, &w2);
+    if (i2 == -1) {
+      return omc_matlab4_read_single_val(res,reader,var->index,i1);
+    } else if (i1 == -1) {
+      return omc_matlab4_read_single_val(res,reader,var->index,i2);
+    } else {
+      if (omc_matlab4_read_single_val(&y1,reader,var->index,i1)) return 1;
+      if (omc_matlab4_read_single_val(&y2,reader,var->index,i2)) return 1;
+      *res = w1*y1 + w2*y2;
+      return 0;
+    }
+    //fprintf(stderr, "i1 %d w1 %g i2 %d w2 %g\n", i1, w1, i2, w2);
+    //return 1;
+  }
+  return 0;
+}
+
+void omc_matlab4_print_all_vars(FILE *stream, ModelicaMatReader *reader)
+{
+  int i;
+  fprintf(stream, "allSortedVars(\"%s\") => {", reader->fileName);
+  for (i=0; i<reader->nall; i++)
+    fprintf(stream, "\"%s\",", reader->allInfo[i].name);
+  fprintf(stream, "}\n");
+}
+
+#if 0
+int main(int argc, char** argv)
+{
+  ModelicaMatReader reader;
+  const char *msg;
+  int i;
+  double r;
+  ModelicaMatVariable_t *var;
+  if (argc < 2) {
+    fprintf(stderr, "Usage: %s filename.mat var0 ... varn\n", *argv);
+    exit(1);
+  }
+  if (0 != (msg=omc_new_matlab4_reader(argv[1],&reader))) {
+    fprintf(stderr, "%s is not in the MATLAB4 subset accepted by OpenModelica: %s\n", argv[1], msg);
+    exit(1);
+  }
+  omc_matlab4_print_all_vars(stderr, &reader);
+  for (i=2; i<argc; i++) {
+    int printAll = *argv[i] == '.';
+    char *name = argv[i] + printAll;
+    var = omc_matlab4_find_var(&reader, name);
+    if (!var) {
+      fprintf(stderr, "%s not found\n", name);
+    } else if (printAll) {
+      int n,j;
+      if (var->isParam) {
+        fprintf(stderr, "%s is param, but tried to read all values", name);
+        continue;
+      }
+      double *vals = omc_matlab4_read_vals(&n,&reader,var->index);
+      if (!vals) {
+        fprintf(stderr, "%s = #FAILED TO READ VALS", name);
+      } else {
+        fprintf(stderr, "  allValues(%s) => {", name);
+        for (j=0; j<n; j++)
+          fprintf(stderr, "%g,", vals[j]);
+        fprintf(stderr, "}\n");
+      }
+    } else {
+      int j;
+      double ts[4] = {-1.0,0.0,0.1,1.0};
+      for (j=0; j<4; j++)
+        if (0==omc_matlab4_val(&r,&reader,var,ts[j]))
+          fprintf(stderr, "  val(\"%s\",%4g) => %g\n", name, ts[j], r);
+        else
+          fprintf(stderr, "  val(\"%s\",%4g) => fail()\n", name, ts[j]);
+    }
+  }
+  omc_free_matlab4_reader(&reader);
+  return 0;
+}
+#endif

c_runtime/read_matlab4.h

@@ -0,0 +1,58 @@
+#ifndef OMC_READ_MATLAB4_H
+#define OMC_READ_MATLAB4_H
+
+#include <stdio.h>
+
+extern const char *omc_mat_Aclass;
+
+typedef struct {
+  uint32_t type;
+  uint32_t mrows;
+  uint32_t ncols;
+  uint32_t imagf;
+  uint32_t namelen;
+} MHeader_t;
+
+typedef struct {
+  char *name; // OpenModelica-style derivatives? Dymola-style?
+  int isParam; // -1=unknown,0=variable,1=param
+  // Parameters are stored in data_1, variables in data_2; parameters are defined at any time, variables only within the simulation start/stop interval
+  int index;
+} ModelicaMatVariable_t;
+
+typedef struct {
+  FILE *file;
+  char *fileName;
+  uint32_t nall;
+  ModelicaMatVariable_t *allInfo; // Sorted array of variables and their associated information
+  uint32_t nparam;
+  double *params; // This has size 2*nparam; the first parameter has row0=startTime,row1=stopTime. Other variables are stored as row0=row1
+  uint32_t nvar,nrows;
+  size_t var_offset; // This is the offset in the FILE
+  double **vars;
+} ModelicaMatReader;
+
+// Returns 0 on success; the error message on error.
+// The internal data is free'd by omc_free_matlab4_reader.
+// The data persists until free'd, and is safe to use in your own data-structures
+const char* omc_new_matlab4_reader(const char *filename, ModelicaMatReader *reader);
+
+// Do not double-free this :)
+void omc_free_matlab4_reader(ModelicaMatReader *reader);
+
+// Returns a variable or NULL
+ModelicaMatVariable_t *omc_matlab4_find_var(ModelicaMatReader *reader, char *varName);
+
+// Writes the number of values in the returned array if nvals is non-NULL
+// Returns all values that the given variable may have.
+// Note: This function is _not_ defined for parameters; check var->isParam and then send the index
+// No bounds checking is performed. The returned data persists until the reader is closed.
+double* omc_matlab4_read_vals(int *nvals, ModelicaMatReader *reader, int varIndex);
+
+// Returns 0 on success
+int omc_matlab4_val(double *res, ModelicaMatReader *reader, ModelicaMatVariable_t *var, double time);
+
+// For debugging
+void omc_matlab4_print_all_vars(FILE *stream, ModelicaMatReader *reader);
+
+#endif