Klamp't  0.8.1
Common_Internal.h
1 #ifndef KLAMPT_SENSING_COMMON_H
2 #define KLAMPT_SENSING_COMMON_H
3 
4 #include <KrisLibrary/File.h>
5 #include <KrisLibrary/math/random.h>
6 #include <KrisLibrary/math/math.h>
7 #include <KrisLibrary/math3d/primitives.h>
8 #include <sstream>
9 
10 using namespace Math;
11 using namespace Math3D;
12 
13 #ifdef WIN32
14 static inline double round(double val) { return floor(val + 0.5); }
15 #endif //WIN32
16 
17 //emulates a process that discretizes a continuous value into a digital one
18 //with resolution resolution, and variance variance
19 inline Real Discretize(Real value,Real resolution,Real variance)
20 {
21  if(variance>0)
22  value += RandGaussian()*Sqrt(variance);
23  if(resolution>0)
24  value = round(value/resolution)*resolution;
25  return value;
26 }
27 
28 inline Vector3 Discretize(const Vector3& value,const Vector3& resolution,const Vector3& variance)
29 {
30  Vector3 res;
31  res.x = Discretize(value.x,resolution.x,variance.x);
32  res.y = Discretize(value.y,resolution.y,variance.y);
33  res.z = Discretize(value.z,resolution.z,variance.z);
34  return res;
35 }
36 
37 
38 inline bool WriteFile(File& f,const string& s)
39 {
40  size_t n=s.length();
41  if(!WriteFile(f,n)) return false;
42  if(n > 0)
43  if(!WriteArrayFile(f,&s[0],s.length())) return false;
44  return true;
45 }
46 
47 inline bool ReadFile(File& f,string& s)
48 {
49  size_t n;
50  if(!ReadFile(f,n)) return false;
51  s.resize(n);
52  if(n > 0)
53  if(!ReadArrayFile(f,&s[0],n)) return false;
54  return true;
55 }
56 
57 template <class T>
58 inline bool WriteFile(File& f,const vector<T>& v)
59 {
60  if(!WriteFile(f,(int)v.size())) return false;
61  if(!v.empty())
62  if(!WriteArrayFile(f,&v[0],v.size())) return false;
63  return true;
64 }
65 
66 template <class T>
67 inline bool ReadFile(File& f,vector<T>& v)
68 {
69  int n;
70  if(!ReadFile(f,n)) return false;
71  v.resize(0);
72  if(n > 0) {
73  v.resize(n);
74  if(!ReadArrayFile(f,&v[0],n)) return false;
75  return true;
76  }
77  return false;
78 }
79 
80 #endif
void Discretize(const LinearPath &in, Real res, vector< Real > &times, vector< Config > &milestones)
Split up the path into keyframes at a given resolution.