8 #define SP_IS_EQUAL(x, y) (((x) - (y)) * ((x) - (y)) < 1.e-10) 9 #define SP_SAFE_DELETE_AR(p) \ 33 template <
typename T,
int DIM,
int DEGREE,
int NUM_MIDDLE,
int CONST_LEVEL_INI,
38 :
NumKnots_(DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN +
40 NumCPs_(NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN) {
42 for (
int i(0); i <
NumCPs_; ++i) {
43 for (
int j(0); j < DIM; ++j)
CPoints_[i][j] = 0.;
45 if (NumKnots_ < 2 * (DEGREE + 1)) {
46 printf(
"Invalid setup (num_knots, degree): %d, %d\n", NumKnots_, DEGREE);
63 bool SetParam(T* init, T* fin, T** middle_pt, T fin_time) {
94 for (
int j(0); j < DIM; ++j) {
96 for (
int i(0); i <= DEGREE; ++i) {
97 _C[j] += _N[i] *
CPoints_[_span - DEGREE + i][j];
101 for (
int i(0); i < DIM; ++i) {
116 if (d > DEGREE)
return 0.0;
124 T** _CK =
new T*[d + 1];
125 for (
int i(0); i < d + 1; ++i) {
129 for (
int m(0); m < DIM; ++m) ret[m] = _CK[d][m];
131 for (
int p(0); p < d + 1; ++p)
delete[] _CK[p];
135 for (
int p(0); p < d + 1; ++p)
delete[] _CK[p];
146 int _NumMidKnot(
NumKnots_ - 2 * DEGREE - 2);
147 T _TimeStep = Tf / (_NumMidKnot + 1);
150 for (_j = 0; _j < DEGREE + 1; ++_j)
Knots_[_i++] = 0.0;
154 for (_j = 0; _j < _NumMidKnot; ++_j) {
159 for (_j = 0; _j < DEGREE + 1; ++_j)
Knots_[_i++] = Tf;
171 T** _nders =
new T*[d + 1];
173 for (_k = 0; _k < d + 1; ++_k) _nders[_k] =
new T[DEGREE + 1];
180 for (_k = 0; _k <= d; ++_k) {
182 for (
int m(0); m < DIM; ++m) CK[_k][m] = 0.;
184 for (_j = 0; _j <= DEGREE; ++_j) {
185 for (
int m(0); m < DIM; ++m) {
186 CK[_k][m] += _nders[_k][_j] *
CPoints_[_span - DEGREE + _j][m];
190 for (_k = 0; _k < d + 1; ++_k)
delete[] _nders[_k];
218 T** _ndu =
new T*[DEGREE + 1];
219 for (_j = 0; _j <= DEGREE; ++_j) _ndu[_j] =
new T[DEGREE + 1];
223 for (_j = 0; _j < 2; ++_j) _a[_j] =
new T[DEGREE + 1];
226 for (_j = 1; _j <= DEGREE; ++_j) {
228 for (_r = 0; _r < _j; ++_r) {
229 _left =
_Left(span, _j - _r, u);
230 _right =
_Right(span, _r + 1, u);
233 _ndu[_j][_r] = _right + _left;
234 _temp = _ndu[_r][_j - 1] / _ndu[_j][_r];
237 _ndu[_r][_j] = _saved + _right * _temp;
238 _saved = _left * _temp;
240 _ndu[_j][_j] = _saved;
244 for (_j = 0; _j <= DEGREE; ++_j) ders[0][_j] = _ndu[_j][DEGREE];
247 for (_r = 0; _r <= DEGREE; ++_r) {
253 for (_k = 1; _k <= n; ++_k) {
259 _a[_s2][0] = _a[_s1][0] / _ndu[_pk + 1][_rk];
260 _d = _a[_s2][0] * _ndu[_rk][_pk];
273 for (_j = _j1; _j <= _j2; ++_j) {
275 (_a[_s1][_j] - _a[_s1][_j - 1]) / _ndu[_pk + 1][_rk + _j];
276 _d += _a[_s2][_j] * _ndu[_rk + _j][_pk];
280 _a[_s2][_k] = -_a[_s1][_k - 1] / _ndu[_pk + 1][_r];
281 _d += _a[_s2][_k] * _ndu[_r][_pk];
295 for (_k = 1; _k <= n; ++_k) {
296 for (_j = 0; _j <= DEGREE; ++_j) ders[_k][_j] *= _r;
302 for (_j = 0; _j <= DEGREE; ++_j)
delete[] _ndu[_j];
305 for (_j = 0; _j < 2; ++_j)
delete[] _a[_j];
331 for (_j = 1; _j <= DEGREE; ++_j) {
333 for (_r = 0; _r < _j; ++_r) {
334 _left =
_Left(span, _j - _r, u);
335 _right =
_Right(span, _r + 1, u);
337 if ((_right + _left) != 0) {
338 _temp = N[_r] / (_right + _left);
341 N[_r] = _saved + _right * _temp;
342 _saved = _left * _temp;
347 inline T
_Left(
int i,
int j, T u) {
return u -
Knots_[i + 1 - j]; }
355 for (
int i(
NumKnots_ - 2); i > -1; --i) {
366 int _mid = (_low + _high) >> 1;
373 _mid = (_low + _high) >> 1;
381 for (
int m(0); m < DIM; ++m) {
386 T** d_mat =
new T*[CONST_LEVEL_INI + 1];
388 for (
int i(0); i < CONST_LEVEL_INI + 1; ++i)
389 d_mat[i] =
new T[CONST_LEVEL_INI + 2];
395 for (
int j(1); j < CONST_LEVEL_INI + 1; ++j) {
396 for (
int k(0); k < DIM; ++k) {
397 ini_const[k] = init[j * DIM + k];
399 for (
int h(j); h > 0; --h) {
400 ini_const[k] -= d_mat[j][h - 1] *
CPoints_[h - 1][k];
402 CPoints_[j][k] = ini_const[k] / d_mat[j][j];
406 for (
int p(0); p < CONST_LEVEL_INI + 1; ++p)
delete[] d_mat[p];
410 T** c_mat =
new T*[CONST_LEVEL_FIN + 1];
412 for (
int i(0); i < CONST_LEVEL_FIN + 1; ++i)
413 c_mat[i] =
new T[CONST_LEVEL_FIN + 2];
419 for (
int j(
NumCPs_ - 2); j >
NumCPs_ - 2 - CONST_LEVEL_FIN; --j) {
420 for (
int k(0); k < DIM; ++k) {
421 ini_const[k] = fin[idx * DIM + k];
423 for (
int h(idx); h > 0; --h) {
427 CPoints_[j][k] = ini_const[k] / c_mat[idx][CONST_LEVEL_FIN + 1 - idx];
431 for (
int p(0); p < CONST_LEVEL_FIN + 1; ++p)
delete[] c_mat[p];
436 printf(
"%i th CP check:\n", i);
437 for (
int m(0); m < DIM; ++m) {
438 for (
int j(0); j <
NumCPs_; ++j) {
447 for (
int i(0); i < NUM_MIDDLE; ++i) {
448 for (
int m(0); m < DIM; ++m) {
449 CPoints_[CONST_LEVEL_INI + 1 + i][m] = middle_pt[i][m];
458 T
Knots_[DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN + 1];
459 T
CPoints_[NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN][DIM];
T _Right(int i, int j, T u)
bool _findSpan(int &ret, T u)
void _CalcConstrainedCPoints(T *init, T *fin, T Tf)
bool _BasisFunsDers(T **ders, int span, T u, int n)
void _CalcCPoints(T **middle_pt)
void _BasisFuns(T *N, int span, T u)
bool _CurveDerivsAlg1V(T **CK, T u, int d)
#define SP_IS_EQUAL(x, y)
void _BasisFuns(T *N, T u)
bool getCurvePoint(T u, T *ret)
T _Left(int i, int j, T u)
bool _BasisFunsDers(T **ders, T u, int n)
#define SP_SAFE_DELETE_AR(p)
bool getCurveDerPoint(T u, int d, T *ret)
bool SetParam(T *init, T *fin, T **middle_pt, T fin_time)
T Knots_[DEGREE+NUM_MIDDLE+2+CONST_LEVEL_INI+CONST_LEVEL_FIN+1]
T CPoints_[NUM_MIDDLE+2+CONST_LEVEL_INI+CONST_LEVEL_FIN][DIM]