-
Notifications
You must be signed in to change notification settings - Fork 0
/
evalFourierFuncs_3P.h
194 lines (134 loc) · 6.52 KB
/
evalFourierFuncs_3P.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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
#include <math.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/KroneckerProduct>
#include <vector>
#include "evalBezierFuncs_3P.h"
using namespace std;
using namespace Eigen;
// arange helper function
vector<double> arange(int start, int stop, int step = 1)
{
vector<double> values;
for (int value = start; value < stop; value += step)
{
values.push_back(value);
}
return values;
}
// the most basic fourier functions
MatrixXd returnFourier(double t, int N)
{
vector<double> ws = arange(1, N+1);
MatrixXd wsX = Map<MatrixXd>(ws.data(), 1, N);
MatrixXd wsXcos = (wsX * 2*M_PI * t).array().cos();
MatrixXd wsXsin = (wsX * 2*M_PI * t).array().sin();
MatrixXd wsXtrig(wsXcos.rows(), wsXcos.cols()+wsXsin.cols());
wsXtrig << wsXcos, wsXsin;
Matrix<double, 1, 1> constant {1};
MatrixXd fourierEvals(constant.rows(), constant.cols()+wsXtrig.cols());
fourierEvals << constant, wsXtrig;
return fourierEvals;
}
MatrixXd returnFourierDeriv(double t, int N)
{
vector<double> ws = arange(1, N+1);
MatrixXd wsX = Map<MatrixXd>(ws.data(), 1, N);
MatrixXd temp = -2*M_PI*(wsX * 2*M_PI * t).array().sin();
MatrixXd wsXd1 = wsX.cwiseProduct(temp);
temp = 2*M_PI*(wsX * 2*M_PI * t).array().cos();
MatrixXd wsXd2 = wsX.cwiseProduct(temp);
MatrixXd wsXtrig(wsXd1.rows(), wsXd1.cols()+wsXd2.cols());
wsXtrig << wsXd1, wsXd2;
Matrix<double, 1, 1> constant {0};
MatrixXd fourierEvals(constant.rows(), constant.cols()+wsXtrig.cols());
fourierEvals << constant, wsXtrig;
return fourierEvals;
}
MatrixXd returnFourier2ndDeriv(double t, int N)
{
vector<double> ws = arange(1, N+1);
MatrixXd wsX = Map<MatrixXd>(ws.data(), 1, N);
MatrixXd temp = -2*M_PI*2*M_PI*(wsX * 2*M_PI * t).array().cos();
MatrixXd wsXdd1 = wsX.cwiseProduct(wsX.cwiseProduct(temp));
temp = -2*M_PI*2*M_PI*(wsX * 2*M_PI * t).array().sin();
MatrixXd wsXdd2 = wsX.cwiseProduct(wsX.cwiseProduct(temp));
MatrixXd wsXtrig(wsXdd1.rows(), wsXdd1.cols()+wsXdd2.cols());
wsXtrig << wsXdd1, wsXdd2;
Matrix<double, 1, 1> constant {0};
MatrixXd fourierEvals(constant.rows(), constant.cols()+wsXtrig.cols());
fourierEvals << constant, wsXtrig;
return fourierEvals;
}
// returns the evaluation of the three basis functions
MatrixXd _returnKroneckerBasisEvald(Matrix<double, 1, Dynamic> inclineFuncs, Matrix<double, 1, Dynamic> stepLengthFuncs,
Matrix<double, 1, Dynamic> phaseFuncs)
{
MatrixXd basisEvald = kroneckerProduct(inclineFuncs, kroneckerProduct(stepLengthFuncs, phaseFuncs));
return basisEvald;
}
// evaluation functions
MatrixXd returnFourierBasis_Eval(double phase, double stepLength, double incline, int phase_order=50, int stride_length_order=1, int incline_order=1)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezier(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezier(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourier(phase, phase_order);
MatrixXd fourierCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourierCoeffs3P;
}
// derivatives
MatrixXd returnFourierBasis_DerivEval_dphase(double phase, double stepLength, double incline, int phase_order, int stride_length_order,
int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezier(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezier(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourierDeriv(phase, phase_order);
MatrixXd fourierDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourierDerivCoeffs3P;
}
MatrixXd returnFourierBasis_DerivEval_dsL(double phase, double stepLength, double incline, int phase_order, int stride_length_order,
int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezier(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezierDeriv(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourier(phase, phase_order);
MatrixXd fourierDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourierDerivCoeffs3P;
}
MatrixXd returnFourierBasis_DerivEval_dincline(double phase, double stepLength, double incline, int phase_order,
int stride_length_order, int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezierDeriv(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezier(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourier(phase, phase_order);
MatrixXd fourierDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourierDerivCoeffs3P;
}
// second derivatives
MatrixXd returnFourierBasis_2ndDerivEval_dphase2(double phase, double stepLength, double incline, int phase_order,
int stride_length_order, int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezier(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezier(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourier2ndDeriv(phase, phase_order);
MatrixXd fourier2ndDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourier2ndDerivCoeffs3P ;
}
MatrixXd returnFourierBasis_2ndDerivEval_dphasedsL(double phase, double stepLength, double incline, int phase_order,
int stride_length_order, int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezier(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezierDeriv(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourierDeriv(phase, phase_order);
MatrixXd fourier2ndDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourier2ndDerivCoeffs3P ;
}
MatrixXd returnFourierBasis_2ndDerivEval_dphasedincline(double phase, double stepLength, double incline, int phase_order,
int stride_length_order, int incline_order)
{
Matrix<double, 1, Dynamic> inclineFuncs = selectOrderBezierDeriv(incline_order, incline);
Matrix<double, 1, Dynamic> stepLengthFuncs = selectOrderBezier(stride_length_order, stepLength);
Matrix<double, 1, Dynamic> phaseFuncs = returnFourierDeriv(phase, phase_order);
MatrixXd fourier2ndDerivCoeffs3P = _returnKroneckerBasisEvald(inclineFuncs, stepLengthFuncs, phaseFuncs);
return fourier2ndDerivCoeffs3P;
}