99 constexpr
int dim_bs = 3;
100 constexpr
int degree = 3;
101 constexpr
int num_middle_pt = 3;
102 constexpr
int ini_cstr_level = 2;
103 constexpr
int fin_cstr_level = 2;
105 BS_Basic<double, dim_bs, degree, num_middle_pt, ini_cstr_level,
108 double ini_pt[dim_bs * (degree)];
109 double fin_pt[dim_bs * (degree)];
111 for (
int i(0); i < dim_bs * degree; ++i) {
115 double height_gap = 0.3;
120 double** middle_pt = NULL;
121 if (num_middle_pt > 1) {
122 middle_pt =
new double*[num_middle_pt];
123 for (
int i(0); i < num_middle_pt; ++i) {
124 middle_pt[i] =
new double[dim_bs];
127 middle_pt[0][0] = 0.4;
128 middle_pt[0][1] = -0.7;
129 middle_pt[0][2] = ini_pt[2] + height_gap * 0.25;
132 middle_pt[1][0] = 0.4;
133 middle_pt[1][1] = -0.7;
134 middle_pt[1][2] = ini_pt[2] + height_gap * 0.5;
137 middle_pt[2][0] = 2.5;
138 middle_pt[2][1] = 0.5;
139 middle_pt[2][2] = ini_pt[2] + height_gap * 0.75;
144 fin_pt[2] = ini_pt[2] + height_gap;
155 bs.SetParam(ini_pt, fin_pt, middle_pt, end_time);
157 double curve_pt[dim_bs];
158 double curve_vel[dim_bs];
159 double curve_acc[dim_bs];
160 double curve_d3[dim_bs];
164 bs.getCurvePoint(mid_time, curve_pt);
165 bs.getCurveDerPoint(mid_time, 1, curve_vel);
166 bs.getCurveDerPoint(mid_time, 2, curve_acc);
168 bs.getCurveDerPoint(mid_time, 3, curve_d3);
170 BS_Basic<double, dim_bs, degree, num_middle_pt, ini_cstr_level,
173 double ini_pt2[dim_bs * degree];
174 double fin_pt2[dim_bs * degree];
175 for (
int i(0); i < dim_bs * degree; ++i) {
179 for (
int i(0); i < 3; ++i) {
180 ini_pt2[i] = curve_pt[i];
181 ini_pt2[i + 3] = curve_vel[i];
182 ini_pt2[i + 6] = curve_acc[i];
184 if (degree > 3) ini_pt2[i + 9] = curve_d3[i];
187 double** middle_pt2 = NULL;
188 if (num_middle_pt > 0) {
189 middle_pt2 =
new double*[num_middle_pt];
190 for (
int i(0); i < num_middle_pt; ++i) {
191 middle_pt2[i] =
new double[dim_bs];
194 middle_pt2[0][0] = 0.4;
195 middle_pt2[0][1] = -0.7;
196 middle_pt2[0][2] = ini_pt2[2] + height_gap * 0.25;
199 middle_pt2[1][0] = 0.4;
200 middle_pt2[1][1] = -0.7;
201 middle_pt2[1][2] = ini_pt2[2] + height_gap * 0.5;
204 middle_pt2[2][0] = 2.5;
205 middle_pt2[2][1] = 0.5;
206 middle_pt2[2][2] = ini_pt2[2] + height_gap * 0.75;
212 fin_pt2[2] = ini_pt2[2] + height_gap;
214 bs_2.
SetParam(ini_pt2, fin_pt2, middle_pt2, end_time);
218 for (
int i(0); i < 1001; ++i) {
222 bs.getCurvePoint(t, curve_pt);
223 bs.getCurveDerPoint(t, 1, curve_vel);
224 bs.getCurveDerPoint(t, 2, curve_acc);
226 bs_2.getCurvePoint(t - mid_time, curve_pt);
227 bs_2.getCurveDerPoint(t - mid_time, 1, curve_vel);
228 bs_2.getCurveDerPoint(t - mid_time, 2, curve_acc);
237 bs.getCurvePoint(0., curve_pt);
238 EXPECT_TRUE(
fpEqual(curve_pt[0], 0., .0001));
239 EXPECT_TRUE(
fpEqual(curve_pt[1], 0.0, .0001));
240 EXPECT_TRUE(
fpEqual(curve_pt[2], 0.4, .0001));
242 bs.getCurvePoint(end_time, curve_pt);
243 EXPECT_TRUE(
fpEqual(curve_pt[0], 4.2, .0001));
244 EXPECT_TRUE(
fpEqual(curve_pt[1], -1.0, .0001));
247 bs.getCurveDerPoint(end_time, 2, curve_acc);
248 EXPECT_TRUE(
fpEqual(curve_acc[0], 0., .0001));
249 EXPECT_TRUE(
fpEqual(curve_acc[1], 0.0, .0001));
250 EXPECT_TRUE(
fpEqual(curve_acc[2], 0.0, .0001));
252 for (
int i(0); i < num_middle_pt; ++i) {
253 delete[] middle_pt[i];
bool fpEqual(T a, T b, T tol)
bool SetParam(T *init, T *fin, T **middle_pt, T fin_time)