CHANGED: the base zoom factors to match 2D and 3D zooms
[mldemos:allopens-mldemos.git] / Core / datasetManager.cpp
1 /*********************************************************************
2 MLDemos: A User-Friendly visualization toolkit for machine learning
3 Copyright (C) 2010  Basilio Noris
4 Contact: mldemos@b4silio.com
5
6 This library is free software; you can redistribute it and/or
7 modify it under the terms of the GNU Lesser General Public
8 License as published by the Free Software Foundation; either
9 version 2.1 of the License, or (at your option) any later version.
10
11 This library is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14 Library General Public License for more details.
15
16 You should have received a copy of the GNU Lesser General Public
17 License along with this library; if not, write to the Free
18 Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *********************************************************************/
20 #include "public.h"
21 #include "basicMath.h"
22 #include "mymaths.h"
23 #include "datasetManager.h"
24 #include <fstream>
25 #include <algorithm>
26 #include <map>
27
28 using namespace std;
29
30 /******************************************/
31 /*                                        */
32 /*    DATASET MANAGER                     */
33 /*                                        */
34 /******************************************/
35 u32 DatasetManager::IDCount = 0;
36
37 DatasetManager::DatasetManager(const int dimension)
38 : size(dimension)
39 {
40     bProjected = false;
41         ID = IDCount++;
42         perm = NULL;
43 }
44
45 DatasetManager::~DatasetManager()
46 {
47         Clear();
48 }
49
50 void DatasetManager::Clear()
51 {
52     bProjected = false;
53         samples.clear();
54         obstacles.clear();
55         flags.clear();
56         labels.clear();
57         sequences.clear();
58         rewards.Clear();
59     categorical.clear();
60         KILL(perm);
61 }
62
63 void DatasetManager::AddSample(const fvec sample, const int label, const dsmFlags flag)
64 {
65         if (!sample.size()) return;
66     int dim = GetDimCount();
67         size = sample.size();
68     if(dim != size) // we need to go through all our data and adjust the dimensions
69     {
70         FOR(i, samples.size())
71         {
72             while(samples[i].size() < size) samples[i].push_back(0.f);
73         }
74     }
75         samples.push_back(sample);
76         labels.push_back(label);
77         flags.push_back(flag);
78         KILL(perm);
79         perm = randPerm(samples.size());
80 }
81
82 void DatasetManager::AddSamples(const std::vector< fvec > newSamples, const ivec newLabels, const std::vector<dsmFlags> newFlags)
83 {
84     if(!newSamples.size()) return;
85     int dim = GetDimCount();
86     size = newSamples[0].size();
87     if(dim != size) // we need to go through all our data and adjust the dimensions
88     {
89         FOR(i, samples.size())
90         {
91             while(samples[i].size() < size) samples[i].push_back(0.f);
92         }
93     }
94     FOR(i, newSamples.size())
95         {
96                 if(newSamples[i].size())
97                 {
98                         samples.push_back(newSamples[i]);
99                         if(i < newFlags.size()) flags.push_back(newFlags[i]);
100                         else flags.push_back(_UNUSED);
101                 }
102         }
103         if(newLabels.size() == newSamples.size()) FOR(i, newLabels.size()) labels.push_back(newLabels[i]);
104         else FOR(i, newSamples.size()) labels.push_back(0);
105         KILL(perm);
106         perm = randPerm(samples.size());
107 }
108
109 void DatasetManager::AddSamples(const DatasetManager &newSamples)
110 {
111         AddSamples(newSamples.GetSamples(), newSamples.GetLabels(), newSamples.GetFlags());
112 }
113
114 void DatasetManager::RemoveSample(const unsigned int index)
115 {
116         if(index >= samples.size()) return;
117         if(samples.size() == 1)
118         {
119                 Clear();
120                 return;
121         }
122         samples[index].clear();
123         for (unsigned int i = index; i < samples.size()-1; i++)
124         {
125                 samples[i] = samples[i+1];
126                 labels[i] = labels[i+1];
127                 flags[i] = flags[i+1];
128         }
129         samples.pop_back();
130         labels.pop_back();
131         flags.pop_back();
132
133         // we need to check if a sequence needs to be shortened
134         FOR(i, sequences.size())
135         {
136                 if(sequences[i].first > index) // later sequences
137                 {
138                         sequences[i].first--;
139                         sequences[i].second--;
140                 }
141                 else if(sequences[i].first == index || sequences[i].second >= index)
142                 {
143                         sequences[i].second--;
144                 }
145                 if(sequences[i].first >= sequences[i].second) // we need to pop out the sequence
146                 {
147                         if(sequences[i].first == sequences[i].second)
148                         {
149                                 flags[sequences[i].first] = _UNUSED;
150                         }
151                         for(int j=i; j<sequences.size()-1; j++)
152                         {
153                                 sequences[j] = sequences[j+1];
154                         }
155                         sequences.pop_back();
156                         i--;
157                 }
158         }
159 }
160
161 void DatasetManager::RemoveSamples(const ivec indices)
162 {
163     if(indices.size() > samples.size()) return;
164     // we sort the indices
165     sort(indices.begin(), indices.end(), less<int>());
166     int offset = 0;
167     FOR(i, indices.size())
168     {
169         int index = indices[i] - offset;
170         if(index < 0 || index > samples.size()) continue;
171         RemoveSample(index);
172         offset++;
173     }
174 }
175
176 void DatasetManager::AddSequence(const int start, const int stop)
177 {
178         if(start >= samples.size() || stop >= samples.size()) return;
179         for(int i=start; i<=stop; i++) flags[i] = _TRAJ;
180         sequences.push_back(ipair(start,stop));
181         // sort sequences by starting value
182         std::sort(sequences.begin(), sequences.end());
183 }
184
185 void DatasetManager::AddSequence(const ipair newSequence)
186 {
187         if(newSequence.first >= samples.size() || newSequence.second >= samples.size()) return;
188         for(int i=newSequence.first; i<=newSequence.second; i++) flags[i] = _TRAJ;
189         sequences.push_back(newSequence);
190         // sort sequences by starting value
191         std::sort(sequences.begin(), sequences.end());
192 }
193
194 void DatasetManager::AddSequences(const std::vector< ipair > newSequences)
195 {
196         sequences.reserve(sequences.size()+newSequences.size());
197         FOR(i, newSequences.size())
198         {
199                 sequences.push_back(newSequences[i]);           
200         }
201 }
202
203 void DatasetManager::RemoveSequence(const unsigned int index)
204 {
205         if(index >= sequences.size()) return;
206         for(int i=index; i<sequences.size()-1; i++) sequences[i] = sequences[i+1];
207         sequences.pop_back();
208 }
209
210 void DatasetManager::AddTimeSerie(const std::string name, const std::vector<fvec> data, const std::vector<long int>  timestamps)
211 {
212         TimeSerie serie;
213         serie.name = name;
214         serie.data = data;
215         serie.timestamps = timestamps;
216         AddTimeSerie(serie);
217 }
218
219 void DatasetManager::AddTimeSerie(const TimeSerie serie)
220 {
221         series.push_back(serie);
222 }
223
224 void DatasetManager::AddTimeSeries(const vector newTimeSeries)
225 {
226         series.insert(series.end(), newTimeSeries.begin(), newTimeSeries.end());
227 }
228
229 void DatasetManager::RemoveTimeSerie(const unsigned int index)
230 {
231         if(index >= series.size()) return;
232         series.erase(series.begin() + index);
233 }
234
235 void DatasetManager::AddObstacle(const fvec center, const fvec axes, const float angle, const fvec power, const fvec repulsion)
236 {
237         Obstacle o;
238         o.center = center;
239         o.axes = axes;
240         o.angle = angle;
241         o.power = power;
242         o.repulsion = repulsion;
243         obstacles.push_back(o);
244 }
245
246 void DatasetManager::AddObstacles(const std::vector<Obstacle> newObstacles)
247 {
248         FOR(i, newObstacles.size()) obstacles.push_back(newObstacles[i]);
249 }
250
251 void DatasetManager::RemoveObstacle(const unsigned int index)
252 {
253         if(index >= obstacles.size()) return;
254         for(int i=index; i<obstacles.size()-1; i++) obstacles[i] = obstacles[i+1];
255         obstacles.pop_back();
256 }
257
258 void DatasetManager::AddReward(const float *values, const ivec size, const fvec lowerBoundary, const fvec higherBoundary)
259 {
260         rewards.SetReward(values, size, lowerBoundary, higherBoundary);
261 }
262
263 // we compare the current sample with all the ones in the dataset
264 // and return the smallest distance
265 double DatasetManager::Compare(const fvec sample) const
266 {
267         if(!sample.size()) return 1.0;
268
269         // now compute the differences
270         double minDist = 1.0;
271         u32 index = 0;
272         FOR(i, samples.size())
273         {
274                 double dist = 0;
275                 FOR(j, size) dist += fabs(sample[j]-samples[i][j]);
276                 dist /= size;
277                 if(minDist > dist)
278                 {
279                         index = i;
280                         minDist = dist;
281                 }
282         }
283         return minDist;
284 }
285
286 void DatasetManager::Randomize(const int seed)
287 {
288         KILL(perm);
289         if(samples.size()) perm = randPerm(samples.size(), seed);
290 }
291
292 void DatasetManager::ResetFlags()
293 {
294         FOR(i, samples.size()) flags[i] = _UNUSED;
295 }
296
297 void DatasetManager::SetSample(const int index, const fvec sample)
298 {
299     if(index >= 0 && index < samples.size()) samples[index] = sample;
300 }
301
302 string DatasetManager::GetCategorical(const int dimension, const int value) const
303 {
304     string s;
305     if(categorical.count(dimension) && value < categorical[dimension].size())
306     {
307         s = categorical[dimension][value];
308     }
309     return s;
310 }
311
312 bool DatasetManager::IsCategorical(const int dimension) const
313 {
314     return categorical.count(dimension) > 0;
315 }
316
317 fvec DatasetManager::GetSampleDim(const int index, const ivec inputDims, const int outputDim) const
318 {
319     if(index >= samples.size()) return fvec();
320     if(!inputDims.size()) return samples[index];
321     if(outputDim == -1)
322     {
323         fvec sample(inputDims.size());
324         FOR(d, inputDims.size()) sample[d] = samples[index][inputDims[d]];
325         return sample;
326     }
327     else
328     {
329         int outputIndex = -1;
330         FOR(d, inputDims.size())
331         {
332             if(outputDim == inputDims[d])
333             {
334                 outputIndex = d;
335                 break;
336             }
337         }
338         fvec sample(inputDims.size() + (outputIndex==-1 ? 0 : 1));
339         FOR(d, inputDims.size())
340         {
341             if(d==outputIndex) sample.back() = samples[index][inputDims[d]];
342             else sample[d<outputIndex ? d : d-1] = samples[index][inputDims[d]];
343         }
344         if(outputIndex == -1) sample.back() = samples[index][outputDim];
345         return sample;
346     }
347
348     int dim = inputDims.size();
349     fvec sample(dim + outputDim!=-1?1:0);
350     FOR(d, dim) sample[d] = samples[index][inputDims[d]];
351     if(outputDim != -1) sample[dim] = samples[index][outputDim];
352     return sample;
353 }
354
355 std::vector< fvec > DatasetManager::GetSampleDims(const ivec inputDims, const int outputDim) const
356 {
357     if(!inputDims.size()) return samples;
358
359     vector<fvec> newSamples = samples;
360     int newDim = inputDims.size();
361     if(outputDim == -1)
362     {
363         FOR(i, samples.size())
364         {
365             fvec newSample(newDim);
366             FOR(d, inputDims.size())
367             {
368                 newSample[d] = samples[i][inputDims[d]];
369             }
370             newSamples[i] = newSample;
371         }
372     }
373     else
374     {
375         int outputIndex = -1;
376         FOR(d, inputDims.size())
377         {
378             if(outputDim == inputDims[d])
379             {
380                 newDim--;
381                 break;
382             }
383         }
384         FOR(i, samples.size())
385         {
386             fvec newSample(newDim);
387             if(outputIndex == -1)
388             {
389                 FOR(d, newDim-1)
390                 {
391                     newSample[d] = samples[i][inputDims[d]];
392                 }
393                 newSample[newDim-1] = samples[i][outputDim];
394             }
395             else
396             {
397                 FOR(d, newDim)
398                 {
399                     if(d == outputIndex) newSample[newDim-1] = samples[i][inputDims[d]];
400                     else newSample[d<outputIndex?d:d-1] = samples[i][inputDims[d]];
401                 }
402             }
403             newSamples[i] = newSample;
404         }
405     }
406     return newSamples;
407 }
408
409 std::vector< fvec > DatasetManager::GetSamples(const u32 count, const dsmFlags flag, const dsmFlags replaceWith) const
410 {
411         std::vector< fvec > selected;
412         if (!samples.size() || !perm) return selected;
413
414         if (!count)
415         {
416                 FOR(i, samples.size())
417                 {
418                         if ( flags[perm[i]] == flag)
419                         {
420                                 selected.push_back(samples[perm[i]]);
421                                 flags[perm[i]] = replaceWith;
422                         }
423                 }
424                 return selected;
425         }
426
427         for ( u32 i=0, cnt=0; i < samples.size() && cnt < count; i++ )
428         {
429                 if ( flags[perm[i]] == flag )
430                 {
431                         selected.push_back(samples[perm[i]]);
432                         flags[perm[i]] = replaceWith;
433                         cnt++;
434                 }
435         }
436
437         return selected;
438 }
439
440 std::vector< std::vector < fvec > > DatasetManager::GetTrajectories(const int resampleType, const int resampleCount, const int centerType, const float dT, const int zeroEnding) const
441 {
442
443         // we split the data into trajectories
444         vector< vector<fvec> > trajectories;
445         if(!sequences.size() || !samples.size()) return trajectories;
446         int dim = samples[0].size();
447         trajectories.resize(sequences.size());
448         FOR(i, sequences.size())
449         {
450                 int length = sequences[i].second-sequences[i].first+1;
451                 trajectories[i].resize(length);
452                 FOR(j, length)
453                 {
454                         trajectories[i][j].resize(dim*2);
455                         // copy data
456                         FOR(d, dim) trajectories[i][j][d] = samples[sequences[i].first + j][d];
457                 }
458         }
459
460         switch(resampleType)
461         {
462         case 0: // none
463         {
464                 FOR(i,sequences.size())
465                 {
466                         int cnt = sequences[i].second-sequences[i].first+1;
467                         if(resampleCount > cnt) resampleCount = cnt;
468                 }
469                 FOR(i, trajectories.size())
470                 {
471                         while(trajectories[i].size() > resampleCount) trajectories[i].pop_back();
472                 }
473         }
474                 break;
475         case 1: // uniform
476         {
477                 FOR(i, trajectories.size())
478                 {
479                         vector<fvec> trajectory = trajectories[i];
480                         trajectories[i] = interpolate(trajectory, resampleCount);
481                 }
482         }
483                 break;
484         case 2: // spline
485         {
486                 FOR(i, trajectories.size())
487                 {
488                         vector<fvec> trajectory = trajectories[i];
489                         trajectories[i] = interpolateSpline(trajectory, resampleCount);
490                 }
491         }
492                 break;
493         }
494
495
496         if(centerType)
497         {
498                 map<int,int> counts;
499                 map<int,fvec> centers;
500                 vector<int> trajLabels(sequences.size());
501                 FOR(i, sequences.size())
502                 {
503                         int index = centerType==1 ? sequences[i].second : sequences[i].first; // start
504                         int label = GetLabel(index);
505                         trajLabels[i] = label;
506                         if(!centers.count(label))
507                         {
508                                 fvec center(dim,0);
509                                 centers[label] = center;
510                                 counts[label] = 0;
511                         }
512                         centers[label] += samples[index];
513                         counts[label]++;
514                 }
515                 for(map<int,int>::iterator p = counts.begin(); p!=counts.end(); ++p)
516                 {
517                         int label = p->first;
518                         centers[label] /= p->second;
519                 }
520                 FOR(i, trajectories.size())
521                 {
522                         if(centerType == 1)
523                         {
524                                 fvec difference = centers[trajLabels[i]] - trajectories[i].back();
525                                 FOR(j, resampleCount) trajectories[i][j] += difference;
526                         }
527                         else
528                         {
529                                 fvec difference = centers[trajLabels[i]] - trajectories[i][0];
530                                 FOR(j, resampleCount) trajectories[i][j] += difference;
531                         }
532                 }
533         }
534
535         float maxV = -FLT_MAX;
536         // we compute the velocity
537         FOR(i, trajectories.size())
538         {
539                 FOR(j, resampleCount-1)
540                 {
541                         FOR(d, dim)
542                         {
543                                 float velocity = (trajectories[i][j+1][d] - trajectories[i][j][d]) / dT;
544                                 trajectories[i][j][dim + d] = velocity;
545                                 if(velocity > maxV) maxV = velocity;
546                         }
547                 }
548                 if(!zeroEnding)
549                 {
550                         FOR(d, dim)
551                         {
552                                 trajectories[i][resampleCount-1][dim + d] = trajectories[i][resampleCount-2][dim + d];
553                         }
554                 }
555         }
556
557         // we normalize the velocities as the variance of the data
558         fvec mean, sigma;
559         mean.resize(dim,0);
560         int cnt = 0;
561         sigma.resize(dim,0);
562         FOR(i, trajectories.size())
563         {
564                 FOR(j, resampleCount)
565                 {
566                         mean += trajectories[i][j];
567                         cnt++;
568                 }
569         }
570         mean /= cnt;
571         FOR(i, trajectories.size())
572         {
573                 FOR(j, resampleCount)
574                 {
575                         fvec diff = (mean - trajectories[i][j]);
576                         FOR(d,dim) sigma[d] += diff[d]*diff[d];
577                 }
578         }
579         sigma /= cnt;
580
581         FOR(i, trajectories.size())
582         {
583                 FOR(j, resampleCount)
584                 {
585                         FOR(d, dim)
586                         {
587                                 trajectories[i][j][dim + d] /= maxV;
588                                 //trajectories[i][j][dim + d] /= sqrt(sigma[d]);
589                         }
590                 }
591         }
592         return trajectories;
593 }
594
595
596 void DatasetManager::Save(const char *filename) const
597 {
598     if(!samples.size() && rewards.Empty()) return;
599         u32 sampleCnt = samples.size();
600     if(sampleCnt) size = samples[0].size();
601
602         ofstream file(filename);
603         if(!file.is_open()) return;
604
605         file << sampleCnt << " " << size << "\n";
606         FOR(i, sampleCnt)
607         {
608                 FOR(j,size)
609                 {
610                         file << samples[i][j] << " ";
611                 }
612                 file << labels[i] << " ";
613                 file << flags[i] << " ";
614                 file << "\n";
615         }
616
617         if(sequences.size())
618         {
619                 file << "s " << sequences.size() << "\n";
620                 FOR(i, sequences.size())
621                 {
622                         file << sequences[i].first << " " << sequences[i].second << "\n";
623                 }
624         }
625
626         // we load the obstacles
627     if(obstacles.size())
628         {
629                 file << "o " << obstacles.size() << "\n";
630                 FOR(i, obstacles.size())
631                 {
632                         FOR(j, size) file << obstacles[i].center[j] << " ";
633                         FOR(j, size) file << obstacles[i].axes[j] << " ";
634                         file << obstacles[i].angle << " ";
635                         file << obstacles[i].power[0] << " ";
636                         file << obstacles[i].power[1] << " ";
637                         file << obstacles[i].repulsion[0] << " ";
638                         file << obstacles[i].repulsion[1] << "\n";
639                 }
640         }
641     if(!rewards.Empty())
642     {
643         file << "r " << rewards.dim << " " << rewards.length << "\n";
644         FOR(i, rewards.dim)
645         {
646             file << rewards.size[i] << " " << rewards.lowerBoundary[i] << " " << rewards.higherBoundary[i] << "\n";
647         }
648         FOR(i, rewards.length)
649         {
650             file << rewards.rewards[i] << " ";
651         }
652     }
653
654         file.close();
655 }
656
657 bool DatasetManager::Load(const char *filename)
658 {
659         ifstream file(filename);
660         if(!file.is_open()) return false;
661         Clear();
662
663         int sampleCnt;
664         file >> sampleCnt;
665         file >> size;
666
667         // we load the samples
668         FOR(i, sampleCnt)
669         {
670                 fvec sample;
671                 sample.resize(size,0);
672                 int label, flag;
673                 FOR(j, size)
674                 {
675                         file >> sample[j];
676                 }
677                 file >> label;
678                 file >> flag;
679                 samples.push_back(sample);
680                 labels.push_back(label);
681                 flags.push_back((dsmFlags)flag);
682         }
683
684         // we load the sequences
685         char tmp[255];
686         file.getline(tmp,255); // we skip the rest of the line
687         int nextChar = file.peek();
688         if(nextChar == 's') // we have sequences!
689         {
690                 char dump;
691                 file >> dump;
692                 int sequenceCount;
693                 file >> sequenceCount;
694                 FOR(i, sequenceCount)
695                 {
696                         int start, stop;
697                         file >> start;
698                         file >> stop;
699                         sequences.push_back(ipair(start,stop));
700                 }
701                 file.getline(tmp,255);
702                 nextChar = file.peek();
703         }
704         // we load the obstacles
705         if(nextChar == 'o')
706         {
707                 char dump;
708                 file >> dump;
709                 int obstacleCount;
710                 file >> obstacleCount;
711                 Obstacle obstacle;
712                 obstacle.center.resize(size);
713                 obstacle.axes.resize(size);
714                 obstacle.power.resize(size);
715                 obstacle.repulsion.resize(size);
716                 FOR(i, obstacleCount)
717                 {
718                         FOR(j, size) file >> obstacle.center[j];
719                         FOR(j, size) file >> obstacle.axes[j];
720                         file >> obstacle.angle;
721                         FOR(j, size) file >> obstacle.power[j];
722                         FOR(j, size) file >> obstacle.repulsion[j];
723                         obstacles.push_back(obstacle);
724                 }
725         }
726     // we load the reward
727     if(nextChar == 'r')
728     {
729         char dump;
730         file >> dump;
731         int dims, length;
732         file >> dims >> length;
733         ivec size(dims);
734         fvec lowerBoundary(dims), higherBoundary(dims);
735         int testLength = 1;
736         FOR(i, dims)
737         {
738             file >> size[i] >> lowerBoundary[i] >> higherBoundary[i];
739             testLength *= size[i];
740         }
741         if(testLength == length)
742         {
743             double *rewardData = new double[length];
744             FOR(i, length)
745             {
746                 double value;
747                 file >> value;
748                 rewardData[i] = value;
749             }
750             rewards.lowerBoundary = lowerBoundary;
751             rewards.higherBoundary = higherBoundary;
752             rewards.size = size;
753             rewards.dim = dims;
754             rewards.length = length;
755             if(rewards.rewards) delete [] rewards.rewards;
756             rewards.rewards = rewardData;
757         }
758     }
759
760         file.close();
761         KILL(perm);
762         perm = randPerm(samples.size());
763         return samples.size() > 0;
764 }
765
766 int DatasetManager::GetDimCount() const
767 {
768         int dim = 2;
769         if(samples.size()) dim = samples[0].size();
770         if(series.size() && series[0].size())
771         {
772                 dim = series[0][0].size()+1;
773         }
774         return dim;
775 }
776
777 std::pair<fvec, fvec> DatasetManager::GetBounds() const
778 {
779     if(!samples.size()) return make_pair(fvec(),fvec());
780     int dim = samples[0].size();
781     fvec mins(dim,FLT_MAX), maxes(dim,-FLT_MAX);
782     FOR(i, samples.size())
783     {
784         fvec& sample = samples[i];
785         int dim = sample.size();
786         FOR(d,dim)
787         {
788             if(mins[d] > sample[d]) mins[d] = sample[d];
789             if(maxes[d] < sample[d]) maxes[d] = sample[d];
790         }
791     }
792     return make_pair(mins, maxes);
793 }
794
795 u32 DatasetManager::GetClassCount(const ivec classes)
796 {
797     int cnt=0;
798     map<int,int> classMap;
799     FOR(i, classes.size()) if(!classMap.count(classes[i])) classMap[classes[i]] = cnt++;
800     return classMap.size();
801 }
802
803 bvec DatasetManager::GetFreeFlags() const
804 {
805         bvec res;
806         FOR(i, flags.size()) res.push_back(flags[i] == _UNUSED);
807         return res;
808 }
809
810
811 /******************************************/
812 /*                                        */
813 /*    REWARD MAPS                         */
814 /*                                        */
815 /******************************************/
816 RewardMap& RewardMap::operator= (const RewardMap& r)
817 {
818   if (this != &r) {
819           dim = r.dim;
820           size = r.size;
821           lowerBoundary = r.lowerBoundary;
822           higherBoundary = r.higherBoundary;
823       if(length != r.length)
824       {
825           length = r.length;
826           if(rewards) delete [] rewards;
827           rewards = new double[length];
828       }
829       memcpy(rewards, r.rewards, length*sizeof(double));
830   }
831   return *this;
832 }
833
834 void RewardMap::SetReward(const double *rewards, const ivec size, const fvec lowerBoundary, const fvec higherBoundary)
835 {
836         this->lowerBoundary = lowerBoundary;
837         this->higherBoundary = higherBoundary;
838         this->size = size;
839         dim = size.size();
840         length = 1;
841         FOR(i, size.size()) length *= size[i];
842         if(this->rewards) delete [] this->rewards;
843     this->rewards = new double[length];
844     memcpy(this->rewards, rewards, length*sizeof(double));
845 }
846
847 void RewardMap::SetReward(const float *rewards, const ivec size, const fvec lowerBoundary, const fvec higherBoundary)
848 {
849     this->lowerBoundary = lowerBoundary;
850     this->higherBoundary = higherBoundary;
851     this->size = size;
852     dim = size.size();
853     length = 1;
854     FOR(i, size.size()) length *= size[i];
855     if(this->rewards) delete [] this->rewards;
856     this->rewards = new double[length];
857     FOR(i, length)
858     {
859         this->rewards[i] = (double)rewards[i];
860     }
861 }
862
863 float *RewardMap::GetRewardFloat() const
864 {
865     if(!length) return 0;
866     float *rewards = new float[length];
867     FOR(i, length) rewards[i] = this->rewards[i];
868     return rewards;
869 }
870
871 void RewardMap::Clear()
872 {
873         dim = 0;
874         size.clear();
875         length = 0;
876         lowerBoundary.clear();
877         higherBoundary.clear();
878     KILL(rewards);
879 }
880
881 void RewardMap::Zero()
882 {
883         FOR(i, length) rewards[i] = 0;
884 }
885
886 // return the value of the reward function at the coordinates provided
887 float RewardMap::ValueAt(fvec sample) const
888 {
889         if(!rewards) return 0.f;
890         ivec index;
891         index.resize(dim);
892         FOR(d, dim)
893         {
894                 //we check if we're outside the boundaries
895                 if(sample[d] < lowerBoundary[d]) sample[d] = lowerBoundary[d];
896                 if(sample[d] > higherBoundary[d]) sample[d] = higherBoundary[d];
897                 // now we get the closest index on the map
898                 index[d] = (int)((sample[d] - lowerBoundary[d]) / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
899         }
900
901         // we convert the map index to a vector index
902         int rewardIndex = 0;
903         FOR(d,dim)
904         {
905                 rewardIndex = rewardIndex*size[dim-d-1]+index[dim-d-1];
906         }
907
908         //printf("sample: %f %f index: %d %d (%d) value: %f\n", sample[0], sample[1], index[0], index[1], rewardIndex, rewards[rewardIndex]);
909         // TODO: return interpolation of closest indices instead of the closest index itself
910         return rewards[rewardIndex];
911 }
912
913 void RewardMap::SetValueAt(const fvec sample, const double value)
914 {
915         if(!rewards) return;
916         ivec index;
917         index.resize(dim);
918         FOR(d, dim)
919         {
920                 //we check if we're outside the boundaries
921                 if(sample[d] < lowerBoundary[d]) return;
922                 if(sample[d] > higherBoundary[d]) return;
923                 // now we get the closest index on the map
924                 index[d] = (int)((sample[d] - lowerBoundary[d]) / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
925         }
926
927         // we convert the map index to a vector index
928         int rewardIndex = 0;
929         FOR(d,dim)
930         {
931                 rewardIndex = rewardIndex*size[dim-d-1]+index[dim-d-1];
932         }
933         rewards[rewardIndex] = value;
934 }
935
936 void RewardMap::ShiftValueAt(const fvec sample, const double shift)
937 {
938         if(!rewards) return;
939         ivec index;
940         index.resize(dim);
941         FOR(d, dim)
942         {
943                 //we check if we're outside the boundaries
944                 if(sample[d] < lowerBoundary[d]) return;
945                 if(sample[d] > higherBoundary[d]) return;
946                 // now we get the closest index on the map
947                 index[d] = (int)((sample[d] - lowerBoundary[d]) / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
948         }
949         // we convert the map index to a vector index
950         int rewardIndex = 0;
951         FOR(d,dim)
952         {
953                 rewardIndex = rewardIndex*size[dim-d-1]+index[dim-d-1];
954         }
955         printf("index: %d value: %f\n", rewardIndex, rewards[rewardIndex]);
956         rewards[rewardIndex] += shift;
957 }
958
959 void RewardMap::ShiftValueAt(const fvec sample, const double radius, const double shift)
960 {
961         if(!rewards) return;
962         ivec index;
963         index.resize(dim);
964         ivec lowIndex = index, hiIndex = index;
965         ivec steps; steps.resize(dim);
966         FOR(d, dim)
967         {
968                 //we check if we're outside the boundaries
969                 if(sample[d] < lowerBoundary[d]) return;
970                 if(sample[d] > higherBoundary[d]) return;
971                 // now we get the closest index on the map
972                 steps[d] = (int)(2*radius / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
973                 index[d] = (int)((sample[d] - lowerBoundary[d]) / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
974                 lowIndex[d] = (int)((sample[d] - radius - lowerBoundary[d]) / (higherBoundary[d] - lowerBoundary[d]) * size[d]);
975         }
976         FOR(i, steps[1])
977         {
978                 FOR(j, steps[0])
979                 {
980                         float x = 2.f*(j - steps[0]*0.5f)/float(steps[0]);
981                         float y = 2.f*(i - steps[1]*0.5f)/float(steps[0]);
982                         if(x*x + y*y > 1) continue;
983                         // we convert the map index to a vector index
984                         int rewardIndex = index[0] - steps[0]/2 + j + (index[1] - steps[1]/2 + i)*size[0];
985                         if(rewardIndex < 0 || rewardIndex>=length) return;
986                         rewards[rewardIndex] += shift;
987                 }
988         }
989 }