-
Notifications
You must be signed in to change notification settings - Fork 1
/
Util.cpp
70 lines (68 loc) · 1.89 KB
/
Util.cpp
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
#include "Util.h"
bool comparator(const intDoublePair &l, const intDoublePair &r) {
return l.second < r.second;
}
void rotate2D(double &vx, double &vy, double theta) {
double cosa = cos(theta), sina = sin(theta);
// rotate goal heading by angle to get this heading
double vxtemp = vx * cosa + vy * -sina;
double vytemp = vx * sina + vy * cosa;
vx = vxtemp;
vy = vytemp;
}
std::vector<intDoublePair> sortWithIndexReturn(std::vector<double> toSort) {
std::vector<intDoublePair> sorted(toSort.size());
for (int i = 0; i < toSort.size(); i++) {
sorted[i] = intDoublePair(i, toSort[i]);
}
std::sort(sorted.begin(), sorted.end(), comparator);
return sorted;
}
std::vector<intDoublePair> sortWithIndexReturn(std::list<double> toSort) {
std::vector<intDoublePair> sorted(toSort.size());
int ix = 0;
for (std::list<double>::iterator iter = toSort.begin(); iter != toSort.end();
++iter) {
sorted[ix] = intDoublePair(ix, (*iter));
ix++;
}
std::sort(sorted.begin(), sorted.end(), comparator);
return sorted;
}
void softmax(Eigen::VectorXd &x) {
double esum = 0;
for (int i = 0; i < x.size(); i++) {
x[i] = std::exp(x[i]);
esum += x[i];
}
for (int i = 0; i < x.size(); i++) {
x[i] = x[i] / esum;
}
}
int argmin(std::vector<double> &x) {
int amin = -1;
double min = 9e99;
for (int i = 0; i < x.size(); i++) {
if (x[i] < min) {
amin = i;
min = x[i];
}
}
return amin;
}
void normalize(double &vx, double &vy) {
double norm = std::sqrt(vx * vx + vy * vy);
vy = vy / norm;
vx = vx / norm;
}
std::vector<std::string> split_string(std::string s, char delim) {
std::istringstream ss(s);
std::vector<std::string> parts;
std::string part;
while (std::getline(ss, part, delim)) {
if (!part.empty()) {
parts.push_back(part);
}
}
return parts;
}