1 #ifndef KLAMPT_SENSING_COMMON_H 2 #define KLAMPT_SENSING_COMMON_H 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> 17 static inline double round(
double val) {
return floor(val + 0.5); }
22 inline Real
Discretize(Real value,Real resolution,Real variance)
25 value += RandGaussian()*Sqrt(variance);
27 value = round(value/resolution)*resolution;
32 inline Real Discretize2(Real value,Real resolution,Real invresolution,Real stdev)
35 value += RandGaussian()*stdev;
37 value = round(value*invresolution)*resolution;
41 inline Vector3
Discretize(
const Vector3& value,
const Vector3& resolution,
const Vector3& variance)
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);
54 inline bool WriteFile(File& f,
const std::string& s)
57 if(!WriteFile(f,n))
return false;
59 if(!WriteArrayFile(f,&s[0],s.length()))
return false;
63 inline bool ReadFile(File& f,std::string& s)
66 if(!ReadFile(f,n))
return false;
69 if(!ReadArrayFile(f,&s[0],n))
return false;
74 inline bool WriteFile(File& f,
const std::vector<T>& v)
76 if(!WriteFile(f,(
int)v.size()))
return false;
78 if(!WriteArrayFile(f,&v[0],v.size()))
return false;
83 inline bool ReadFile(File& f,std::vector<T>& v)
86 if(!ReadFile(f,n))
return false;
90 if(!ReadArrayFile(f,&v[0],n))
return false;
93 LOG4CXX_WARN(KrisLibrary::logger(),
"ReadFile(vector): Invalid size "<<n);
97 inline bool WriteFile(File& f,
const File& fbuf)
99 const unsigned char* buf = fbuf.GetDataBuffer();
101 LOG4CXX_WARN(KrisLibrary::logger(),
"ReadFile(File): file is not a buffer");
105 if(!WriteFile(f,n))
return false;
107 if(!WriteArrayFile(f,buf,n))
return false;
111 inline bool ReadFile(File& f,File& fbuf)
113 if(!fbuf.OpenData()) {
114 LOG4CXX_WARN(KrisLibrary::logger(),
"ReadFile(File): unable to open file as buffer");
117 if(!ReadFile(f,n))
return false;
119 unsigned char* data =
new unsigned char[n];
120 if(!ReadArrayFile(f,data,n)) {
124 if(!fbuf.WriteData(data,n)) {
125 LOG4CXX_WARN(KrisLibrary::logger(),
"ReadFile(File): unable to write data to buffer?");
129 fbuf.Seek(0,FILESEEKSTART);
void Discretize(const LinearPath &in, Real res, vector< Real > ×, vector< Config > &milestones)
Split up the path into keyframes at a given resolution.
Definition: ContactDistance.h:6