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