Initial commit 3

C_code Trajectory clustering TBI/dataformat/DOCUM

+  Dataformat
+
+Here are some brief description of the main classes defined in the
+dataformat library.
+
+
+  DataObject
+
+A class to read, save, and operate on rather general "row and column
+based" data. The data is supposed to be stored with one entry per
+line, and with a fixed number of (typically tab-separated) columns,
+where each column has a specific type which may be continuous values,
+discrete values, or symbols (i.e. strings).
+
+Internally all discrete and symbolic data is stored as integers and
+continuous data as floats, In more detail each data entry (row) is
+stored as a vector of the union intfloat, which can store either an
+integer or a float. There may be unknown values anywhere in the data,
+which is then stored internally in the intfloat as the integer -1. A
+Format object keeps track of the format of each column and how to
+interpret the internal values. The format can either be read from file
+or constructed internally.
+
+Some selected methods:
+
+DataObject(char* formatfile, char* datafile);
+DataObject(Format* format, char* datafile);
+
+Constructors. Read in data from file, either using a format file or an
+internally constructed format description. By omitting the data file
+name, an empty DataObject is constructed, which can then be filled by
+entries.
+
+int size();
+
+The number of entries in the DataObject.
+
+int length();
+
+The number of values (columns) of each entry (row) in the dataObject.
+
+union intfloat* newentry();
+
+Creates and returns a new entry (row) in the DataObject.
+
+void sort(int (*func)(union intfloat* v1, union intfloat* v2));
+
+Sorts the data entries according to the ordering function.
+
+int save(char* datafile);
+
+Saves the DataObject to file.
+
+Format* format();
+
+Return the Format associated with the DataObject.
+
+To access the element in row i column j of the DataObject data, use
+(*data)[i][j].i or (*data)[i][j].f depending on whether it is a
+discrete or continuous value.
+
+
+  Format and FormatSpec
+
+The Format object keeps track on formats of the values in the
+DataObject. For each column it has a FormatSpec which can be asked to
+interpret a value. Some methods are:
+
+  form->nth(int n)  : Returns the FormatSpec of the n:th column
+
+  form->nth(n)->type() :  Returns an integer depending on the type.  
+                          3 means continuous, in which case .f should be
+                          used, 2 means discrete and 4 means symbolic,
+                          in both of which cases .i should be used.
+
+  form->nth(n)->represent(intfloat v) : Returns the string
+                      representation of the value, as it is stored on
+                      file. Is useful to find the symbolic
+                      representation of an internal value.
+
+  form->nth(n)->interpret(char* str)  : Return the internal intfloat
+                      representation corresponding to he string
+                      value. Useful to find the integer corresponding
+                      to a symbolic value.
+
+  form->nth(n)->getnum()  : In the case of discrete and symbolic
+                            attributes, returns the number of
+                            different possible values in that column.
+
+
+
+  IndexTable
+
+A helper class that is used to assign integer indices to strings. Is
+used for symbolic attributes, but can be used separately to map
+back and forth between unique integers to strings. Implemented as a
+combined hash table and linked list, to get both fast lookup and 
+sorted order.
+
+Some methods:
+
+  IndexTable()  : Creator. Creates a new empty table.
+
+  void insert(char* str)  : Inserts a new string, in sort order.
+
+  void insert_last(char* str)  : Inserts a new string last, i.e no
+                                 resorting and thus not changing
+                                 previous indices.
+
+  int get(char* str)  : returns the index of the string.
+
+  char* nth(int n)    : returns the string with the index.
+
+
+
+
+  TokenLink* readtokens(FILE* f);
+
+Another useful helper function, that reads lines from files and parses
+them into tokens. Used by DataObject when reading data files, but can
+be used separately for reading lines of tab or space separated tokens
+from any file. A TokenLink is a single linked list declare as:
+
+struct TokenLink {
+  char* token;
+  TokenLink* next;
+};
+
+Other functions used together with readtokens are:
+
+  void freetokens(TokenLink* list) : Must be used to free the list
+                                     returned by readtokens.
+
+  int length(TokenLink* list)      : The number of tokens in the list.
+

C_code Trajectory clustering TBI/hgm/Makefile

+# Makefile
+
+ifdef OPT
+OFLAGS = -O3
+else
+# Mac OS: uncomment if problems with linking libraries not compiled for 64 bits architecture
+#OFLAGS = -m32 -O2
+OFLAGS = -O2
+endif
+
+LDLIBS = -L../dataformat -ldataformat
+INCL = -I../include -I../dataformat
+
+CXX = g++
+# CXXFLAGS = $(OFLAGS) -DGZLIB -Wall $(PG) ${DEFINES}
+CXXFLAGS = $(OFLAGS) -Wall $(INCL) ${DEFINES}
+CC = g++
+CFLAGS = $(OFLAGS) -Wall $(INCL) ${DEFINES}
+
+OBJS = hgm.o ../include/hmatrix.o ../include/gamma.o
+
+all: libhgm.a hgmtest hgmtest_graph hgmtest_ts_daily
+
+hgmtest: hgmtest.o ${OBJS}
+
+hgmtest_graph: hgmtest_graph.o ${OBJS}
+
+hgmtest_ts_daily: hgmtest_ts_daily.o ${OBJS}
+
+libhgm.a: ${OBJS}
+	rm -f libhgm.a
+	ar clq libhgm.a $^
+	ranlib libhgm.a
+
+clean:
+	rm -f hgm.o hgmtest.o hgmtest_graph.o hgmtest_ts_daily.o
+
+depend:
+	rm -f make.depend
+	${CC} -MM *.cc ${CFLAGS} > make.depend
+
+include make.depend

C_code Trajectory clustering TBI/hgm/hgm.hh

+ /*
+ --------------------------------------------------------------------------
+ Copyright (C) 2020 Anders Holst, RISE AB, anders.holst@ri.se
+
+ This code is free software: you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This code is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ --------------------------------------------------------------------------
+ */
+
+
+#include "random.hh"
+#include "growable.hh"
+#include "hmatrix.hh"
+#include "format.hh"
+#include "formatcont.hh"
+#include "data.hh"
+
+
+struct FeetList {
+  FeetList(FormatSpec* fs, int ind, FeetList* nx = 0) { index = ind; fspec = fs; next = nx; };
+  FeetList(FeetList& fl) { index = fl.index; fspec = fl.fspec; next = 0; };
+  ~FeetList() { if (next) delete next; };
+  FeetList* copy() { FeetList* fl = new FeetList(*this); if (next) fl->next = next->copy(); return fl; };
+  int length() { int len; FeetList* ll; for(len=0, ll=this; ll; len++, ll=ll->next); return len; };
+  int issubset(FeetList* fl);
+  int isidentical(FeetList* fl) { FeetList* ll; for (ll=this; ll && fl && ll->index == fl->index; ll=ll->next, fl=fl->next); return !(ll || fl); };
+  int issame(FeetList* fl) { return (fl && length() == fl->length() && issubset(fl)); };
+  int maxindex() { int mi=-1; FeetList* ll; for(ll=this; ll; ll=ll->next) if (ll->index > mi) mi=ll->index; return mi+1; };
+  int anyunknown(union intfloat* vec) { FeetList* ll; for(ll=this; ll; ll=ll->next) if (vec[ll->index].i == -1) return 1; return 0; };
+  int anyknown(union intfloat* vec) { FeetList* ll; for(ll=this; ll; ll=ll->next) if (vec[ll->index].i != -1) return 1; return 0; };
+  FeetList* knownfeet(union intfloat* vec, int rev = 0);
+  FeetList* overlap(FeetList* fl);
+  void printlegs();
+  void printvalues(union intfloat* vec);
+  int index;
+  FormatSpec* fspec;
+  FeetList* next;
+};
+
+extern void AddFoot(FeetList*& feet, int ind, Format* form);
+
+enum ModelType {
+  EmptyType,
+  BernoulliType,
+  GaussianType,
+  ProductType,
+  MixtureType
+};
+
+
+class Model {
+  friend class ProductModel;
+  friend class MixtureModel;
+public:
+  Model(FeetList* l) { legs = (l ? l->copy() : 0); dim = (l ? l->length() : 0); count = 0.0; alpha = 0.0; prior = 0; ownprior = 0; prepared = 0; };
+  virtual ~Model() { if (legs) delete legs; if (ownprior && prior) delete prior; };
+  virtual FeetList* Feet() { return legs; };
+  virtual ModelType Type() = 0;
+  virtual void Clear() = 0;
+  virtual Model* MakePriorModel(int force = 0) = 0;
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0) = 0;
+  virtual Model* Copy() = 0;
+  virtual Model* GetPrior() { return prior; };
+  virtual void Estimate(DataObject* data, double* weights = 0) = 0;
+  virtual void EstimateInit() = 0;
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0) = 0;
+  virtual union intfloat* Generate(union intfloat* vec) = 0;
+  virtual Model* Margin(FeetList* l) = 0;
+  virtual Model* PredictDistr(union intfloat* vec) = 0;
+  virtual void PredictValues(union intfloat* vec) = 0;
+  virtual double Mode(union intfloat* vec) = 0;
+  virtual double LogProbability(union intfloat* vec) = 0;
+  virtual double Probability(union intfloat* vec) = 0;
+  virtual double Anomaly(union intfloat* vec) = 0;
+  virtual double AnomalyByProb(double logprob) = 0;
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode) = 0;
+  virtual double Entropy() = 0;
+  virtual double LogModelProb() = 0;
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec) = 0;
+  virtual void PrintModel(int verbose = 0, int indent = 0) = 0;
+  virtual void DemoteModel(double fact) = 0;
+  virtual Model* ModelAverage(int num, Model** modvec) = 0;
+protected:
+  virtual void prepare() {};
+  FeetList* legs;
+  int dim;
+  double count;
+  double alpha;
+  Model* prior;
+  int ownprior;
+  int prepared;
+};
+
+
+class EmptyModel : public Model {
+public:
+  EmptyModel() : Model(0) {};
+  virtual ~EmptyModel() {};
+  virtual ModelType Type() { return EmptyType; };
+  virtual void Clear() {};
+  virtual Model* MakePriorModel(int force = 0) { return (force ? new EmptyModel() : 0); };
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0) { if (pmod && own) delete pmod; };
+  virtual EmptyModel* Copy() { return new EmptyModel(); };
+  virtual void Estimate(DataObject* data, double* weights = 0) {};
+  virtual void EstimateInit() {};
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0) {};
+  virtual union intfloat* Generate(union intfloat* vec) { return vec; };
+  virtual Model* Margin(FeetList* l) { return new EmptyModel(); };
+  virtual Model* PredictDistr(union intfloat* vec) { return new EmptyModel(); };
+  virtual void PredictValues(union intfloat* vec) {};
+  virtual double Mode(union intfloat* vec) { return 0.0; };
+  virtual double LogProbability(union intfloat* vec) { return 0.0; };
+  virtual double Probability(union intfloat* vec) { return 1.0; };
+  virtual double Anomaly(union intfloat* vec) { return 0.0; };
+  virtual double AnomalyByProb(double logprob) { return 0.0; };
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode) {};
+  virtual double Entropy() { return 0.0; };
+  virtual double LogModelProb() { return 0.0; };
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec) { return 0.0; };
+  virtual void PrintModel(int verbose = 0, int indent = 0);
+  virtual void DemoteModel(double fact) {};
+  virtual Model* ModelAverage(int num, Model** modvec) { return new EmptyModel(); };
+protected:
+};
+
+
+class BernoulliModel : public Model {
+  friend class MixtureModel;
+public:
+  BernoulliModel(FeetList* l);
+  virtual ~BernoulliModel();
+  virtual ModelType Type() { return BernoulliType; };
+  virtual void Clear();
+  virtual Model* MakePriorModel(int force = 0);
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0);
+  virtual BernoulliModel* Copy();
+  virtual void Estimate(DataObject* data, double* weights = 0);
+  virtual void EstimateInit();
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0);
+  virtual union intfloat* Generate(union intfloat* vec);
+  virtual Model* Margin(FeetList* l);
+  virtual Model* PredictDistr(union intfloat* vec);
+  virtual void PredictValues(union intfloat* vec);
+  virtual double Mode(union intfloat* vec);
+  virtual double LogProbability(union intfloat* vec);
+  virtual double Probability(union intfloat* vec);
+  virtual double Anomaly(union intfloat* vec);
+  virtual double AnomalyByProb(double logprob);
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode);
+  virtual double Entropy();
+  virtual double LogModelProb();
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec);
+  virtual void PrintModel(int verbose = 0, int indent = 0);
+  virtual void DemoteModel(double fact);
+  virtual BernoulliModel* ModelAverage(int num, Model** modvec);
+  void GetProbabilities(double* pvec);
+  void SetProbabilities(double* pvec);
+protected:
+  virtual void prepare();
+  int findindex(union intfloat* vec);
+  int totlen;
+  int* lens;
+  double* carray;
+  double* probs;
+};
+
+
+class GaussianModel : public Model {
+public:
+  GaussianModel(FeetList* l);
+  virtual ~GaussianModel();
+  virtual ModelType Type() { return GaussianType; };
+  virtual void Clear();
+  virtual Model* MakePriorModel(int force = 0);
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0);
+  virtual GaussianModel* Copy();
+  virtual void Estimate(DataObject* data, double* weights = 0);
+  virtual void EstimateInit();
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0);
+  virtual union intfloat* Generate(union intfloat* vec);
+  virtual Model* Margin(FeetList* l);
+  virtual Model* PredictDistr(union intfloat* vec);
+  virtual void PredictValues(union intfloat* vec);
+  virtual double Mode(union intfloat* vec);
+  virtual double LogProbability(union intfloat* vec);
+  virtual double Probability(union intfloat* vec);
+  virtual double Anomaly(union intfloat* vec);
+  virtual double AnomalyByProb(double logprob);
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode);
+  virtual double Entropy();
+  virtual double LogModelProb();
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec);
+  virtual void PrintModel(int verbose = 0, int indent = 0);
+  virtual void DemoteModel(double fact);
+  virtual GaussianModel* ModelAverage(int num, Model** modvec);
+  void GetMeanVariance(double* m, HMatrix* v);
+  void SetMeanVariance(double* m, HMatrix* v);
+protected:
+  virtual void prepare();
+  void fillfrommodel(Model* mod, double* tmean, HMatrix* tvar);
+  double* mean0;
+  double* mean;
+  HMatrix* var0;
+  HMatrix* var;
+};
+
+
+class ProductModel : public Model {
+public:
+  ProductModel();
+  virtual ~ProductModel();
+  void InsertModel(Model* mod);
+  virtual ModelType Type() { return ProductType; };
+  virtual void Clear();
+  virtual Model* MakePriorModel(int force = 0);
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0);
+  virtual ProductModel* Copy();
+  virtual void Estimate(DataObject* data, double* weights = 0);
+  virtual void EstimateInit();
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0);
+  virtual union intfloat* Generate(union intfloat* vec);
+  virtual Model* Margin(FeetList* l);
+  virtual Model* PredictDistr(union intfloat* vec);
+  virtual void PredictValues(union intfloat* vec);
+  virtual double Mode(union intfloat* vec);
+  virtual double LogProbability(union intfloat* vec);
+  virtual double Probability(union intfloat* vec);
+  virtual double Anomaly(union intfloat* vec);
+  virtual double AnomalyByProb(double logprob);
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode);
+  virtual double Entropy();
+  virtual double LogModelProb();
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec);
+  virtual void PrintModel(int verbose = 0, int indent = 0);
+  virtual void DemoteModel(double fact);
+  virtual ProductModel* ModelAverage(int num, Model** modvec);
+  int NumModels() { return nmodels; };
+  Model* GetModel(int ind) { return (ind >= 0 && ind < nmodels ? models[ind] : 0); };
+protected:
+  void fillcounts(int* countvec, int ind, int weight);
+  void setupgenorder_iter(int ind, int* done);
+  void setupgenorder();
+  void setupinvstructure();
+  int nmodels;
+  Growable<Model*> models;
+  Growable<int*> structure;
+  Growable<int*> invstructure;
+  int ngenorder;
+  int* genorder;
+  int initedinvstructure;
+};
+
+
+enum MixtureState { Incomplete, Untrained, BurnIn, Stable };
+
+class MixtureModel : public Model {
+public:
+  MixtureModel(FeetList* l, FeetList* cidx = 0);
+  virtual ~MixtureModel();
+  void InsertModel(Model* mod);
+  virtual ModelType Type() { return MixtureType; };
+  virtual void Clear();
+  virtual Model* MakePriorModel(int force = 0);
+  virtual void SetPrior(double ap, Model* pmod = 0, int own = 0);
+  virtual MixtureModel* Copy();
+  virtual void Estimate(DataObject* data, double* weights = 0);
+  virtual void EstimateFromBelong(DataObject* data, DataObject* belongdata, int initstyle);
+  virtual void EstimateInit();
+  virtual void EstimateIncr(union intfloat* vec, double weight = 1.0);
+  virtual union intfloat* Generate(union intfloat* vec);
+  virtual Model* Margin(FeetList* l);
+  virtual Model* PredictDistr(union intfloat* vec);
+  virtual void PredictValues(union intfloat* vec);
+  virtual double Mode(union intfloat* vec);
+  virtual double LogProbability(union intfloat* vec);
+  virtual double Probability(union intfloat* vec);
+  virtual double Anomaly(union intfloat* vec);
+  virtual double AnomalyByProb(double logprob);
+  virtual void AnomalyAuxStats(double& logpmean, double& logpvar, double& logpmode);
+  virtual double Entropy();
+  virtual double LogModelProb();
+  virtual double AdjustToMargins(int num, Model** modvec, int* countvec);
+  virtual void PrintModel(int verbose = 0, int indent = 0);
+  virtual void DemoteModel(double fact);
+  virtual MixtureModel* ModelAverage(int num, Model** modvec);
+  int NumModels() { return nmodels; };
+  Model* GetModel(int ind) { return (ind >= 0 && ind < nmodels ? models[ind] : 0); };
+  int Classify(union intfloat* vec);
+  int Classifyprob(union intfloat* vec, double* pvec);
+  void GetProbabilities(double* pvec);
+  void RegisterDiff();
+  Growable<double>* GetDiffs();
+protected:
+  void init_belong(double** belong, DataObject* data, double* weights);
+  double calc_belong(double** belong, DataObject* data, double* weights);
+  void estimate_from_belong(double** belong, DataObject* data);
+  int nmodels;
+  FeetList* clindex;
+  Growable<Model*> models;
+  Growable<double> probs;
+  MixtureState state;
+  Growable<double>* diffvector;
+};

C_code Trajectory clustering TBI/include/gamma.cc

+ /*
+ --------------------------------------------------------------------------
+ Copyright (C) 2006, 2015 SICS Swedish ICT AB
+
+ Main author: Anders Holst <aho@sics.se>
+
+ The code in this file is based on publicly published algorithms.
+
+ This code is free software: you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This code is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ --------------------------------------------------------------------------
+ */
+
+#include <math.h>
+
+double lngamma(double x)
+{
+  static double lastx = -1.0;
+  static double lastgl = 0.0;
+  double tmp; 
+  if (x==lastx) return lastgl;
+  lastx = x;
+  tmp = 2.5066282746310005*(1.000000000190015 + 76.18009172947146/(x+1.0) - 86.50532032941677/(x+2.0) + 24.01409824083091/(x+3.0) - 1.231739572450155/(x+4.0) + 0.001208650973866179/(x+5.0) - 0.000005395239384953/(x+6.0));
+  return (lastgl = (x + 0.5)*log(x + 5.5) - x - 5.5 + log(tmp/x));
+}
+
+double digamma(double x)
+{
+  double tmp1, tmp2; 
+  tmp1 = 1.000000000190015 + 76.18009172947146/x - 86.50532032941677/(x+1.0) + 24.01409824083091/(x+2.0) - 1.231739572450155/(x+3.0) + 0.001208650973866179/(x+4.0) - 0.000005395239384953/(x+5.0);
+  tmp2 = 76.18009172947146/(x*x) - 86.50532032941677/((x+1.0)*(x+1.0)) + 24.01409824083091/((x+2.0)*(x+2.0)) - 1.231739572450155/((x+3.0)*(x+3.0)) + 0.001208650973866179/((x+4.0)*(x+4.0)) - 0.000005395239384953/((x+5.0)*(x+5.0));
+  return log(x+4.5) + (x-0.5)/(x+4.5) - 1.0 - tmp2/tmp1;
+}
+
+double ddigamma(double x)
+{
+  double tmp1, tmp2, tmp3; 
+  tmp1 = 1.000000000190015 + 76.18009172947146/x - 86.50532032941677/(x+1.0) + 24.01409824083091/(x+2.0) - 1.231739572450155/(x+3.0) + 0.001208650973866179/(x+4.0) - 0.000005395239384953/(x+5.0);
+  tmp2 = 76.18009172947146/(x*x) - 86.50532032941677/((x+1.0)*(x+1.0)) + 24.01409824083091/((x+2.0)*(x+2.0)) - 1.231739572450155/((x+3.0)*(x+3.0)) + 0.001208650973866179/((x+4.0)*(x+4.0)) - 0.000005395239384953/((x+5.0)*(x+5.0));
+  tmp3 = 76.18009172947146/(x*x*x) - 86.50532032941677/((x+1.0)*(x+1.0)*(x+1.0)) + 24.01409824083091/((x+2.0)*(x+2.0)*(x+2.0)) - 1.231739572450155/((x+3.0)*(x+3.0)*(x+3.0)) + 0.001208650973866179/((x+4.0)*(x+4.0)*(x+4.0)) - 0.000005395239384953/((x+5.0)*(x+5.0)*(x+5.0));
+  return 1.0/(x+4.5) + 5.0/((x+4.5)*(x+4.5)) + (2*tmp3*tmp1 - tmp2*tmp2)/(tmp1*tmp1);
+}
+
+double incgammap_s(double x, double y)
+// The incomplete gamma function P(x, y) evaluated by its series representation. 
+{
+  int i;
+  double sum, tmp, xx;
+  if (y <= 0.0) {
+    return 0.0;
+  } else {
+    tmp = sum = 1.0/x;
+    for (i=0, xx=x; fabs(tmp) > 1e-15*fabs(sum) && i<100; i++)
+      xx += 1.0, tmp *= y/xx, sum += tmp;
+    return sum*exp(-y + x*log(y) - lngamma(x));
+  }
+}
+
+double incgammaq_cf(double x, double y)
+// The incomplete gamma function Q(x, y) evaluated by its continued fraction representation.
+{
+  int i;
+  double a, b, c, d, h;
+  b = y+1.0-x;
+  c = 0.0;
+  d = 1.0/b;
+  h = d;
+  for (i=1; fabs(d-c) > 1e-15*fabs(c) && i<=100; i++) {
+    a = -i*(i-x);
+    b += 2.0;
+    d = 1.0/(a*d+b);
+    c = 1.0/(a*c+b);
+    h *= d/c;
+  }
+  return exp(-y + x*log(y) - lngamma(x))*h;
+}
+
+double incgammap(double x, double y)
+// Returns the incomplete gamma function P(x, y).
+{
+  if (y < 0.0 || x <= 0.0)
+    return 0.0;
+  if (y < x+1.0)
+    return incgammap_s(x, y);
+  else
+    return 1.0-incgammaq_cf(x, y);
+}
+
+double incgammaq(double x, double y)
+// Returns the incomplete gamma function Q(a, x) = 1-P(a, x).
+{
+  if (y < 0.0 || x <= 0.0)
+    return 0.0;
+  if (y < x+1.0)
+    return 1.0-incgammap_s(x, y);
+  else
+    return incgammaq_cf(x, y);
+}
+

C_code Trajectory clustering TBI/include/gamma.hh

+ /*
+ --------------------------------------------------------------------------
+ Copyright (C) 2006, 2015 SICS Swedish ICT AB
+
+ Main author: Anders Holst <aho@sics.se>
+
+ This code is free software: you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This code is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ --------------------------------------------------------------------------
+ */
+
+double lngamma(double x);
+
+double digamma(double x);
+
+double ddigamma(double x);
+
+double incgammap(double x, double y);
+
+double incgammaq(double x, double y);
+

C_code Trajectory clustering TBI/include/generalhashtable.hh

+#ifndef TEMPLATE_GHASHTABLE
+
+#define TEMPLATE_GHASHTABLE
+
+/*
+  Typical hash and equal functions:
+
+  int hash(const char* str, int bits) {
+    register int h = 0;
+    const unsigned char *p;
+    for (p=(const unsigned char*)str; *p; p++) {
+      h ^= *p;
+      h <<= 1;
+      h = (h >> bits) ^ (h & ~(-1 << bits));
+    }
+    return h;
+  }
+
+  int equal(const char* str1, const char* str2)
+  {
+    return !strcmp(tr1, str2);
+  }
+
+  int hash(int ind, int bits) {
+    register int h;
+    if (ind<0) ind &= 0x7fffffff;
+    for (h=0; ind; h^=ind, ind>>=bits);
+    return h & ~(-1 << bits);
+  }
+
+  int equal(int ind1, int ind2)
+  {
+    return ind1 == ind2;
+  }
+ */
+
+template<class K,class T> class GHTEle {
+public:
+  GHTEle(K k) {
+    key = k;
+  };
+  ~GHTEle() { };
+  K key;
+  T val;
+  GHTEle<K,T>* next;
+};
+
+template<class K,class T,int (*hash)(K k, int b), int (*equal)(K k1, K k2)> class GeneralHashTable {
+public:
+  GeneralHashTable() {
+    num = 0;
+    _size = 0;
+    _blockresize = 0;
+    vec = 0;
+    currele = 0;
+    def = 0;
+    resize(32);
+  }; 
+  GeneralHashTable(const T& d) {
+    num = 0;
+    _size = 0;
+    _blockresize = 0;
+    vec = 0;
+    currele = 0;
+    def = &d;
+    resize(32);
+  }; 
+  ~GeneralHashTable() {
+    GHTEle<K,T> *ele, *ele2;
+    int i;
+    for (i=0; i<_size; i++)
+      for (ele=vec[i]; ele; ele=ele2) {
+        ele2 = ele->next;
+        delete ele;
+      }
+    delete [] vec;
+  };
+  T& operator()(K k) {
+    GHTEle<K,T>* ele;
+    int h = hash(k, _bits);
+    for (ele=vec[h]; ele && !equal(ele->key, k); ele=ele->next);
+    if (!ele)
+      ele = add(h, k);
+    return ele->val;
+  };
+  void reset() {
+    currele = 0;
+    currhind = -1;
+    _blockresize = 1;
+  };
+  int end() {
+    if (currele && currele->next)
+      currele = currele->next;
+    else
+      for (currele=0; currhind<_size-1 && !(currele=vec[++currhind]); );
+    if (currele)
+      return 0;
+    else {
+      _blockresize = 0;
+      return 1;
+    }
+  };
+  T& currval() { return currele->val; }
+  K currind() { return currele->key; }
+  int size() { return num; }
+  int operator!() { return (num == 0); }
+  operator int() { return (num > 0); }
+private:
+  GHTEle<K,T>* add(int h, K k) {
+    GHTEle<K,T>* ele = new GHTEle<K,T>(k);
+    if (def) ele->val = *def;
+    ele->next = vec[h];
+    vec[h] = ele;
+    num++;
+    if (!_blockresize && num > _size*4) 
+      resize(_size*8);
+    return ele;
+  };
+  void resize(int sz) {
+    GHTEle<K,T>** oldvec;
+    GHTEle<K,T> *ele, *ele2;
+    int hind;
+    int i;
+    int oldsize;
+    oldsize = _size;
+    oldvec = vec;
+    _size = sz;
+    for (_bits=-1; sz; _bits++, sz>>=1);
+    vec = new GHTEle<K,T>*[_size];
+    num = 0;
+    for (i=0; i<_size; i++)
+      vec[i] = 0;
+    if (oldvec) {
+      for (i=0; i<oldsize; i++)
+        for (ele=oldvec[i]; ele; ele=ele2) {
+          ele2 = ele->next;
+          hind = hash(ele->key, _bits);
+          ele->next = vec[hind];
+          vec[hind] = ele;
+          ++num;
+        }
+      delete [] oldvec;
+    }
+  };
+  int _size;
+  int _bits;
+  int _blockresize;
+  int num;
+  const T *def;
+  GHTEle<K,T> *currele;
+  int currhind;
+  GHTEle<K,T>** vec;
+};
+
+#ifndef FORHASH
+#define FORHASH( ht, var, key) for( (ht).reset(); !((ht).end())&&((var=(ht).currval()),(key=(ht).currind()),1); )
+#endif
+
+#endif

C_code Trajectory clustering TBI/include/generalhashtable_v0.1.hh

+#ifndef TEMPLATE_GHASHTABLE
+
+#define TEMPLATE_GHASHTABLE
+
+/*
+  Typical hash and equal functions:
+
+  int hash(const char* str, int bits) {
+    register int h = 0;
+    const unsigned char *p;
+    for (p=(const unsigned char*)str; *p; p++) {
+      h ^= *p;
+      h <<= 1;
+      h = (h >> bits) ^ (h & ~(-1 << bits));
+    }
+    return h;
+  }
+
+  int equal(const char* str1, const char* str2)
+  {
+    return !strcmp(tr1, str2);
+  }
+
+  int hash(int ind, int bits) {
+    register int h;
+    if (ind<0) ind &= 0x7fffffff;
+    for (h=0; ind; h^=ind, ind>>=bits);
+    return h & ~(-1 << bits);
+  }
+
+  int equal(int ind1, int ind2)
+  {
+    return ind1 == ind2;
+  }
+ */
+
+template<class K,class T> class GHTEle {
+public:
+  GHTEle(K k) {
+    key = k;
+  };
+  ~GHTEle() { };
+  K key;
+  T val;
+  GHTEle<K,T>* next;
+};
+
+template<class K,class T,int (*hash)(K k, int b), int (*equal)(K k1, K k2)> class GeneralHashTable {
+public:
+  GeneralHashTable() {
+    num = 0;
+    _size = 0;
+    _blockresize = 0;
+    vec = 0;
+    currele = 0;
+    def = 0;
+    resize(32);
+  }; 
+  GeneralHashTable(const T& d) {
+    num = 0;
+    _size = 0;
+    _blockresize = 0;
+    vec = 0;
+    currele = 0;
+    def = &d;
+    resize(32);
+  }; 
+  ~GeneralHashTable() {
+    GHTEle<K,T> *ele, *ele2;
+    int i;
+    for (i=0; i<_size; i++)
+      for (ele=vec[i]; ele; ele=ele2) {
+        ele2 = ele->next;
+        delete ele;
+      }
+    delete [] vec;
+  };
+  T& operator()(K k) {
+    GHTEle<K,T>* ele;
+    int h = hash(k, _bits);
+    for (ele=vec[h]; ele && equal(ele->key, k); ele=ele->next);
+    if (!ele)
+      ele = add(h, k);
+    return ele->val;
+  };
+  void reset() {
+    currele = 0;
+    currhind = -1;
+    _blockresize = 1;
+  };
+  int end() {
+    if (currele && currele->next)
+      currele = currele->next;
+    else
+      for (currele=0; currhind<_size-1 && !(currele=vec[++currhind]); );
+    if (currele)
+      return 0;
+    else {
+      _blockresize = 0;
+      return 1;
+    }
+  };
+  T& currval() { return currele->val; }
+  char* currind() { return currele->key; }
+  int size() { return num; }
+  int operator!() { return (num == 0); }
+  operator int() { return (num > 0); }
+private:
+  GHTEle<K,T>* add(int h, K k) {
+    GHTEle<K,T>* ele = new GHTEle<K,T>(k);
+    if (def) ele->val = *def;
+    ele->next = vec[h];
+    vec[h] = ele;
+    num++;
+    if (!_blockresize && num > _size*4) 
+      resize(_size*8);
+    return ele;
+  };
+  void resize(int sz) {
+    GHTEle<K,T>** oldvec;
+    GHTEle<K,T> *ele, *ele2;
+    int hind;
+    int i;
+    int oldsize;
+    oldsize = _size;
+    oldvec = vec;
+    _size = sz;
+    for (_bits=-1; sz; _bits++, sz>>=1);
+    vec = new GHTEle<K,T>*[_size];
+    num = 0;
+    for (i=0; i<_size; i++)
+      vec[i] = 0;
+    if (oldvec) {
+      for (i=0; i<oldsize; i++)
+        for (ele=oldvec[i]; ele; ele=ele2) {
+          ele2 = ele->next;
+          hind = hash(ele->key, _bits);
+          ele->next = vec[hind];
+          vec[hind] = ele;
+          ++num;
+        }
+      delete [] oldvec;
+    }
+  };
+  int _size;
+  int _bits;
+  int _blockresize;
+  int num;
+  const T *def;
+  GHTEle<K,T> *currele;
+  int currhind;
+  GHTEle<K,T>** vec;
+};
+
+#ifndef FORHASH
+#define FORHASH( ht, var, key) for( (ht).reset(); !((ht).end())&&((var=(ht).currval()),(key=(ht).currind()),1); )
+#endif
+
+#endif

C_code Trajectory clustering TBI/include/hmatrix.cc

+ /*
+ --------------------------------------------------------------------------
+ Copyright (C) 2006, 2015 SICS Swedish ICT AB
+
+ Main author: Anders Holst <aho@sics.se>
+
+ This code is free software: you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This code is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ --------------------------------------------------------------------------
+ */
+
+#include <math.h>
+#include "hmatrix.hh"
+
+/*
+class HMatrix
+{
+public:
+  HMatrix(int n, int tp) { typ = tp; dim = n; len = (tp==2 ? n*(n+1)/2 : n); uvec = 0; livec = 0; vec = new double[len]; for (int i=0; i<len; i++) vec[i]=0.0; };
+  HMatrix(HMatrix& m) { typ = m.typ; dim = m.dim; len = m.len; uvec = 0; livec = 0; vec = new double[len]; for (int i=0; i<len; i++) vec[i]=m.vec[i]; };
+  virtual ~HMatrix() { delete [] vec; if (uvec) delete [] uvec; if (livec) delete [] livec; };
+  virtual void set(HMatrix& m) { clean(); if (dim != m.dim || typ != m.typ) { delete [] vec; typ = m.typ; dim = m.dim; len = (typ==2 ? dim*(dim+1)/2 : dim); vec = new double[len]; }; for (int i=0; i<len; i++) vec[i]=m.vec[i]; };
+  virtual double& operator()(int i, int j);
+  virtual HMatrix* permuted(int pdim, int* perm);
+  virtual void zero() { clean(); for (int i=0; i<len; i++) vec[i]=0.0; };
+  virtual void unit() { clean(); if (typ==2) { for (int i=0; i<len; i++) vec[i]=0.0; for (int i=0; i<dim; i++) vec[(2*dim*i-i*i+i)/2]=1.0; } else for (int i=0; i<len; i++) vec[i]=1.0; };
+  virtual void scale(double sc) { clean(); for (int i=0; i<len; i++) vec[i] *= sc; };
+  virtual void interpolate(HMatrix* m, double fact) { clean(); for (int i=0; i<len; i++) vec[i] = vec[i]*(1.0-fact) + m->vec[i]*fact; };
+  virtual void addweighted(HMatrix* m, double fact) { clean(); for (int i=0; i<len; i++) vec[i] += m->vec[i]*fact; };
+  virtual void accum(double* av, double sc);
+  virtual void prod(double* v, double* dest);
+  virtual void produ(double* v, double* dest);
+  virtual void prodli(double* v, double* dest);
+  virtual double sqprod(double* v);
+  virtual double sqprodinv(double* v);
+  virtual double sqprodinv_part(double* v, int cdim);
+  virtual double sqprodinv_cond(double* v, int cdim);
+  virtual void var_cond(HMatrix* dest, int cdim);
+  virtual void mean_cond(double* v1, double* v2, int cdim);
+  virtual double det();
+  virtual double det_part(int cdim);
+  virtual double det_cond(int cdim);
+  virtual double trace();
+  virtual double trace2mat(HMatrix* m);
+  virtual HMatrix* inverse();
+protected:
+  void clean() { if (uvec) delete [] uvec; if (livec) delete [] livec; uvec = 0; livec = 0; };
+  void factor();
+  int typ;
+  int dim;
+  int len;
+  double* vec;
+  double* uvec;
+  double* livec;
+};
+
+*/ 
+
+ /*
+  *  Half matrix, representing a covariance matrix and its cholesky factorization 
+  *  C = U' * U, inv(C) = Linv' * Linv
+  *  
+  *            0123             0
+  *  C and U:   456      Linv:  14
+  *              78             257
+  *               9             3689
+  */
+
+double& HMatrix::operator()(int i, int j)
+{
+  static double dummy = 0.0;
+  if (i>=0 && i<dim && j>=0 && j<dim) {
+    if (typ == 2) {
+      if (j<=i)
+        return vec[i + (2*dim*j-j*j-j)/2];
+      else
+        return vec[j + (2*dim*i-i*i-i)/2];
+    } else {
+      if (i == j)
+        return vec[i];
+      else {
+        dummy = 0.0;
+        return dummy;
+      }
+    }
+  }
+  else {
+    dummy = 0.0;
+    return dummy;
+  }
+}
+
+void HMatrix::accum(double* v, double sc)
+{
+  double* p;
+  double a;
+  int i, j;
+  clean();
+  if (typ == 2) {
+    for (i=0, p=vec; i<dim; i++) {
+      a = v[i] * sc;
+      for (j=i; j<dim; j++, p++)
+        *p += a * v[j];
+    }
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      *p += v[i] * v[i] * sc;
+  }
+}
+
+// The perm vector corresponds to the new matrix, and the values in it the old col/row
+HMatrix* HMatrix::permuted(int pdim, int* perm)
+{
+  int i, j;
+  double *p;
+  HMatrix* mat = new HMatrix(pdim, typ);
+  if (typ == 2) {
+    for (i=0, p=mat->vec; i<pdim; i++) {
+      for (j=i; j<pdim; j++, p++)
+        *p = (*this)(perm[i],perm[j]);
+    }
+  } else {
+    for(i=0; i<pdim; i++)
+      mat->vec[i] = vec[perm[i]];
+  }
+  return mat;
+}
+
+HMatrix* HMatrix::inverse()
+{
+  int i, j, k;
+  double *p, *q, *r;
+  HMatrix* inv = new HMatrix(dim, typ);
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0, p=livec, q=p, r=inv->vec; i<dim; i++, q=p) {
+      for (j=i; j<dim; p++, r++, q+=(dim-j), j++) {
+        *r = 0.0;
+        for (k=0; k<dim-j; k++)
+          *r += p[k]*q[k];
+      }
+    }
+  } else {
+    for(i=0; i<dim; i++)
+      inv->vec[i] = (vec[i] <= 0.0 ? HUGE_VAL : 1.0/vec[i]);
+  }
+  return inv;
+}
+
+void HMatrix::produ(double* v, double* dest)
+{
+  int i, j;
+  double* p;
+  double e;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+// Multiplication from left, useful for generating random numbers
+    for (i=0; i<dim; i++) {
+      e = 0.0;
+      for (j=0, p=uvec+i; j<=i; j++, p+=dim-j)
+        e += *p * v[j];
+      dest[i] = e;
+    }
+/*
+// Multiplication from right, not so useful
+    for (i=0, p=uvec; i<dim; i++) {
+      e = 0.0;
+      for (j=i; j<dim; j++, p++)
+        e += *p * v[j];
+      dest[i] = e;
+    }
+*/
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      dest[i] = sqrt(*p) * v[i];
+  }
+}
+
+void HMatrix::prod(double* v, double* dest)
+{
+  int i, j;
+  double* p;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<dim; i++)
+      dest[i] = 0.0;
+    for (i=0, p=vec; i<dim; i++) {
+      dest[i] += *p * v[i];
+      p++;
+      for (j=i+1; j<dim; j++, p++) {
+        dest[i] += *p * v[j];
+        dest[j] += *p * v[i];
+      }
+    }
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      dest[i] = *p * v[i];
+  }
+}
+
+void HMatrix::prodli(double* v, double* dest)
+{
+  int i, j;
+  double* p;
+  double e;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<dim; i++) {
+      e = 0.0;
+      for (j=0, p=livec+i; j<=i; j++, p+=dim-j)
+        e += *p * v[j];
+      dest[i] = e;
+    }
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      dest[i] = v[i] / sqrt(*p <= 1e-12 ? 1e-12 : *p);
+  }
+}
+
+double HMatrix::sqprod(double* v)
+{
+  int i, j;
+  double* p;
+  double r=0.0, e;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0, p=uvec; i<dim; i++) {
+      e = 0.0;
+      for (j=i; j<dim; j++, p++)
+        e += *p * v[j];
+      r += e * e;
+    }
+    return r;
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      r += *p * v[i] * v[i];
+    return r;
+  }
+}
+
+double HMatrix::sqprodinv(double* v)
+{
+  int i, j;
+  double* p;
+  double r=0.0, e;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<dim; i++) {
+      e = 0.0;
+      for (j=0, p=livec+i; j<=i; j++, p+=dim-j)
+        e += *p * v[j];
+      r += e * e;
+    }
+    return r;
+  } else {
+    for (i=0, p=vec; i<dim; i++, p++)
+      r += v[i] * v[i] / (*p <= 1e-12 ? 1e-12 : *p);
+    return r;
+  }
+}
+
+// The square distance of the first cdim dimensions
+double HMatrix::sqprodinv_part(double* v, int cdim)
+{
+  int i, j;
+  double* p;
+  double r=0.0, e;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<cdim; i++) {
+      e = 0.0;
+      for (j=0, p=livec+i; j<=i; j++, p+=dim-j)
+        e += *p * v[j];
+      r += e * e;
+    }
+    return r;
+  } else {
+    for (i=0, p=vec; i<cdim; i++, p++)
+      r += v[i] * v[i] / (*p <= 1e-12 ? 1e-12 : *p);
+    return r;
+  }
+}
+
+// The square distance of the last dim-cdim dimensions, conditionally of the first cdim dimensions
+double HMatrix::sqprodinv_cond(double* v, int cdim)
+{
+  int i, j;
+  double* p;
+  double res, tmp;
+  if (typ == 2) {
+    double* va = new double[dim-cdim];
+    double* vb = new double[dim-cdim];
+    if (!uvec || !livec)
+      factor();
+    // vb = B*v1
+    for (i=0; i<dim-cdim; i++) {
+      vb[i] = 0.0;
+      for (j=0, p=livec+cdim+i; j<cdim; j++, p+=dim-j)
+        vb[i] += *p * v[j];
+    }
+    // dim*(dim+1) - (dim-cdim)*(dim-cdim+1) = 2*dim*cdim -cdim^2+cdim 
+    // va = C'*vb
+    for (i=0; i<dim-cdim; i++) {
+      va[i] = 0.0;
+      for (j=0, p=uvec+(cdim*(2*dim-cdim+1)/2)+i; j<=i; j++, p+=dim-j)
+        va[i] += *p * vb[j];
+    }
+    // vb = v2 + va
+    for (i=0; i<dim-cdim; i++)
+      vb[i] = v[i+cdim] + va[i];
+    // (D*vb)^2
+    res = 0.0;
+    for (i=0; i<dim-cdim; i++) {
+      tmp = 0.0;
+      for (j=0, p=livec+(cdim*(2*dim-cdim+1)/2)+i; j<=i; j++, p+=dim-j)
+        tmp += *p * vb[j];
+      res += tmp * tmp;
+    }
+    delete [] va;
+    delete [] vb;
+    return res;
+  } else {
+    res = 0.0;
+    for (i=cdim, p=vec+cdim; i<dim; i++, p++)
+      res += v[i] * v[i] / (*p <= 1e-12 ? 1e-12 : *p);
+    return res;
+  }
+}
+
+// Expected value of the last dim-cdim dimensions, conditionally of the first cdim dimensions
+void HMatrix::mean_cond(double* v1, double* v2, int cdim)
+{
+  int i, j;
+  double* p;
+  if (typ == 2) {
+    double* vb = new double[dim-cdim];
+    if (!uvec || !livec)
+      factor();
+    // vb = B*v1
+    for (i=0; i<dim-cdim; i++) {
+      vb[i] = 0.0;
+      for (j=0, p=livec+cdim+i; j<cdim; j++, p+=dim-j)
+        vb[i] += *p * v1[j];
+    }
+    // dim*(dim+1) - (dim-cdim)*(dim-cdim+1) = 2*dim*cdim -cdim^2+cdim 
+    // v2 = -C'*vb
+    for (i=0; i<dim-cdim; i++) {
+      v2[i] = 0.0;
+      for (j=0, p=uvec+(cdim*(2*dim-cdim+1)/2)+i; j<=i; j++, p+=dim-j)
+        v2[i] -= *p * vb[j];
+    }
+    delete [] vb;
+  } else {
+    // Diagonal matrix means conditional independence
+    for (i=0; i<dim-cdim; i++)
+      v2[i] = 0.0;
+  }
+}
+
+//  0     0123    00 01 02 03                          
+//  14  *  456  =    11 12 13 +  44 45 46 +        +   
+//  257     78          22 23       55 56    77 78     
+//  3689     9             33          66       88    99
+
+// Covariance matrix of the last dim-cdim dimensions, conditionally of the first cdim dimensions
+void HMatrix::var_cond(HMatrix* dest, int cdim)
+{
+  int i, j, k;
+  double *p, *q, *r, *pp, *rr;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    dest->zero();
+    for (k=cdim, rr=dest->vec, pp=uvec+(cdim*(2*dim-cdim+1)/2); k<dim; rr+=(dim-k), pp+=(dim-k), k++)
+      for (i=0, p=pp, q=p, r=rr; i<dim-k; i++, p++, q=p)
+        for (j=i; j<dim-k; q++, r++, j++)
+          *r += *p * *q;
+  } else {
+    // Diagonal matrix means conditional independence
+    dest->clean();
+    for(i=0; i<dim-cdim; i++)
+      dest->vec[i] = vec[i+cdim];
+  }
+}
+
+double HMatrix::det()
+{
+  int i;
+  double d=1.0;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<dim; i++)
+      d *= uvec[(2*dim*i-i*i+i)/2];
+    return d*d;
+  } else {
+    for (i=0; i<dim; i++)
+      d *= (vec[i] <= 0.0 ? 0.0 : vec[i]);
+    return d;
+  }
+}
+
+// The determinant for the first cdim dimensions
+double HMatrix::det_part(int cdim)
+{
+  int i;
+  double d=1.0;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=0; i<cdim; i++)
+      d *= uvec[(2*dim*i-i*i+i)/2];
+    return d*d;
+  } else {
+    for (i=0; i<cdim; i++)
+      d *= (vec[i] <= 0.0 ? 0.0 : vec[i]);
+    return d;
+  }
+}
+
+// The determinant for the last dim-cdim dimensions, conditionally on the cdim first dimensions
+double HMatrix::det_cond(int cdim)
+{
+  int i;
+  double d=1.0;
+  if (typ == 2) {
+    if (!uvec || !livec)
+      factor();
+    for (i=cdim; i<dim; i++)
+      d *= uvec[(2*dim*i-i*i+i)/2];
+    return d*d;
+  } else {
+    for (i=cdim; i<dim; i++)
+      d *= (vec[i] <= 0.0 ? 0.0 : vec[i]);
+    return d;
+  }
+}
+
+double HMatrix::trace()
+{
+  int i;
+  double tr = 0.0;
+  if (typ == 2) {
+    for (i=0; i<dim; i++)
+      tr += vec[(2*dim*i-i*i+i)/2];
+  } else {
+    for (i=0; i<dim; i++)
+      tr += vec[i];
+  }
+  return tr;
+}
+
+double HMatrix::trace2mat(HMatrix* m)
+{
+  int i, j;
+  double *p1, *p2;
+  double tr = 0.0;
+  if (typ == 2 && m->typ == 2) {
+    for (i=0, p1=vec, p2=m->vec; i<dim; i++) {
+      tr += *p1 * *p2;
+      for (j=i+1, p1++, p2++; j<dim; j++, p1++, p2++)
+        tr += 2 * *p1 * *p2;
+    }
+  } else {
+    for (i=0; i<dim; i++)
+      tr += (*this)(i,i) * (*m)(i,i);
+  }
+  return tr;
+}
+
+void HMatrix::factor()
+{
+  int i, j, k;
+  double *b, *c, *p, *q;
+  double fac;
+  uvec = new double[len];
+  livec = new double[len];
+  for (i=0; i<len; i++)
+    uvec[i] = vec[i], livec[i] = 0.0;
+  for (i=0; i<dim; i++)
+    livec[(2*dim*i-i*i+i)/2] = 1.0;
+  for (i=0, b=uvec; i<dim; i++)
+    if (*b <=0) {
+      for (j=0, c=uvec+i; j<i; j++, c+=dim-j)
+        *c = 0.0;
+      *b++ = 0;
+      for (j=1; j<dim-i; j++)
+        *b++ = 0.0;
+    } else
+      b += dim-i;
+  for (i=0, b=uvec, c=livec; i<dim; b+=dim-i, i++, c++) {
+    if (*b <= 0) {
+      *b = 0;
+      for (j=1, p=b+1; j<dim-i; j++)
+        *p++ = 0.0;
+    }
+    if (i<dim-1) {
+      p = b + dim-i;
+      for (k=i+1; k<dim; k++) {
+        fac = b[k-i] / *b;
+        for (j=k; j<dim; j++, p++)
+          *p -= fac*b[j-i];
+      }
+      for (k=0; k<=i; k++) {
+        q = c + k*(2*dim-k-1)/2;
+        fac = *q / *b;
+        for (j=i+1, q++; j<dim; j++, q++)
+          *q -= fac*b[j-i];
+      }
+    }
+    fac = (*b <= 0 ? 1e20 : 1.0 / sqrt(*b));
+    for (k=i, p=b; k<dim; k++, p++)
+      *p *= fac;
+    for (k=0, q=c; k<=i; k++, q+=dim-k)
+      *q *= fac;
+  }
+}
+

C_code Trajectory clustering TBI/include/hmatrix.hh

+ /*
+ --------------------------------------------------------------------------
+ Copyright (C) 2006, 2015 SICS Swedish ICT AB
+
+ Main author: Anders Holst <aho@sics.se>
+
+ This code is free software: you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This code is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ --------------------------------------------------------------------------
+ */
+
+#ifndef _HMATRIX
+
+class HMatrix
+{
+public:
+  HMatrix(int n, int tp) { typ = tp; dim = n; len = (tp==2 ? n*(n+1)/2 : n); uvec = 0; livec = 0; vec = new double[len]; for (int i=0; i<len; i++) vec[i]=0.0; };
+  HMatrix(HMatrix& m) { typ = m.typ; dim = m.dim; len = m.len; uvec = 0; livec = 0; vec = new double[len]; for (int i=0; i<len; i++) vec[i]=m.vec[i]; };
+  virtual ~HMatrix() { delete [] vec; if (uvec) delete [] uvec; if (livec) delete [] livec; };
+  virtual void set(HMatrix& m) { clean(); if (dim != m.dim || typ != m.typ) { delete [] vec; typ = m.typ; dim = m.dim; len = (typ==2 ? dim*(dim+1)/2 : dim); vec = new double[len]; }; for (int i=0; i<len; i++) vec[i]=m.vec[i]; };
+  virtual double& operator()(int i, int j);
+  virtual HMatrix* permuted(int pdim, int* perm);
+  virtual void zero() { clean(); for (int i=0; i<len; i++) vec[i]=0.0; };
+  virtual void unit() { clean(); if (typ==2) { for (int i=0; i<len; i++) vec[i]=0.0; for (int i=0; i<dim; i++) vec[(2*dim*i-i*i+i)/2]=1.0; } else for (int i=0; i<len; i++) vec[i]=1.0; };
+  virtual void scale(double sc) { clean(); for (int i=0; i<len; i++) vec[i] *= sc; };
+  virtual void interpolate(HMatrix* m, double fact) { clean(); for (int i=0; i<len; i++) vec[i] = vec[i]*(1.0-fact) + m->vec[i]*fact; };
+  virtual void addweighted(HMatrix* m, double fact) { clean(); for (int i=0; i<len; i++) vec[i] += m->vec[i]*fact; };
+  virtual void accum(double* av, double sc);
+  virtual void prod(double* v, double* dest);
+  virtual void produ(double* v, double* dest);
+  virtual void prodli(double* v, double* dest);
+  virtual double sqprod(double* v);
+  virtual double sqprodinv(double* v);
+  virtual double sqprodinv_part(double* v, int cdim);
+  virtual double sqprodinv_cond(double* v, int cdim);
+  virtual void var_cond(HMatrix* dest, int cdim);
+  virtual void mean_cond(double* v1, double* v2, int cdim);
+  virtual double det();
+  virtual double det_part(int cdim);
+  virtual double det_cond(int cdim);
+  virtual double trace();
+  virtual double trace2mat(HMatrix* m);
+  virtual HMatrix* inverse();
+protected:
+  void clean() { if (uvec) delete [] uvec; if (livec) delete [] livec; uvec = 0; livec = 0; };
+  void factor();
+  int typ;
+  int dim;
+  int len;
+  double* vec;
+  double* uvec;
+  double* livec;
+};
+
+#define _HMATRIX
+#endif

C_code Trajectory clustering TBI/include/queue.hh

+#ifndef TEMPLATE_QUEUE
+
+#define TEMPLATE_QUEUE
+
+template<class T> class QueueEle {
+public:
+  QueueEle(T v) { val = v; next = 0; };
+  T val;
+  QueueEle<T>* next;
+};
+
+template<class T> class Queue {
+public:
+  Queue() {
+    num = 0;
+    first = last = curr = 0;
+  }; 
+  Queue(T d) {
+    num = 0;
+    first = last = curr = 0;
+    def = d;
+  }; 
+  ~Queue() { clear(); };
+  T& operator[](int ind) {
+    for (curr=first; ind; ind--, curr=curr->next);
+    return curr->val;
+  };
+  T& operator()() {return curr->val; };
+  Queue<T>& reset() { curr = 0; };
+  int end() {
+    if (!curr)
+      curr = first;
+    else
+      curr = curr->next;
+    return (!curr);
+  };
+  int size() { return num; }
+  int operator!() { return (num == 0); }
+  operator int() { return (num > 0); }
+  int contains(T v, int(*equal)(const T&, const T&)=0) {
+    for (QueueEle<T>* ele = first; ele; ele=ele->next)
+      if (equal ? equal(ele->val, v) : ele->val == v)
+        return 1;
+    return 0;
+  };
+  void enqueue(T v) {
+    QueueEle<T>* ele = new QueueEle<T>(v);
+    if (!last)
+      first = last = ele;
+    else {
+      last = last->next = ele;
+    }
+    num++;
+  };
+  T dequeue() {
+    if (!first)
+      return def;
+    else {
+      T val = first->val;
+      QueueEle<T>* ele = first;
+      first = first->next;
+      if (!first)
+        last = 0;
+      delete ele;
+      num--;
+      return val;
+    }
+  };
+  void clear() {
+    for (QueueEle<T> *e1=first, *e2; e1; e1=e2) {
+      e2=e1->next;
+      delete e1;
+    }
+    num = 0;
+    first = last = curr = 0;
+  };
+
+private:
+  QueueEle<T>* first;
+  QueueEle<T>* last;
+  QueueEle<T>* curr;
+  int num;
+  T def;
+};
+
+#define FORQUEUE( q, var) for((q).reset(); !((q).end())&&((var=(q)()),1); )
+
+#endif

C_code Trajectory clustering TBI/include/queue_v0.1.hh

+#ifndef TEMPLATE_QUEUE
+
+#define TEMPLATE_QUEUE
+
+template<class T> class QueueEle {
+public:
+  QueueEle(T v) { val = v; next = 0; };
+  T val;
+  QueueEle<T>* next;
+};
+
+template<class T> class Queue {
+public:
+  Queue() {
+    num = 0;
+    first = last = curr = 0;
+  }; 
+  Queue(T d) {
+    num = 0;
+    first = last = curr = 0;
+    def = d;
+  }; 
+  ~Queue() { clear(); };
+  T& operator[](int ind) {
+    for (curr=first; ind; ind--, curr=curr->next);
+    return curr->val;
+  };
+  T& operator()() {return curr->val; };
+  Queue<T>& reset() { curr = 0; };
+  int end() {
+    if (!curr)
+      curr = first;
+    else
+      curr = curr->next;
+    return (!curr);
+  };
+  int size() { return num; }
+  int operator!() { return (num == 0); }
+  operator int() { return (num > 0); }
+  void enqueue(T v) {
+    QueueEle<T>* ele = new QueueEle<T>(v);
+    if (!last)
+      first = last = ele;
+    else
+      last->next = ele;
+    num++;
+  };
+  T dequeue() {
+    if (!first)
+      return def;
+    else {
+      T val = first->val;
+      QueueEle<T>* ele = first;
+      first = first->next;
+      if (!first)
+        last = 0;
+      delete ele;
+      num--;
+      return val;
+    }
+  };
+  void clear() {
+    for (QueueEle<T> *e1=first, *e2; e1; e1=e2) {
+      e2=e1->next;
+      delete e1;
+    }
+    num = 0;
+    first = last = curr = 0;
+  };
+
+private:
+  QueueEle<T>* first;
+  QueueEle<T>* last;
+  QueueEle<T>* curr;
+  int num;
+  T def;
+};
+
+#define FORQUEUE( q, var) for((q).reset(); !((q).end())&&((var=(q)()),1); )
+
+#endif

C_code Trajectory clustering TBI/include/random.hh

+#ifndef RANDOM_HH
+#define RANDOM_HH
+
+#include <stdlib.h>
+#include <math.h>
+
+// Ndvndigt fr att f M_PI m.m. i VC++
+#define _USE_MATH_DEFINES
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#include <math.h>
+// extern "C" int random();
+// extern "C" void srandom(unsigned);
+#ifndef _TIME_H
+extern "C" long time(long*);
+#endif
+
+inline void randomize()
+{
+#ifndef GLIBC
+  srand(time(0));
+#else
+  srandom(time(0));
+#endif
+}
+
+inline double uniform()
+{
+#ifndef GLIBC
+  const int maxint = RAND_MAX;
+  return ((rand() % maxint) / (double) maxint);
+#else
+  const int maxint = 2147483647;
+  return ((random() % maxint) / (double) maxint);
+#endif
+}
+
+inline double uniform(double start, double end)
+{
+  return start + (end - start) * uniform();
+}
+
+inline int binary(double prob)
+{
+  return (uniform() < prob);
+}
+
+inline int discrete(int num)
+{
+  return (int) floor(num * uniform());
+}
+
+inline int discrete(int num, double* prob)
+{
+  int i;
+  double s=0.0;
+  double u=uniform();
+  for (i=0; i<num; i++)
+    if (u < (s += prob[i])) return i;
+  return -1;
+}
+
+inline double gauss()
+{
+  static int nextp=0;
+  static double next=0.0;
+  double r, fi;
+  if (nextp) {
+    nextp = 0;
+    return next;
+  } else {
+    r = sqrt(-2 * log(uniform()));
+    fi = 2*M_PI*uniform();
+    nextp = 1;
+    next = r*cos(fi);
+    return r*sin(fi);
+  }
+}
+
+inline double gauss(double mean, double std)
+{
+  return mean + std * gauss();
+}
+
+#endif
+