Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpTemplateTrackerMIBSpline.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
41 
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 
44 void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et, int Nc, double val,
45  const int &degre)
46 {
47  switch (degre) {
48  case 4:
49  PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
50  break;
51  default:
52  PutPVBsplineD3(Prt, cr, er, ct, et, Nc,
53  val); // std::cout<<"DEFAUT"<<std::endl;
54  }
55 }
56 
57 void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
58 {
59  int sr = 0;
60  int st = 0;
61  if (er > 0.5) {
62  sr = 1;
63  er = er - 1;
64  }
65  if (et > 0.5) {
66  st = 1;
67  et = et - 1;
68  }
69  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9];
70  for (int ir = -1; ir <= 1; ir++)
71  for (int it = -1; it <= 1; it++) {
72  *pt++ += Bspline3(ir - er) * Bspline3(it - et) * val;
73  // pt++;
74  }
75 }
76 
77 void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
78 {
79  double Bti[4];
80 
81  double *ptBti = &Bti[0];
82  for (int it = -1; it <= 2; it++) {
83  *ptBti++ = Bspline4i(it - et, it);
84  // pt++;
85  }
86  double *pt = &Prt[(cr * Nc + ct) * 16];
87  for (int ir = -1; ir <= 2; ir++) {
88  double Br = Bspline4i(ir - er, ir);
89  ptBti = &Bti[0];
90  for (int it = -1; it <= 2; it++) {
91  *pt++ += Br * *ptBti++ * val;
92  }
93  }
94 }
95 
96 double vpTemplateTrackerMIBSpline::Bspline3(double diff)
97 {
98  // double result;
99  double aDiff = std::fabs(diff);
100  // if(aDiff<0.5)
102  // return (-(aDiff * aDiff) + 0.75);
103  // else if(aDiff<1.5)
104  // return (0.5*(1.5-aDiff)*(1.5-aDiff));
105 
106  if (aDiff < 1.5) {
107  if (aDiff < 0.5)
108  return (-(aDiff * aDiff) + 0.75);
109  return (0.5 * (1.5 - aDiff) * (1.5 - aDiff));
110  }
111 
112  return 0;
113 }
114 
115 double vpTemplateTrackerMIBSpline::Bspline4i(double diff, int &interv)
116 {
117  switch (interv) {
118  case -1:
119  return ((2. + diff) * (2. + diff) * (2. + diff) / 6.);
120  case 0:
121  return (-diff * diff * diff / 2. - diff * diff + 4. / 6.);
122  case 1:
123  return (diff * diff * diff / 2. - diff * diff + 4. / 6.);
124  case 2:
125  return ((2. - diff) * (2. - diff) * (2. - diff) / 6.);
126  default:
127  return 0;
128  }
129 }
130 
131 double vpTemplateTrackerMIBSpline::dBspline3(double diff)
132 {
133  if ((diff > -1.5) && (diff <= -0.5))
134  return diff + 1.5;
135  else if ((diff > -0.5) && (diff <= 0.5))
136  return -2. * diff;
137  else if ((diff > 0.5) && (diff <= 1.5))
138  return diff - 1.5;
139 
140  // if(fabs(diff + 1.5) <= (-0.5 + 1.5))
141  // return diff+1.5;
142  // else if(fabs(diff + 0.5) <= (0.5 + 0.5))
143  // return -2.*diff;
144  // else if(fabs(diff - 0.5) <= (1.5 - 0.5))
145  // return diff-1.5;
146 
147  return 0;
148 }
149 
150 double vpTemplateTrackerMIBSpline::dBspline4(double diff)
151 {
152  if ((diff > -2.) && (diff <= -1.))
153  return (diff + 2.) * (diff + 2.) / 2.;
154  else if ((diff > -1.) && (diff <= 0.))
155  return -3. * diff * diff / 2. - 2. * diff;
156  else if ((diff > 0.) && (diff <= 1.))
157  return 3. * diff * diff / 2. - 2. * diff;
158  else if ((diff > 1.) && (diff <= 2.))
159  return -(diff - 2.) * (diff - 2.) / 2.;
160  else
161  return 0;
162 }
163 
164 double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
165 {
166  if ((diff > -1.5) && (diff <= -0.5))
167  return 1.;
168  else if ((diff > -0.5) && (diff <= 0.5))
169  return -2.;
170  else if ((diff > 0.5) && (diff <= 1.5))
171  return 1.;
172  else
173  return 0;
174 }
175 
176 double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
177 {
178  if ((diff > -2.) && (diff <= -1.))
179  return (diff + 2.);
180  else if ((diff > -1.) && (diff <= 0.))
181  return -3. * diff - 2.;
182  else if ((diff > 0.) && (diff <= 1.))
183  return 3. * diff - 2.;
184  else if ((diff > 1.) && (diff <= 2.))
185  return -(diff - 2.);
186  else
187  return 0;
188 }
189 
190 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc,
191  double *val, unsigned int &NbParam, int &degree)
192 {
193  switch (degree) {
194  case 4:
195  PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
196  break;
197  default:
198  PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
199  }
200 }
201 
202 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
203  double &et, int Ncb, double *val, unsigned int &NbParam, int &degree)
204 {
205  switch (degree) {
206  case 4:
207  PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
208  break;
209  default:
210  PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
211  }
212 }
213 
214 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc,
215  double *val, unsigned int &NbParam)
216 {
217  short int sr = 0;
218  short int st = 0;
219  if (er > 0.5) {
220  sr = 1;
221  er = er - 1;
222  }
223  if (et > 0.5) {
224  st = 1;
225  et = et - 1;
226  }
227 
228  double Bti[3];
229  double dBti[3];
230  double d2Bti[3];
231 
232  double *ptBti = &Bti[0];
233  double *ptdBti = &dBti[0];
234  double *ptd2Bti = &d2Bti[0];
235 
236  for (short int it = 1; it >= -1; it--) {
237  *ptBti++ = Bspline3(it + et);
238  *ptdBti++ = dBspline3(it + et);
239  *ptd2Bti++ = d2Bspline3(it + et);
240  }
241 
242  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + (int)(NbParam + NbParam * NbParam))];
243  for (short int ir = -1; ir <= 1; ++ir) {
244  double Br = Bspline3(-ir + er);
245 
246  for (short unsigned int it = 0; it <= 2; ++it) {
247  *pt++ += Br * (Bti[it]);
248 
249  double v1 = Br * (dBti[it]);
250  for (short unsigned int ip = 0; ip < NbParam; ++ip) {
251  *pt++ -= v1 * val[ip];
252  double v2 = Br * (d2Bti[it]) * val[ip];
253  for (short unsigned int ip2 = 0; ip2 < NbParam; ++ip2)
254  *pt++ += v2 * val[ip2];
255  }
256  }
257  }
258 }
259 
260 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
261  double &et, int Ncb, double *val, unsigned int &NbParam)
262 {
263  short int sr = 0;
264  short int st = 0;
265  if (er > 0.5) {
266  sr = 1;
267  er = er - 1;
268  }
269  if (et > 0.5) {
270  st = 1;
271  et = et - 1;
272  }
273 
274  double Bti[3];
275  double dBti[3];
276  double d2Bti[3];
277 
278  double *ptBti = &Bti[0];
279  double *ptdBti = &dBti[0];
280  double *ptd2Bti = &d2Bti[0];
281 
282  for (short int it = 1; it >= -1; it--) {
283  *ptBti++ = Bspline3(it + et);
284  *ptdBti++ = dBspline3(it + et);
285  *ptd2Bti++ = d2Bspline3(it + et);
286  }
287 
288  int NbParam_ = (int)NbParam;
289  for (short int ir = -1; ir <= 1; ++ir) {
290  double Br = Bspline3(-ir + er);
291  short int irInd = ir + 1;
292  short int ind = (cr + sr + irInd) * Ncb;
293  for (short int it = 0; it <= 2; ++it) {
294  Prt[ind + (ct + st + it)] += Br * (Bti[it]);
295 
296  double v1 = Br * (dBti[it]);
297  int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
298  for (int ip = 0; ip < NbParam_; ++ip) {
299  dPrt[ind1 + ip] -= v1 * val[ip];
300  double v2 = Br * (d2Bti[it]) * val[ip];
301  int ind2 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_ * NbParam_ + ip * NbParam_;
302  for (short int ip2 = 0; ip2 < NbParam_; ++ip2)
303  d2Prt[ind2 + ip2] += v2 * val[ip2];
304  }
305  }
306  }
307 }
308 
309 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
310 {
311 // double Br;
312 // for(short int ir=-1;ir<=1;++ir)
313 // {
314 // Br=Bspline3(-ir+er);
315 
316 // for(short unsigned int it=0;it < size;++it)
317 // *Prt++ += Br * bt[it];
318 // }
319 #define LSIZE 12
320 
321  double *bt0 = &bt[0];
322  if (er > 0.5) {
323  er = er - 1.0;
324  }
325 
326  for (int ir = -1; ir <= 1; ++ir) {
327  double Br = Bspline3(-ir + er);
328  const double *btend = bt0 + size;
329  bt = bt0;
330 
331  if (size >= LSIZE) {
332  btend -= LSIZE - 1;
333  for (; bt < btend; bt += LSIZE) {
334  *Prt++ += Br * bt[0];
335  *Prt++ += Br * bt[1];
336  *Prt++ += Br * bt[2];
337  *Prt++ += Br * bt[3];
338  *Prt++ += Br * bt[4];
339  *Prt++ += Br * bt[5];
340  *Prt++ += Br * bt[6];
341  *Prt++ += Br * bt[7];
342  *Prt++ += Br * bt[8];
343  *Prt++ += Br * bt[9];
344  *Prt++ += Br * bt[10];
345  *Prt++ += Br * bt[11];
346  }
347  btend += LSIZE - 1;
348  }
349  for (; bt < btend; *Prt++ += Br * *bt++) {
350  };
351  }
352 #undef LSIZE
353 }
354 
355 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc,
356  double *val, unsigned int &NbParam)
357 {
358  double Bti[4];
359  double dBti[4];
360  double d2Bti[4];
361 
362  double *ptBti = &Bti[0];
363  double *ptdBti = &dBti[0];
364  double *ptd2Bti = &d2Bti[0];
365  for (char it = -1; it <= 2; it++) {
366  *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
367  *ptdBti++ = dBspline4(-it + et);
368  *ptd2Bti++ = d2Bspline4(-it + et);
369  }
370 
371  int NbParam_ = (int)NbParam;
372 
373  double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
374  for (char ir = -1; ir <= 2; ir++) {
375  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
376  ptBti = &Bti[0];
377  ptdBti = &dBti[0];
378  ptd2Bti = &d2Bti[0];
379  for (char it = -1; it <= 2; it++) {
380  *pt++ += Br * *ptBti;
381  for (short int ip = 0; ip < NbParam_; ip++) {
382  *pt++ -= Br * *ptdBti * val[ip];
383  for (short int ip2 = 0; ip2 < NbParam_; ip2++)
384  *pt++ += Br * *ptd2Bti * val[ip] * val[ip2];
385  }
386  ptBti++;
387  ptdBti++;
388  ptd2Bti++;
389  }
390  }
391 }
392 
393 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct,
394  double et, int Ncb, double *val, unsigned int &NbParam)
395 {
396  double Bti[4];
397  double dBti[4];
398  double d2Bti[4];
399 
400  double *ptBti = &Bti[0];
401  double *ptdBti = &dBti[0];
402  double *ptd2Bti = &d2Bti[0];
403  for (char it = -1; it <= 2; it++) {
404  *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
405  *ptdBti++ = dBspline4(-it + et);
406  *ptd2Bti++ = d2Bspline4(-it + et);
407  }
408 
409  int NbParam_ = (int)NbParam;
410 
411  for (int ir = -1; ir <= 2; ir++) {
412  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
413  int irInd = ir + 1;
414  int ind = (cr + irInd) * Ncb + ct;
415 
416  ptBti = &Bti[0];
417  ptdBti = &dBti[0];
418  ptd2Bti = &d2Bti[0];
419 
420  for (int it = -1; it <= 2; it++) {
421  Prt[ind + it] += Br * *ptBti;
422  int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
423  for (int ip = 0; ip < NbParam_; ip++) {
424  dPrt[ind1 + ip] -= Br * *ptdBti * val[ip];
425  int ind2 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_ * NbParam_ + ip * NbParam_;
426  for (int ip2 = 0; ip2 < NbParam_; ip2++)
427  d2Prt[ind2 + ip2] += Br * *ptd2Bti * val[ip] * val[ip2];
428  }
429  ptBti++;
430  ptdBti++;
431  ptd2Bti++;
432  }
433  }
434 }
435 
436 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
437 {
438 #define LSIZE 12
439  double *bt0 = &bt[0];
440 
441  for (int ir = -1; ir <= 2; ++ir) {
442  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
443  const double *btend = bt0 + size;
444  bt = bt0;
445 
446  if (size >= LSIZE) {
447  btend -= LSIZE - 1;
448  for (; bt < btend; bt += LSIZE) {
449  *Prt++ += Br * bt[0];
450  *Prt++ += Br * bt[1];
451  *Prt++ += Br * bt[2];
452  *Prt++ += Br * bt[3];
453  *Prt++ += Br * bt[4];
454  *Prt++ += Br * bt[5];
455  *Prt++ += Br * bt[6];
456  *Prt++ += Br * bt[7];
457  *Prt++ += Br * bt[8];
458  *Prt++ += Br * bt[9];
459  *Prt++ += Br * bt[10];
460  *Prt++ += Br * bt[11];
461  }
462  btend += LSIZE - 1;
463  }
464  for (; bt < btend; *Prt++ += Br * *bt++) {
465  };
466  }
467 #undef LSIZE
468 }
469 
470 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
471  double *val, unsigned int &NbParam, int &degree)
472 {
473  switch (degree) {
474  case 4:
475  PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
476  break;
477  default:
478  PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
479  }
480 }
481 
482 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
483  double &et, int &Ncb, double *val, unsigned int &NbParam,
484  int &degree)
485 {
486  switch (degree) {
487  case 4:
488  PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
489  break;
490  default:
491  PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
492  }
493 }
494 
495 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
496  int &Nc, double *val, unsigned int &NbParam)
497 {
498  int sr = 0;
499  int st = 0;
500  if (er > 0.5) {
501  sr = 1;
502  er = er - 1;
503  }
504  if (et > 0.5) {
505  st = 1;
506  et = et - 1;
507  }
508 
509  double Bti[3];
510  double dBti[3];
511 
512  double *ptBti = &Bti[0];
513  double *ptdBti = &dBti[0];
514  for (char it = -1; it <= 1; it++) {
515  *ptBti++ = Bspline3(-it + et);
516  *ptdBti++ = dBspline3(-it + et);
517  }
518 
519  int NbParam_ = (int)NbParam;
520 
521  double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_ + NbParam_ * NbParam_)];
522  for (char ir = -1; ir <= 1; ir++) {
523  double Br = Bspline3(-ir + er);
524  ptBti = &Bti[0];
525  ptdBti = &dBti[0];
526  for (char it = -1; it <= 1; it++) {
527  *pt++ += Br * *ptBti;
528  for (unsigned int ip = 0; ip < NbParam; ip++) {
529  *pt++ -= Br * *ptdBti * val[ip];
530  pt = pt + NbParam; // Modif AY
531  }
532  // pt=pt+NbParam*NbParam; // Modif AY
533  ptBti++;
534  ptdBti++;
535  }
536  }
537 }
538 
539 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
540  double &et, int &Ncb, double *val, unsigned int &NbParam)
541 {
542  int sr = 0;
543  int st = 0;
544  if (er > 0.5) {
545  sr = 1;
546  er = er - 1;
547  }
548  if (et > 0.5) {
549  st = 1;
550  et = et - 1;
551  }
552 
553  double Bti[3];
554  double dBti[3];
555 
556  double *ptBti = &Bti[0];
557  double *ptdBti = &dBti[0];
558  for (char it = -1; it <= 1; it++) {
559  *ptBti++ = Bspline3(-it + et);
560  *ptdBti++ = dBspline3(-it + et);
561  }
562 
563  int NbParam_ = (int)NbParam;
564 
565  for (char ir = -1; ir <= 1; ir++) {
566  double Br = Bspline3(-ir + er);
567 
568  int irInd = ir + 1;
569  int ind = (cr + sr + irInd) * Ncb;
570 
571  ptBti = &Bti[0];
572  ptdBti = &dBti[0];
573  for (char it = -1; it <= 1; it++) {
574  Prt[ind + (ct + st + it)] += Br * *ptBti;
575  int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
576  for (short int ip = 0; ip < NbParam_; ip++) {
577  dPrt[ind1 + ip] -= Br * *ptdBti * val[ip];
578  }
579  ptBti++;
580  ptdBti++;
581  }
582  }
583 }
584 
585 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
586  int &Nc, double *val, unsigned int &NbParam)
587 {
588  double Bti[4] = {0, 0, 0, 0};
589  double dBti[4] = {0, 0, 0, 0};
590 
591  for (char it = -1; it <= 2; it++) {
592  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
593  dBti[it + 1] = dBspline4(-it + et);
594  }
595 
596  int NbParam_ = (int)NbParam;
597 
598  double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)];
599  for (int ir = -1; ir <= 2; ir++) {
600  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
601  for (int it = 0; it <= 3; it++) {
602  (*pt++) += Br * Bti[it];
603 
604  for (int ip = 0; ip < NbParam_; ip++) {
605  (*pt++) -= Br * dBti[it] * val[ip];
606  pt = pt + NbParam_; // Modif AY
607  }
608  // pt=pt+NbParam*NbParam; // Modif AY
609  }
610  }
611 }
612 
613 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct,
614  double &et, int &Ncb, double *val, unsigned int &NbParam)
615 {
616  double Bti[4] = {0, 0, 0, 0};
617  double dBti[4] = {0, 0, 0, 0};
618 
619  for (char it = -1; it <= 2; it++) {
620  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
621  dBti[it + 1] = dBspline4(-it + et);
622  }
623 
624  int NbParam_ = (int)NbParam;
625 
626  for (int ir = -1; ir <= 2; ir++) {
627  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
628  int irInd = ir + 1;
629  int ind = (cr + irInd) * Ncb + ct;
630 
631  for (int it = 0; it <= 3; it++) {
632  Prt[ind + it] += Br * Bti[it];
633  int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
634 
635  for (int ip = 0; ip < NbParam_; ip++) {
636  dPrt[ind1 + ip] -= Br * dBti[it] * val[ip];
637  }
638  }
639  }
640 }
641 
642 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
643  int &Nc, unsigned int &NbParam, int &degree)
644 {
645  switch (degree) {
646  case 4:
647  PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
648  break;
649  default:
650  PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
651  }
652 }
653 
654 void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
655  int &Nc, unsigned int &NbParam)
656 {
657  int sr = 0;
658  int st = 0;
659  if (er > 0.5) {
660  sr = 1;
661  er = er - 1;
662  }
663  if (et > 0.5) {
664  st = 1;
665  et = et - 1;
666  }
667 
668  double Bti[3] = {0, 0, 0};
669 
670  for (char it = -1; it <= 1; it++) {
671  Bti[it + 1] = Bspline3(-it + et);
672  }
673 
674  int NbParam_ = (int)NbParam;
675  int NbParam_val = NbParam_ + NbParam_ * NbParam_;
676 
677  double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_val))];
678  for (int ir = -1; ir <= 1; ir++) {
679  double Br = Bspline3(-ir + er);
680  for (int it = 0; it <= 2; it++) {
681  (*pt++) += Br * Bti[it];
682  pt = pt + NbParam_val;
683  }
684  }
685 }
686 void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
687  int &Nc, unsigned int &NbParam)
688 {
689  double Bti[4] = {0, 0, 0, 0};
690 
691  for (char it = -1; it <= 2; it++) {
692  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
693  }
694 
695  int NbParam_ = (int)NbParam;
696  int NbParam_val = NbParam_ + NbParam_ * NbParam_;
697  double *pt = &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))];
698  for (int ir = -1; ir <= 2; ir++) {
699  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
700  for (int it = 0; it <= 3; it++) {
701  (*pt++) += Br * Bti[it];
702  pt = pt + NbParam_val;
703  }
704  }
705 }
706 
707 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb,
708  unsigned int &NbParam, int &degree)
709 {
710  switch (degree) {
711  case 4:
712  PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
713  break;
714  default:
715  PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
716  }
717 }
718 
719 void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
720 {
721 
722  int sr = 0;
723  int st = 0;
724  if (er > 0.5) {
725  sr = 1;
726  er = er - 1;
727  }
728  if (et > 0.5) {
729  st = 1;
730  et = et - 1;
731  }
732 
733  double Bti[3] = {0, 0, 0};
734 
735  for (char it = -1; it <= 1; it++) {
736  Bti[it + 1] = Bspline3(-it + et);
737  }
738 
739  for (int ir = -1; ir <= 1; ir++) {
740  double Br = Bspline3(-ir + er);
741 
742  int irInd = ir + 1;
743  int ind = (cr + sr + irInd) * Ncb;
744  for (int it = 0; it <= 2; it++) {
745  Prt[ind + (ct + st + it)] += Br * Bti[it];
746  }
747  }
748 }
749 
750 void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
751 {
752  double Bti[4] = {0, 0, 0, 0};
753 
754  for (char it = -1; it <= 2; it++) {
755  Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
756  }
757 
758  for (int ir = -1; ir <= 2; ir++) {
759  double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
760  int irInd = ir + 1;
761  int ind = (cr + irInd) * Ncb + ct;
762 
763  for (int it = 0; it <= 3; it++) {
764  Prt[ind + it] += Br * Bti[it];
765  }
766  }
767 }
768 
769 #endif