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