forked from arpg/CarPlanner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
CVarHelpers.h
125 lines (103 loc) · 3.29 KB
/
CVarHelpers.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#ifndef _CVAR_HELPERS_
#define _CVAR_HELPERS_
#include <cvars/CVarVectorIO.h>
#include <Eigen/Core>
//#include "UiCommon.h"
//namespace Eigen
//{
// typedef Matrix<double,5,1> Vector5d;
//}
////////////////////////////////////////////////////////////////////////////
// Overloading Eigen for CVars
namespace CVarUtils
{
inline std::ostream& operator<<( std::ostream& Stream, Eigen::MatrixXd& Mat )
{
int nRows = Mat.rows();
int nCols = Mat.cols();
if(nRows && nCols){
Stream << "[ ";
for( int ii = 0; ii < nRows-1; ii++ ) {
for( int jj = 0; jj < nCols-1; jj++ ) {
Stream << Mat(ii, jj);
Stream << ", ";
}
Stream << Mat(ii, nCols-1);
Stream << "; ";
}
for( int jj = 0; jj < nCols-1; jj++ ) {
Stream << Mat(nRows-1, jj);
Stream << ", ";
}
Stream << Mat(nRows-1, nCols-1);
Stream << " ]";
}
return Stream;
}
////////////////////////////////////////////////////////////////////////////
inline std::istream& operator>>( std::istream& Stream, Eigen::MatrixXd& Mat )
{
int nRows = Mat.rows();
int nCols = Mat.cols();
char str[256];
if(nRows && nCols){
Stream.getline(str, 255, '[');
if( Stream.gcount() > 1 ) {
return Stream;
}
for( int ii = 0; ii < nRows-1; ii++ ) {
for( int jj = 0; jj < nCols-1; jj++ ) {
Stream.getline(str, 255, ',');
Mat(ii, jj) = std::strtod(str, NULL);
}
Stream.getline(str, 255, ';');
Mat(ii, nCols-1) = std::strtod(str, NULL);
}
for( int jj = 0; jj < nCols-1; jj++ ) {
Stream.getline(str, 255, ',');
Mat(nRows-1, jj) = std::strtod(str, NULL);
}
Stream.getline(str, 255, ']');
Mat(nRows-1, nCols-1) = std::strtod(str, NULL);
}
return Stream;
}
inline std::ostream& operator<<(std::ostream& Stream, std::vector<Eigen::MatrixXd>& vEigen ) {
if( vEigen.size() == 0 ) {
Stream << "[ ]";
return Stream;
}
Stream << "[ " << vEigen[0];
for( size_t i=1; i<vEigen.size(); i++ ) {
Stream << " " << vEigen[i];
}
Stream << " ]";
return Stream;
}
inline std::istream& operator>>(std::istream& Stream, std::vector<Eigen::MatrixXd>& vEigen ) {
std::stringstream sBuf;
Eigen::MatrixXd Mat;
vEigen.clear();
char str[256];
// strip first bracket
Stream.getline(str, 255, '[');
if( Stream.gcount() > 1 ) {
return Stream;
}
// get first element
Stream.getline(str, 255, '[');
Stream.getline(str, 255, ']');
while( Stream.gcount() > 1 ) {
sBuf << "[";
sBuf << str;
sBuf << "]";
sBuf >> Mat;
vEigen.push_back(Mat);
// get next element
Stream.getline(str, 255, '[');
Stream.getline(str, 255, ']');
}
return Stream;
}
}
#endif