test_param_func_manip_spline0_dist.cpp
Go to the documentation of this file.
1 
2 #include "gtest/gtest.h"
3 #include <array>
4 #include <boost/concept_check.hpp>
5 #include <tuw_control/param_func_new/param_func_spline/param_func_spline0_dist.hpp>
6 
7 using namespace tuw;
8 using namespace std;
9 
10 namespace
11 {
12 using ParamFuncsExtType = ParamFuncsSpline0Dist<double, -1, -1>;
13 using ParamFuncsType = ParamFuncsBaseVirt<double>;
14 using ParamFuncsSPtr = std::shared_ptr<ParamFuncsBaseVirt<double>>;
15 using ParamFuncsExtTypePtr = std::shared_ptr<ParamFuncsExtType>;
16 
17 // The fixture for testing class Foo.
18 class ParamFuncManipSplineDistTest : public ::testing::Test
19 {
20 protected:
21  // You can remove any or all of the following functions if its body
22  // is empty.
23 
24  ParamFuncManipSplineDistTest()
25  {
26  funcs = std::shared_ptr<ParamFuncsExtType>(new ParamFuncsExtType);
27  }
28 
29  virtual ~ParamFuncManipSplineDistTest()
30  {
31  }
32 
33  // If the constructor and destructor are not enough for setting up
34  // and cleaning up each test, you can define the following methods:
35 
36  virtual void SetUp()
37  {
38  Test::SetUp();
39  // Code here will be called immediately after the constructor (right
40  // before each test).
41  }
42 
43  virtual void TearDown()
44  {
45  Test::TearDown();
46  // Code here will be called immediately after each test (right
47  // before the destructor).
48  }
49  ParamFuncsSPtr funcs;
50 };
51 
53 
54 TEST_F(ParamFuncManipSplineDistTest, Initialization0)
55 {
56  using PFS = ParamFuncsStructure;
57  using PfCpD = CtrlPtDim;
58  using FeM = FuncEvalMode;
59  size_t funcIdx;
60  vector<PFS> pf(3, PFS());
61  pf[0].ctrlPtsSize = 5;
62  pf[0].ctrlPtsArcRefIdx = 0;
63  pf[1].ctrlPtsSize = 5;
64  pf[1].ctrlPtsArcRefIdx = 0;
65  pf[2].ctrlPtsSize = 8;
66  pf[2].ctrlPtsArcRefIdx = 1;
67 
68  pf[0].evalReq[(size_t)FeM::DIFF1] = false;
69  pf[0].evalReq[(size_t)FeM::DIFF2] = false;
70  pf[0].evalReq[(size_t)FeM::INT1] = false;
71  pf[0].evalReq[(size_t)FeM::INT2] = false;
72  pf[1].evalReq[(size_t)FeM::DIFF1] = false;
73  pf[1].evalReq[(size_t)FeM::DIFF2] = false;
74  pf[1].evalReq[(size_t)FeM::INT1] = false;
75  pf[1].evalReq[(size_t)FeM::INT2] = false;
76  pf[2].evalReq[(size_t)FeM::DIFF1] = false;
77  pf[2].evalReq[(size_t)FeM::DIFF2] = false;
78  pf[2].evalReq[(size_t)FeM::INT1] = false;
79  pf[2].evalReq[(size_t)FeM::INT2] = false;
80  funcs->init(pf);
81 
82  funcIdx = 0;
83  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
84  {
85  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = 10 - 2 * j;
86  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = j;
87  }
88  funcIdx = 1;
89  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
90  {
91  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = j;
92  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 2 * j;
93  }
94  funcIdx = 2;
95  for (size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx); j++)
96  {
97  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = -2 + 7 * j;
98  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 33 + 3 * j;
99  }
100 
101  funcIdx = 2;
102  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 666;
103  funcs->ctrlPtVal(funcIdx, funcs->funcCtrlPtSize(funcIdx) - 1, PfCpD::ARC) = 555;
104  funcIdx = 1;
105  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 123;
106 
107  funcs->precompute();
108 
109  size_t jj;
110  funcIdx = 1;
111  jj = 0;
112  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
113  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
114  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
115  funcIdx = 2;
116  jj = 0;
117  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
118  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
119  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
120 
121  // funcIdx = 0; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(10-2*j,
122  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
123  // funcIdx = 1; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ( j,
124  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
125  // funcIdx = 2; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(-2+7*j,
126  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ(33+3*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
127 }
128 
129 TEST_F(ParamFuncManipSplineDistTest, Initialization0FuncStatic)
130 {
131  funcs = std::shared_ptr<ParamFuncsSpline0Dist<double, 3, -1>>(new ParamFuncsSpline0Dist<double, 3, -1>);
132 
133  using PFS = ParamFuncsStructure;
134  using PfCpD = CtrlPtDim;
135  using FeM = FuncEvalMode;
136  size_t funcIdx;
137  vector<PFS> pf(3, PFS());
138  pf[0].ctrlPtsSize = 5;
139  pf[0].ctrlPtsArcRefIdx = 0;
140  pf[1].ctrlPtsSize = 5;
141  pf[1].ctrlPtsArcRefIdx = 0;
142  pf[2].ctrlPtsSize = 8;
143  pf[2].ctrlPtsArcRefIdx = 1;
144 
145  pf[0].evalReq[(size_t)FeM::DIFF1] = false;
146  pf[0].evalReq[(size_t)FeM::DIFF2] = false;
147  pf[0].evalReq[(size_t)FeM::INT1] = false;
148  pf[0].evalReq[(size_t)FeM::INT2] = false;
149  pf[1].evalReq[(size_t)FeM::DIFF1] = false;
150  pf[1].evalReq[(size_t)FeM::DIFF2] = false;
151  pf[1].evalReq[(size_t)FeM::INT1] = false;
152  pf[1].evalReq[(size_t)FeM::INT2] = false;
153  pf[2].evalReq[(size_t)FeM::DIFF1] = false;
154  pf[2].evalReq[(size_t)FeM::DIFF2] = false;
155  pf[2].evalReq[(size_t)FeM::INT1] = false;
156  pf[2].evalReq[(size_t)FeM::INT2] = false;
157  funcs->init(pf);
158 
159  funcIdx = 0;
160  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
161  {
162  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = 10 - 2 * j;
163  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = j;
164  }
165  funcIdx = 1;
166  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
167  {
168  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = j;
169  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 2 * j;
170  }
171  funcIdx = 2;
172  for (size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx); j++)
173  {
174  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = -2 + 7 * j;
175  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 33 + 3 * j;
176  }
177 
178  funcIdx = 2;
179  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 666;
180  funcs->ctrlPtVal(funcIdx, funcs->funcCtrlPtSize(funcIdx) - 1, PfCpD::ARC) = 555;
181  funcIdx = 1;
182  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 123;
183 
184  funcs->precompute();
185 
186  size_t jj;
187  funcIdx = 1;
188  jj = 0;
189  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
190  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
191  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
192  funcIdx = 2;
193  jj = 0;
194  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
195  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
196  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
197 
198  // funcIdx = 0; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(10-2*j,
199  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
200  // funcIdx = 1; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ( j,
201  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
202  // funcIdx = 2; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(-2+7*j,
203  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ(33+3*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
204 }
205 
206 TEST_F(ParamFuncManipSplineDistTest, Initialization0FuncAllStatic)
207 {
208  funcs = std::shared_ptr<ParamFuncsSpline0Dist<double, 3, 2>>(new ParamFuncsSpline0Dist<double, 3, 2>);
209 
210  using PFS = ParamFuncsStructure;
211  using PfCpD = CtrlPtDim;
212  using FeM = FuncEvalMode;
213  size_t funcIdx;
214  vector<PFS> pf(3, PFS());
215  pf[0].ctrlPtsSize = 5;
216  pf[0].ctrlPtsArcRefIdx = 0;
217  pf[1].ctrlPtsSize = 5;
218  pf[1].ctrlPtsArcRefIdx = 0;
219  pf[2].ctrlPtsSize = 8;
220  pf[2].ctrlPtsArcRefIdx = 1;
221 
222  pf[0].evalReq[(size_t)FeM::DIFF1] = false;
223  pf[0].evalReq[(size_t)FeM::DIFF2] = false;
224  pf[0].evalReq[(size_t)FeM::INT1] = false;
225  pf[0].evalReq[(size_t)FeM::INT2] = false;
226  pf[1].evalReq[(size_t)FeM::DIFF1] = false;
227  pf[1].evalReq[(size_t)FeM::DIFF2] = false;
228  pf[1].evalReq[(size_t)FeM::INT1] = false;
229  pf[1].evalReq[(size_t)FeM::INT2] = false;
230  pf[2].evalReq[(size_t)FeM::DIFF1] = false;
231  pf[2].evalReq[(size_t)FeM::DIFF2] = false;
232  pf[2].evalReq[(size_t)FeM::INT1] = false;
233  pf[2].evalReq[(size_t)FeM::INT2] = false;
234  funcs->init(pf);
235 
236  funcIdx = 0;
237  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
238  {
239  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = 10 - 2 * j;
240  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = j;
241  }
242  funcIdx = 1;
243  for (size_t j = 0; j < funcs->funcCtrlPtSize(funcIdx); j++)
244  {
245  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = j;
246  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 2 * j;
247  }
248  funcIdx = 2;
249  for (size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx); j++)
250  {
251  funcs->ctrlPtVal(funcIdx, j, PfCpD::VAL) = -2 + 7 * j;
252  funcs->ctrlPtVal(funcIdx, j, PfCpD::ARC) = 33 + 3 * j;
253  }
254 
255  funcIdx = 2;
256  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 666;
257  funcs->ctrlPtVal(funcIdx, funcs->funcCtrlPtSize(funcIdx) - 1, PfCpD::ARC) = 555;
258  funcIdx = 1;
259  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 123;
260 
261  funcs->precompute();
262 
263  size_t jj;
264  funcIdx = 1;
265  jj = 0;
266  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
267  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
268  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
269  funcIdx = 2;
270  jj = 0;
271  EXPECT_EQ(123, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
272  jj = funcs->funcCtrlPtSize(funcIdx) - 1;
273  EXPECT_EQ(555, funcs->ctrlPtVal(funcIdx, jj, PfCpD::ARC));
274 
275  // funcIdx = 0; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(10-2*j,
276  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
277  // funcIdx = 1; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ( j,
278  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ( 2*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
279  // funcIdx = 2; for(size_t j = 1; j < funcs->funcCtrlPtSize(funcIdx)-1; j++){ EXPECT_EQ(-2+7*j,
280  // funcs->ctrlPtVal(funcIdx,j,PfCpD::VAL) ); EXPECT_EQ(33+3*j, funcs->ctrlPtVal(funcIdx,j,PfCpD::ARC) ); }
281 }
282 
283 namespace EvalArcFuncValFuncValDiff1
284 {
285 using PFS = ParamFuncsStructure;
286 using PfCpD = CtrlPtDim;
287 using FeM = FuncEvalMode;
288 using EaG = EvalArcGuarantee;
289 
290 void initValDiff(ParamFuncsSPtr funcs, double initT)
291 {
292  size_t funcIdx = 0;
293  vector<PFS> pf(4, PFS());
294  pf[0].ctrlPtsSize = 4;
295  pf[0].ctrlPtsArcRefIdx = 0;
296  pf[1].ctrlPtsSize = 4;
297  pf[1].ctrlPtsArcRefIdx = 0;
298  pf[2].ctrlPtsSize = 7;
299  pf[2].ctrlPtsArcRefIdx = 1;
300  pf[3].ctrlPtsSize = 9;
301  pf[3].ctrlPtsArcRefIdx = 2;
302 
303  pf[0].evalReq[(size_t)FeM::DIFF1] = false;
304  pf[0].evalReq[(size_t)FeM::DIFF2] = false;
305  pf[0].evalReq[(size_t)FeM::INT1] = false;
306  pf[0].evalReq[(size_t)FeM::INT2] = false;
307  funcs->init(pf);
308 
309  funcIdx = 0;
310  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0;
311  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
312  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 2;
313  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
314  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1;
315  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3 + initT;
316  funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = 5;
317  funcs->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4 + initT;
318 
319  funcIdx = 1;
320  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 1;
321  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 3;
322  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 2;
323  funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = 6;
324 
325  funcs->precompute();
326 }
327 
328 void testValDiff(ParamFuncsSPtr funcs, double initT, size_t funcIdx, double funcShift)
329 {
330  funcs->setEvalArc(0 + initT, EaG::NONE);
331  EXPECT_DOUBLE_EQ(0 + funcShift, funcs->computeFuncVal(funcIdx));
332  funcs->setEvalArc(1 + initT, EaG::NONE);
333  EXPECT_DOUBLE_EQ(1 + funcShift, funcs->computeFuncVal(funcIdx));
334  funcs->setEvalArc(1.5 + initT, EaG::NONE);
335  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
336  funcs->setEvalArc(2.0 - 1e-5 + initT, EaG::NONE);
337  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
338  funcs->setEvalArc(2.0 + initT, EaG::NONE);
339  EXPECT_DOUBLE_EQ(2.0 + funcShift, funcs->computeFuncVal(funcIdx));
340  funcs->setEvalArc(2.0 + 1e-5 + initT, EaG::NONE);
341  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
342  funcs->setEvalArc(2.5 + initT, EaG::NONE);
343  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
344  funcs->setEvalArc(3.5 + initT, EaG::NONE);
345  EXPECT_DOUBLE_EQ(3 + funcShift, funcs->computeFuncVal(funcIdx));
346 
347  funcs->setEvalArc(0 + initT, EaG::NONE);
348  funcs->setEvalArc(0 + initT, EaG::NEAR_LAST);
349  EXPECT_DOUBLE_EQ(0 + funcShift, funcs->computeFuncVal(funcIdx));
350  funcs->setEvalArc(1 + initT, EaG::NEAR_LAST);
351  EXPECT_DOUBLE_EQ(1 + funcShift, funcs->computeFuncVal(funcIdx));
352  funcs->setEvalArc(1.5 + initT, EaG::NEAR_LAST);
353  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
354  funcs->setEvalArc(2.0 - 1e-5 + initT, EaG::NEAR_LAST);
355  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
356  funcs->setEvalArc(2.0 + initT, EaG::NEAR_LAST);
357  EXPECT_DOUBLE_EQ(2.0 + funcShift, funcs->computeFuncVal(funcIdx));
358  funcs->setEvalArc(2.0 + 1e-5 + initT, EaG::NEAR_LAST);
359  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
360  funcs->setEvalArc(2.5 + initT, EaG::NEAR_LAST);
361  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
362  funcs->setEvalArc(3.5 + initT, EaG::NEAR_LAST);
363  EXPECT_DOUBLE_EQ(3 + funcShift, funcs->computeFuncVal(funcIdx));
364 
365  funcs->setEvalArc(4 + initT, EaG::NONE);
366  funcs->setEvalArc(3.5 + initT, EaG::NEAR_LAST);
367  EXPECT_DOUBLE_EQ(3 + funcShift, funcs->computeFuncVal(funcIdx));
368  funcs->setEvalArc(2.5 + initT, EaG::NEAR_LAST);
369  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
370  funcs->setEvalArc(2.0 + 1e-5 + initT, EaG::NEAR_LAST);
371  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
372  funcs->setEvalArc(2.0 + initT, EaG::NEAR_LAST);
373  EXPECT_DOUBLE_EQ(2.0 + funcShift, funcs->computeFuncVal(funcIdx));
374  funcs->setEvalArc(2.0 - 1e-5 + initT, EaG::NEAR_LAST);
375  EXPECT_DOUBLE_EQ(2.0 - 1e-5 + funcShift, funcs->computeFuncVal(funcIdx));
376  funcs->setEvalArc(1.5 + initT, EaG::NEAR_LAST);
377  EXPECT_DOUBLE_EQ(1.5 + funcShift, funcs->computeFuncVal(funcIdx));
378  funcs->setEvalArc(1 + initT, EaG::NEAR_LAST);
379  EXPECT_DOUBLE_EQ(1 + funcShift, funcs->computeFuncVal(funcIdx));
380  funcs->setEvalArc(0 + initT, EaG::NEAR_LAST);
381  EXPECT_DOUBLE_EQ(0 + funcShift, funcs->computeFuncVal(funcIdx));
382 }
383 }
384 
385 TEST_F(ParamFuncManipSplineDistTest, evalArcFuncValFuncValDiff1)
386 {
387  size_t funcIdx = 0;
388  double initT = 2;
389  double funcShift;
390 
391  EvalArcFuncValFuncValDiff1::initValDiff(funcs, initT);
392 
393  funcIdx = 0;
394  EvalArcFuncValFuncValDiff1::testValDiff(funcs, initT, funcIdx, funcShift = 0);
395  funcIdx = 1;
396  EvalArcFuncValFuncValDiff1::testValDiff(funcs, initT, funcIdx, funcShift = 1);
397 }
398 
399 TEST_F(ParamFuncManipSplineDistTest, funcValInt1funcValInt2)
400 {
401  using PFS = ParamFuncsStructure;
402  using PfCpD = CtrlPtDim;
403  using FeM = FuncEvalMode;
404  using EaG = EvalArcGuarantee;
405  size_t funcIdx = 0;
406  vector<PFS> pf(4, PFS());
407  pf[0].ctrlPtsSize = 4;
408  pf[0].ctrlPtsArcRefIdx = 0;
409  pf[1].ctrlPtsSize = 4;
410  pf[1].ctrlPtsArcRefIdx = 0;
411  pf[2].ctrlPtsSize = 7;
412  pf[2].ctrlPtsArcRefIdx = 1;
413  pf[3].ctrlPtsSize = 9;
414  pf[3].ctrlPtsArcRefIdx = 2;
415 
416  pf[0].evalReq[(size_t)FeM::INT1] = true;
417  pf[0].evalReq[(size_t)FeM::INT2] = true;
418  funcs->init(pf);
419 
420  double initT = 2;
421  funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0;
422  funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
423  funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 2;
424  funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
425  funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1;
426  funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3 + initT;
427  funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = 5;
428  funcs->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4 + initT;
429 
430  funcs->precompute();
431 
432  funcs->setEvalArc(0 + initT, EaG::NONE);
433  EXPECT_DOUBLE_EQ(0, funcs->computeFuncInt1(0));
434  funcs->setEvalArc(1 + initT, EaG::NONE);
435  EXPECT_DOUBLE_EQ(0.5, funcs->computeFuncInt1(0));
436  funcs->setEvalArc(2.5 + initT, EaG::NONE);
437  EXPECT_DOUBLE_EQ(2.875, funcs->computeFuncInt1(0));
438  funcs->setEvalArc(3.5 + initT, EaG::NONE);
439  EXPECT_DOUBLE_EQ(4.5, funcs->computeFuncInt1(0));
440  funcs->setEvalArc(4.0 + initT, EaG::NONE);
441  EXPECT_DOUBLE_EQ(6.5, funcs->computeFuncInt1(0));
442 
443  funcs->setEvalArc(0 + initT, EaG::NONE);
444  EXPECT_DOUBLE_EQ(0, funcs->computeFuncInt2(0));
445  funcs->setEvalArc(1 + initT, EaG::NONE);
446  EXPECT_DOUBLE_EQ(1. / 6., funcs->computeFuncInt2(0));
447  funcs->setEvalArc(2.5 + initT, EaG::NONE);
448  EXPECT_DOUBLE_EQ(1.5625, funcs->computeFuncInt2(0));
449  funcs->setEvalArc(3.5 + initT, EaG::NONE);
450  EXPECT_DOUBLE_EQ(2.375, funcs->computeFuncInt2(0));
451  funcs->setEvalArc(4.0 + initT, EaG::NONE);
452  EXPECT_DOUBLE_EQ(10. / 3., funcs->computeFuncInt2(0));
453 
454  funcs->setEvalArc(2 + initT, EaG::NONE);
455  EXPECT_DOUBLE_EQ(1, funcs->computeFuncDiff1(0));
456  funcs->setEvalArc(2 + initT, EaG::NEAR_LAST);
457  EXPECT_DOUBLE_EQ(1, funcs->computeFuncDiff1(0));
458 
459  ParamFuncsExtType funcs2(dynamic_cast<ParamFuncsExtType &>(*funcs));
460 
461  funcs2.precompute();
462 
463  funcs2.setEvalArc(0 + initT, EaG::NONE);
464  EXPECT_DOUBLE_EQ(0, funcs2.computeFuncInt1(0));
465  funcs2.setEvalArc(1 + initT, EaG::NONE);
466  EXPECT_DOUBLE_EQ(0.5, funcs2.computeFuncInt1(0));
467  funcs2.setEvalArc(2.5 + initT, EaG::NONE);
468  EXPECT_DOUBLE_EQ(2.875, funcs2.computeFuncInt1(0));
469  funcs2.setEvalArc(3.5 + initT, EaG::NONE);
470  EXPECT_DOUBLE_EQ(4.5, funcs2.computeFuncInt1(0));
471  funcs2.setEvalArc(4.0 + initT, EaG::NONE);
472  EXPECT_DOUBLE_EQ(6.5, funcs2.computeFuncInt1(0));
473 
474  funcs2.setEvalArc(0 + initT, EaG::NONE);
475  EXPECT_DOUBLE_EQ(0, funcs2.computeFuncInt2(0));
476  funcs2.setEvalArc(1 + initT, EaG::NONE);
477  EXPECT_DOUBLE_EQ(1. / 6., funcs2.computeFuncInt2(0));
478  funcs2.setEvalArc(2.5 + initT, EaG::NONE);
479  EXPECT_DOUBLE_EQ(1.5625, funcs2.computeFuncInt2(0));
480  funcs2.setEvalArc(3.5 + initT, EaG::NONE);
481  EXPECT_DOUBLE_EQ(2.375, funcs2.computeFuncInt2(0));
482  funcs2.setEvalArc(4.0 + initT, EaG::NONE);
483  EXPECT_DOUBLE_EQ(10. / 3., funcs2.computeFuncInt2(0));
484 
485  funcs2.setEvalArc(2 + initT, EaG::NONE);
486  EXPECT_DOUBLE_EQ(1, funcs2.computeFuncDiff1(0));
487  funcs2.setEvalArc(2 + initT, EaG::NEAR_LAST);
488  EXPECT_DOUBLE_EQ(1, funcs2.computeFuncDiff1(0));
489 
490  ParamFuncsExtType funcs3;
491  funcs3 = dynamic_cast<ParamFuncsExtType &>(*funcs);
492 
493  funcs3.precompute();
494 
495  funcs3.setEvalArc(0 + initT, EaG::NONE);
496  EXPECT_DOUBLE_EQ(0, funcs3.computeFuncInt1(0));
497  funcs3.setEvalArc(1 + initT, EaG::NONE);
498  EXPECT_DOUBLE_EQ(0.5, funcs3.computeFuncInt1(0));
499  funcs3.setEvalArc(2.5 + initT, EaG::NONE);
500  EXPECT_DOUBLE_EQ(2.875, funcs3.computeFuncInt1(0));
501  funcs3.setEvalArc(3.5 + initT, EaG::NONE);
502  EXPECT_DOUBLE_EQ(4.5, funcs3.computeFuncInt1(0));
503  funcs3.setEvalArc(4.0 + initT, EaG::NONE);
504  EXPECT_DOUBLE_EQ(6.5, funcs3.computeFuncInt1(0));
505 
506  funcs3.setEvalArc(0 + initT, EaG::NONE);
507  EXPECT_DOUBLE_EQ(0, funcs3.computeFuncInt2(0));
508  funcs3.setEvalArc(1 + initT, EaG::NONE);
509  EXPECT_DOUBLE_EQ(1. / 6., funcs3.computeFuncInt2(0));
510  funcs3.setEvalArc(2.5 + initT, EaG::NONE);
511  EXPECT_DOUBLE_EQ(1.5625, funcs3.computeFuncInt2(0));
512  funcs3.setEvalArc(3.5 + initT, EaG::NONE);
513  EXPECT_DOUBLE_EQ(2.375, funcs3.computeFuncInt2(0));
514  funcs3.setEvalArc(4.0 + initT, EaG::NONE);
515  EXPECT_DOUBLE_EQ(10. / 3., funcs3.computeFuncInt2(0));
516 
517  funcs3.setEvalArc(2 + initT, EaG::NONE);
518  EXPECT_DOUBLE_EQ(1, funcs3.computeFuncDiff1(0));
519  funcs3.setEvalArc(2 + initT, EaG::NEAR_LAST);
520  EXPECT_DOUBLE_EQ(1, funcs3.computeFuncDiff1(0));
521 }
522 
524 
525 using pfsd = ParamFuncsExtType;
526 TEST(Spline0computeDtDs_V_AV_Test, TPos_VPos_A0)
527 {
528  double dt = 1, v = 1, av = 0, dsAnsGiven = 1;
529  double dsAns, dtAns;
530  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
531  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
532  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
533  EXPECT_DOUBLE_EQ(dt, dtAns);
534 }
535 TEST(Spline0computeDtDs_V_AV_Test, TPos_VNeg_A0)
536 {
537  double dt = 1, v = -1, av = 0, dsAnsGiven = 1;
538  double dsAns, dtAns;
539  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
540  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
541  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
542  EXPECT_DOUBLE_EQ(dt, dtAns);
543 }
544 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VPos_A0)
545 {
546  double dt = -1, v = +1, av = 0, dsAnsGiven = -1;
547  double dsAns, dtAns;
548  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
549  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
550  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
551  EXPECT_DOUBLE_EQ(dt, dtAns);
552 }
553 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VNeg_A0)
554 {
555  double dt = -1, v = -1, av = 0, dsAnsGiven = -1;
556  double dsAns, dtAns;
557  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
558  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
559  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
560  EXPECT_DOUBLE_EQ(dt, dtAns);
561 }
563 TEST(Spline0computeDtDs_V_AV_Test, TPos_VPos_APos)
564 {
565  double dt = 1, v = 1, av = 1, dsAnsGiven = 1.5;
566  double dsAns, dtAns;
567  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
568  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
569  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
570  EXPECT_DOUBLE_EQ(dt, dtAns);
571 }
572 TEST(Spline0computeDtDs_V_AV_Test, TPos_VNeg_ANeg)
573 {
574  double dt = 1, v = -1, av = -1, dsAnsGiven = 1.5;
575  double dsAns, dtAns;
576  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
577  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
578  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
579  EXPECT_DOUBLE_EQ(dt, dtAns);
580 }
581 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VPos_APos)
582 {
583  double dt = -1, v = +1, av = +1, dsAnsGiven = -1.5;
584  double dsAns, dtAns;
585  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
586  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
587  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
588  EXPECT_DOUBLE_EQ(dt, dtAns);
589 }
590 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VNeg_ANeg)
591 {
592  double dt = -1, v = -1, av = -1, dsAnsGiven = -1.5;
593  double dsAns, dtAns;
594  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
595  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
596  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
597  EXPECT_DOUBLE_EQ(dt, dtAns);
598 }
600 TEST(Spline0computeDtDs_V_AV_Test, TPos_VPos_ANeg_Above0)
601 {
602  double dt = 1, v = 1, av = -0.5, dsAnsGiven = 0.75;
603  double dsAns, dtAns;
604  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
605  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
606  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
607  EXPECT_DOUBLE_EQ(dt, dtAns);
608 }
609 TEST(Spline0computeDtDs_V_AV_Test, TPos_VNeg_APos_Below0)
610 {
611  double dt = 1, v = -1, av = 0.5, dsAnsGiven = 0.75;
612  double dsAns, dtAns;
613  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
614  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
615  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
616  EXPECT_DOUBLE_EQ(dt, dtAns);
617 }
618 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VPos_ANeg_Above0)
619 {
620  double dt = -1, v = 1, av = -0.5, dsAnsGiven = -0.75;
621  double dsAns, dtAns;
622  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
623  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
624  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
625  EXPECT_DOUBLE_EQ(dt, dtAns);
626 }
627 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VNeg_APos_Below0)
628 {
629  double dt = -1, v = -1, av = 0.5, dsAnsGiven = -0.75;
630  double dsAns, dtAns;
631  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
632  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
633  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
634  EXPECT_DOUBLE_EQ(dt, dtAns);
635 }
637 TEST(Spline0computeDtDs_V_AV_Test, TPos_VPos_ANeg_Below0_1)
638 {
639  double dt = 1, v = 1, av = -1.5, dsAnsGiven = 5. / 12.;
640  double dsAns, dtAns;
641  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
642  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
643  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
644  EXPECT_DOUBLE_EQ(dt, dtAns);
645 }
646 TEST(Spline0computeDtDs_V_AV_Test, TPos_VNeg_APos_Above0_1)
647 {
648  double dt = 1, v = -1, av = +1.5, dsAnsGiven = 5. / 12.;
649  double dsAns, dtAns;
650  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
651  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
652  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
653  EXPECT_DOUBLE_EQ(dt, dtAns);
654 }
655 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VPos_ANeg_Below0_1)
656 {
657  double dt = -1, v = 1, av = -1.5, dsAnsGiven = -5. / 12.;
658  double dsAns, dtAns;
659  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
660  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
661  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
662  EXPECT_DOUBLE_EQ(dt, dtAns);
663 }
664 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VNeg_APos_Above0_1)
665 {
666  double dt = -1, v = -1, av = +1.5, dsAnsGiven = -5. / 12.;
667  double dsAns, dtAns;
668  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
669  EXPECT_DOUBLE_EQ(dsAnsGiven, dsAns);
670  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
671  EXPECT_DOUBLE_EQ(dt, dtAns);
672 }
674 TEST(Spline0computeDtDs_V_AV_Test, TPos_VPos_ANeg_Below0)
675 {
676  double dt = 6, v = 3, av = -3.5;
677  double dsAns, dtAns;
678  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
679  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
680  EXPECT_DOUBLE_EQ(dt, dtAns);
681 }
682 TEST(Spline0computeDtDs_V_AV_Test, TPos_VNeg_APos_Above0)
683 {
684  double dt = 6, v = -3, av = +3.5;
685  double dsAns, dtAns;
686  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
687  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
688  EXPECT_DOUBLE_EQ(dt, dtAns);
689 }
690 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VPos_ANeg_Below0)
691 {
692  double dt = -6, v = 3, av = -3.5;
693  double dsAns, dtAns;
694  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
695  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
696  EXPECT_DOUBLE_EQ(dt, dtAns);
697 }
698 TEST(Spline0computeDtDs_V_AV_Test, TNeg_VNeg_APos_Above0)
699 {
700  double dt = -6, v = -3, av = +3.5;
701  double dsAns, dtAns;
702  dsAns = pfsd::computeDeltaS_V_AV(dt, v, av);
703  dtAns = pfsd::computeDeltaT_V_AV(dsAns, v, av);
704  EXPECT_DOUBLE_EQ(dt, dtAns);
705 }
707 
708 TEST_F(ParamFuncManipSplineDistTest, distTimeV)
709 {
710  ParamFuncsExtTypePtr funcss = dynamic_pointer_cast<ParamFuncsExtType>(funcs);
711 
712  using PFS = ParamFuncsStructure;
713  using PfCpD = CtrlPtDim;
714  using FeM = FuncEvalMode;
715  using EaG = EvalArcGuarantee;
716  size_t funcIdx = 0;
717  vector<PFS> pf(4, PFS());
718  pf[0].ctrlPtsSize = 4;
719  pf[0].ctrlPtsArcRefIdx = 0;
720  pf[1].ctrlPtsSize = 4;
721  pf[1].ctrlPtsArcRefIdx = 0;
722  pf[2].ctrlPtsSize = 7;
723  pf[2].ctrlPtsArcRefIdx = 1;
724  pf[3].ctrlPtsSize = 9;
725  pf[3].ctrlPtsArcRefIdx = 2;
726 
727  pf[0].evalReq[(size_t)FeM::INT1] = true;
728  pf[0].evalReq[(size_t)FeM::INT2] = true;
729  funcss->init(pf);
730  vector<size_t> idxCfV(1, 0);
731  funcss->setDistCfMode(TraveledDistCfMode::V, idxCfV);
732 
733  double initT = 3;
734  funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = -1;
735  funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
736  funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1;
737  funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
738  funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -1;
739  funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3 + initT;
740  funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = -5;
741  funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4 + initT;
742  funcss->precompute();
743 
744  funcss->setEvalArc(1 + initT, EaG::NONE);
745  EXPECT_DOUBLE_EQ(0.5, funcss->computeS());
746  EXPECT_DOUBLE_EQ(1 + initT, funcss->computeT(0.5));
747  funcss->setEvalArc(2 + initT, EaG::NONE);
748  EXPECT_DOUBLE_EQ(1, funcss->computeS());
749  EXPECT_DOUBLE_EQ(2 + initT, funcss->computeT(1));
750  funcss->setEvalArc(3 + initT, EaG::NONE);
751  EXPECT_DOUBLE_EQ(1.5, funcss->computeS());
752  EXPECT_DOUBLE_EQ(3 + initT, funcss->computeT(1.5));
753  funcss->setEvalArc(3.5 + initT, EaG::NONE);
754  EXPECT_DOUBLE_EQ(2.5, funcss->computeS());
755  EXPECT_DOUBLE_EQ(3.5 + initT, funcss->computeT(2.5));
756  funcss->setEvalArc(4 + initT, EaG::NONE);
757  EXPECT_DOUBLE_EQ(4.5, funcss->computeS());
758  EXPECT_DOUBLE_EQ(4 + initT, funcss->computeT(4.5));
759 
760  funcss->setEvalArc(1 + initT, EaG::NONE);
761  EXPECT_DOUBLE_EQ(0.5, funcss->computeS());
762 
763  EXPECT_DOUBLE_EQ(1 + initT, funcss->computeT(0.5, EaG::NEAR_LAST));
764  EXPECT_DOUBLE_EQ(2 + initT, funcss->computeT(1, EaG::NEAR_LAST));
765  EXPECT_DOUBLE_EQ(3 + initT, funcss->computeT(1.5, EaG::NEAR_LAST));
766  EXPECT_DOUBLE_EQ(3.5 + initT, funcss->computeT(2.5, EaG::NEAR_LAST));
767  EXPECT_DOUBLE_EQ(4 + initT, funcss->computeT(4.5, EaG::NEAR_LAST));
768 
769  funcss->setEvalArc(2 + initT, EaG::NEAR_LAST);
770  EXPECT_DOUBLE_EQ(1, funcss->computeS());
771  funcss->setEvalArc(3 + initT, EaG::NEAR_LAST);
772  EXPECT_DOUBLE_EQ(1.5, funcss->computeS());
773  funcss->setEvalArc(3.5 + initT, EaG::NEAR_LAST);
774  EXPECT_DOUBLE_EQ(2.5, funcss->computeS());
775  funcss->setEvalArc(4 + initT, EaG::NEAR_LAST);
776  EXPECT_DOUBLE_EQ(4.5, funcss->computeS());
777 
778  double evalTempSol;
779  for (size_t i = 0; i < 4; i++)
780  {
781  funcss->setEvalArc(1 + initT, EaG::NONE);
782  evalTempSol = funcss->computeFuncVal(i);
783  funcss->setEvalDist(0.5, EaG::NONE);
784  EXPECT_DOUBLE_EQ(evalTempSol, funcss->computeFuncVal(i));
785  }
786  for (size_t i = 0; i < 4; i++)
787  {
788  funcss->setEvalArc(2 + initT, EaG::NONE);
789  evalTempSol = funcss->computeFuncVal(i);
790  funcss->setEvalDist(1, EaG::NONE);
791  EXPECT_DOUBLE_EQ(evalTempSol, funcss->computeFuncVal(i));
792  }
793 
794  vector<double> sLattice(8, 0);
795  sLattice[0] = 0;
796  sLattice[1] = 0.5;
797  sLattice[2] = 1;
798  sLattice[3] = 1.5;
799  sLattice[4] = 2.5;
800  sLattice[5] = 4.5;
801  sLattice[6] = 6;
802  sLattice[7] = -1;
803  vector<double> tLattice(120, 0);
804  funcss->computeS2TLattice(sLattice, tLattice);
805 
806  EXPECT_DOUBLE_EQ(6, tLattice.size());
807 
808  EXPECT_DOUBLE_EQ(0 + initT, tLattice[0]);
809  EXPECT_DOUBLE_EQ(1 + initT, tLattice[1]);
810  EXPECT_DOUBLE_EQ(2 + initT, tLattice[2]);
811  EXPECT_DOUBLE_EQ(3 + initT, tLattice[3]);
812  EXPECT_DOUBLE_EQ(3.5 + initT, tLattice[4]);
813  EXPECT_DOUBLE_EQ(4 + initT, tLattice[5]);
814 
815  sLattice.resize(4);
816  sLattice[0] = 0;
817  sLattice[1] = 0.5;
818  sLattice[2] = 1;
819  sLattice[3] = 0.5;
820  funcss->computeS2TLattice(sLattice, tLattice);
821 
822  EXPECT_DOUBLE_EQ(4, tLattice.size());
823 
824  EXPECT_DOUBLE_EQ(0 + initT, tLattice[0]);
825  EXPECT_DOUBLE_EQ(1 + initT, tLattice[1]);
826  EXPECT_DOUBLE_EQ(2 + initT, tLattice[2]);
827  EXPECT_DOUBLE_EQ(4 + initT, tLattice[3]);
828 
829  sLattice.resize(2);
830  sLattice[0] = 0;
831  sLattice[1] = 100;
832  funcss->computeS2TLattice(sLattice, tLattice);
833 
834  EXPECT_DOUBLE_EQ(2, tLattice.size());
835 
836  EXPECT_DOUBLE_EQ(0 + initT, tLattice[0]);
837  EXPECT_DOUBLE_EQ(4 + initT, tLattice[1]);
838 }
839 
840 TEST_F(ParamFuncManipSplineDistTest, timeShift)
841 {
842  ParamFuncsExtTypePtr funcss = dynamic_pointer_cast<ParamFuncsExtType>(funcs);
843 
844  using PFS = ParamFuncsStructure;
845  using PfCpD = CtrlPtDim;
846  using FeM = FuncEvalMode; // using EaG = EvalArcGuarantee;
847  size_t funcIdx = 0;
848  vector<PFS> pf(4, PFS());
849  pf[0].ctrlPtsSize = 4;
850  pf[0].ctrlPtsArcRefIdx = 0;
851  pf[1].ctrlPtsSize = 4;
852  pf[1].ctrlPtsArcRefIdx = 0;
853  pf[2].ctrlPtsSize = 3;
854  pf[2].ctrlPtsArcRefIdx = 1;
855  pf[3].ctrlPtsSize = 3;
856  pf[3].ctrlPtsArcRefIdx = 2;
857 
858  pf[0].evalReq[(size_t)FeM::INT1] = true;
859  pf[0].evalReq[(size_t)FeM::INT2] = true;
860  funcss->init(pf);
861  vector<size_t> idxCfV(1, 0);
862  funcss->setDistCfMode(TraveledDistCfMode::V, idxCfV);
863 
864  double initT = 0;
865  funcIdx = 0;
866  funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = -1;
867  funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
868  funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1;
869  funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
870  funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 0;
871  funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3 + initT;
872  funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = -5;
873  funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4 + initT;
874  funcIdx = 1;
875  funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0;
876  funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
877  funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1;
878  funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
879  funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -1;
880  funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3 + initT;
881  funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = -5;
882  funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4 + initT;
883  funcIdx = 2;
884  funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = -10;
885  funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
886  funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 6;
887  funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
888  funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 0;
889  funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 4 + initT;
890  funcIdx = 3;
891  funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = -1;
892  funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0 + initT;
893  funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1;
894  funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2 + initT;
895  funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -1;
896  funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 4 + initT;
897  funcss->precompute();
898 
899  double shift;
900  shift = 1;
901  funcss->shiftStartCtrlPt(shift);
902  funcIdx = 0;
903  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
904  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
905  EXPECT_DOUBLE_EQ(1, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
906  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
907  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
908  EXPECT_DOUBLE_EQ(fmax(3 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
909  EXPECT_DOUBLE_EQ(-5, funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL));
910  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC));
911 
912  funcIdx = 1;
913  EXPECT_DOUBLE_EQ(0.5, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
914  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
915  EXPECT_DOUBLE_EQ(1, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
916  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
917  EXPECT_DOUBLE_EQ(-1, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
918  EXPECT_DOUBLE_EQ(fmax(3 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
919  EXPECT_DOUBLE_EQ(-5, funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL));
920  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC));
921 
922  funcIdx = 2;
923  EXPECT_DOUBLE_EQ(-2, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
924  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
925  EXPECT_DOUBLE_EQ(6, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
926  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
927  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
928  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
929 
930  funcIdx = 3;
931  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
932  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
933  EXPECT_DOUBLE_EQ(1, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
934  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
935  EXPECT_DOUBLE_EQ(-1, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
936  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
937 
938  shift = 1.5;
939  funcss->shiftStartCtrlPt(shift);
940  funcIdx = 0;
941  EXPECT_DOUBLE_EQ(0.5, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
942  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
943  EXPECT_DOUBLE_EQ(0.5, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
944  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
945  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
946  EXPECT_DOUBLE_EQ(fmax(3 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
947  EXPECT_DOUBLE_EQ(-5, funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL));
948  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC));
949 
950  funcIdx = 1;
951  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
952  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
953  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
954  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
955  EXPECT_DOUBLE_EQ(-1, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
956  EXPECT_DOUBLE_EQ(fmax(3 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
957  EXPECT_DOUBLE_EQ(-5, funcss->ctrlPtVal(funcIdx, 3, PfCpD::VAL));
958  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 3, PfCpD::ARC));
959 
960  funcIdx = 2;
961  EXPECT_DOUBLE_EQ(4.5, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
962  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
963  EXPECT_DOUBLE_EQ(4.5, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
964  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
965  EXPECT_DOUBLE_EQ(0, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
966  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
967 
968  funcIdx = 3;
969  EXPECT_DOUBLE_EQ(0.5, funcss->ctrlPtVal(funcIdx, 0, PfCpD::VAL));
970  EXPECT_DOUBLE_EQ(initT, funcss->ctrlPtVal(funcIdx, 0, PfCpD::ARC));
971  EXPECT_DOUBLE_EQ(0.5, funcss->ctrlPtVal(funcIdx, 1, PfCpD::VAL));
972  EXPECT_DOUBLE_EQ(fmax(2 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 1, PfCpD::ARC));
973  EXPECT_DOUBLE_EQ(-1, funcss->ctrlPtVal(funcIdx, 2, PfCpD::VAL));
974  EXPECT_DOUBLE_EQ(fmax(4 + initT - shift - 1, initT), funcss->ctrlPtVal(funcIdx, 2, PfCpD::ARC));
975 }
976 
977 // TEST_F ( ParamFuncManipSplineDistTest, evalArcFuncValFuncValInconsistentArc ) {
978 //
979 // using PFS = ParamFuncsStructure; using PfCpD = CtrlPtDim; using EaG = EvalArcGuarantee;
980 // size_t funcIdx = 0; vector<PFS> pf( 2 , PFS() );
981 // pf[0].ctrlPtsSize = 4; pf[0].ctrlPtsArcRefIdx = 0;
982 // pf[1].ctrlPtsSize = 6; pf[1].ctrlPtsArcRefIdx = 1;
983 //
984 // funcs->init( pf );
985 //
986 // double initT = 3;
987 // funcIdx = 0;
988 // funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = -1; funcs->ctrlPtVal(funcIdx, 0, PfCpD::ARC) = 0+initT;
989 // funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1; funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 2+initT;
990 // funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = -1; funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 3+initT;
991 // funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = -5; funcs->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4+initT;
992 //
993 // funcIdx = 1;
994 // funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0;
995 // funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = -1; funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 3+initT;
996 // funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1; funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 5+initT;
997 // funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = 3; funcs->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 4+initT;
998 // funcs->ctrlPtVal(funcIdx, 4, PfCpD::VAL) = -5; funcs->ctrlPtVal(funcIdx, 4, PfCpD::ARC) = 6+initT;
999 // funcs->ctrlPtVal(funcIdx, 5, PfCpD::VAL) = 0;
1000 // //so this temp val is 0 -1 1 3 -5 0
1001 // //so this temp arc is 0 3 5 4 7 4
1002 // funcs->precompute();
1003 //
1004 // funcs->setEvalArc(4.0+initT, EaG::NONE ); EXPECT_DOUBLE_EQ( 0, funcs->computeFuncVal(1) );
1005 // funcs->setEvalArc(5.0+initT, EaG::NEAR_LAST); EXPECT_DOUBLE_EQ( -1, funcs->computeFuncVal(1) );//goes till 2nd
1006 // range where 5 is found...first range is [3,5)
1007 //
1008 // funcIdx = 1;
1009 // funcs->ctrlPtVal(funcIdx, 0, PfCpD::VAL) = 0;
1010 // funcs->ctrlPtVal(funcIdx, 1, PfCpD::VAL) = 1; funcs->ctrlPtVal(funcIdx, 1, PfCpD::ARC) = 3+initT;
1011 // funcs->ctrlPtVal(funcIdx, 2, PfCpD::VAL) = 1; funcs->ctrlPtVal(funcIdx, 2, PfCpD::ARC) = 2+initT;
1012 // funcs->ctrlPtVal(funcIdx, 3, PfCpD::VAL) = 3; funcs->ctrlPtVal(funcIdx, 3, PfCpD::ARC) = 2+initT;
1013 // funcs->ctrlPtVal(funcIdx, 4, PfCpD::VAL) = -3; funcs->ctrlPtVal(funcIdx, 4, PfCpD::ARC) = 1+initT;
1014 // funcs->ctrlPtVal(funcIdx, 5, PfCpD::VAL) = -1; funcs->ctrlPtVal(funcIdx, 4, PfCpD::ARC) =-1+initT;
1015 // //so this temp val is 0 -1 1 3 -5 0
1016 // //so this temp arc is 0 3 2 2 1 -1
1017 // funcs->precompute();
1018 //
1019 // funcs->setEvalArc(0.0+initT, EaG::NONE ); EXPECT_DOUBLE_EQ( 0, funcs->computeFuncVal(1) );
1020 // }
1021 
1022 } // namespace
1023 
1024 int main(int argc, char **argv)
1025 {
1026  ::testing::InitGoogleTest(&argc, argv);
1027  return RUN_ALL_TESTS();
1028 }
tuw::ParamFuncsStructure
Containts parametric function initialization data.
Definition: param_func.hpp:76
tuw::TEST
TEST(TestStateMap, CopyAssignmentOperatorArray)
Definition: test_state_map.cpp:17
tuw::ParamFuncsSPtr
std::shared_ptr< ParamFuncs > ParamFuncsSPtr
Definition: param_func.h:55
tuw::CtrlPtDim::VAL
control point value
tuw::CtrlPtDim::ARC
control point arc parameter
param_func_spline0_dist.hpp
std
Helper function needed to upgrade c++ 2011.
Definition: utils.h:193
tuw::FuncEvalMode::DIFF2
function second derivative
tuw::FuncEvalMode::INT1
function single integral
main
int main(int argc, char **argv)
Definition: test_param_func_manip_spline0_dist.cpp:1024
tuw::TraveledDistCfMode::V
agent base center linear velocity is parametric function
tuw::EvalArcGuarantee
EvalArcGuarantee
Flags if any guarantees about evaluation arc relative to last evaluation arc are present.
Definition: param_func.hpp:61
tuw::CtrlPtDim
CtrlPtDim
Control point variable dimension.
Definition: param_func.hpp:55
tuw::EvalArcGuarantee::NEAR_LAST
close to previous evaluation arc
tuw::FuncEvalMode
FuncEvalMode
Required type of computation relative to the parametric function.
Definition: param_func.hpp:45
tuw::FuncEvalMode::INT2
function double integral
tuw::FuncEvalMode::DIFF1
function first derivative

tuw_control
Author(s): George Todoran
autogenerated on Mon Jun 10 2019 15:27:22