Klamp't  0.9.0
Common_Internal.h
1 #ifndef KLAMPT_SENSING_COMMON_H
2 #define KLAMPT_SENSING_COMMON_H
3 
4 #include <KrisLibrary/File.h>
5 #include <KrisLibrary/Logger.h>
6 #include <KrisLibrary/math/random.h>
7 #include <KrisLibrary/math/math.h>
8 #include <KrisLibrary/math3d/primitives.h>
9 #include <sstream>
10 
11 namespace Klampt {
12 
13 using namespace Math;
14 using namespace Math3D;
15 
16 #ifdef WIN32
17 static inline double round(double val) { return floor(val + 0.5); }
18 #endif //WIN32
19 
20 //emulates a process that discretizes a continuous value into a digital one
21 //with resolution resolution, and variance variance
22 inline Real Discretize(Real value,Real resolution,Real variance)
23 {
24  if(variance>0)
25  value += RandGaussian()*Sqrt(variance);
26  if(resolution>0)
27  value = round(value/resolution)*resolution;
28  return value;
29 }
30 
31 //a faster version of Discretize
32 inline Real Discretize2(Real value,Real resolution,Real invresolution,Real stdev)
33 {
34  if(stdev>0)
35  value += RandGaussian()*stdev;
36  if(resolution>0)
37  value = round(value*invresolution)*resolution;
38  return value;
39 }
40 
41 inline Vector3 Discretize(const Vector3& value,const Vector3& resolution,const Vector3& variance)
42 {
43  Vector3 res;
44  res.x = Discretize(value.x,resolution.x,variance.x);
45  res.y = Discretize(value.y,resolution.y,variance.y);
46  res.z = Discretize(value.z,resolution.z,variance.z);
47  return res;
48 }
49 
50 } //namespace Klampt
51 
52 //The following are overrides for ReadFile/WriteFile
53 
54 inline bool WriteFile(File& f,const std::string& s)
55 {
56  size_t n=s.length();
57  if(!WriteFile(f,n)) return false;
58  if(n > 0)
59  if(!WriteArrayFile(f,&s[0],s.length())) return false;
60  return true;
61 }
62 
63 inline bool ReadFile(File& f,std::string& s)
64 {
65  size_t n;
66  if(!ReadFile(f,n)) return false;
67  s.resize(n);
68  if(n > 0)
69  if(!ReadArrayFile(f,&s[0],n)) return false;
70  return true;
71 }
72 
73 template <class T>
74 inline bool WriteFile(File& f,const std::vector<T>& v)
75 {
76  if(!WriteFile(f,(int)v.size())) return false;
77  if(!v.empty())
78  if(!WriteArrayFile(f,&v[0],v.size())) return false;
79  return true;
80 }
81 
82 template <class T>
83 inline bool ReadFile(File& f,std::vector<T>& v)
84 {
85  int n;
86  if(!ReadFile(f,n)) return false;
87  v.resize(0);
88  if(n >= 0) {
89  v.resize(n);
90  if(!ReadArrayFile(f,&v[0],n)) return false;
91  return true;
92  }
93  LOG4CXX_WARN(KrisLibrary::logger(),"ReadFile(vector): Invalid size "<<n);
94  return false;
95 }
96 
97 inline bool WriteFile(File& f,const File& fbuf)
98 {
99  const unsigned char* buf = fbuf.GetDataBuffer();
100  if(!buf) {
101  LOG4CXX_WARN(KrisLibrary::logger(),"ReadFile(File): file is not a buffer");
102  return false;
103  }
104  int n=fbuf.Length();
105  if(!WriteFile(f,n)) return false;
106  if(n > 0)
107  if(!WriteArrayFile(f,buf,n)) return false;
108  return true;
109 }
110 
111 inline bool ReadFile(File& f,File& fbuf)
112 {
113  if(!fbuf.OpenData()) {
114  LOG4CXX_WARN(KrisLibrary::logger(),"ReadFile(File): unable to open file as buffer");
115  }
116  int n;
117  if(!ReadFile(f,n)) return false;
118  if(n > 0) {
119  unsigned char* data = new unsigned char[n];
120  if(!ReadArrayFile(f,data,n)) {
121  delete [] data;
122  return false;
123  }
124  if(!fbuf.WriteData(data,n)) {
125  LOG4CXX_WARN(KrisLibrary::logger(),"ReadFile(File): unable to write data to buffer?");
126  return false;
127  }
128  delete [] data;
129  fbuf.Seek(0,FILESEEKSTART);
130  return true;
131  }
132  return true;
133 }
134 
135 #endif
void Discretize(const LinearPath &in, Real res, vector< Real > &times, vector< Config > &milestones)
Split up the path into keyframes at a given resolution.
Definition: ContactDistance.h:6