Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpTemplateTrackerMIBSpline.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 *ptBti=&Bti[0];
74  for(int it=-1;it<=2;it++)
75  {
76  *ptBti++ =Bspline4i(it-et,it);
77  //pt++;
78  }
79  double *pt=&Prt[(cr*Nc+ct)*16];
80  for(int ir=-1;ir<=2;ir++)
81  {
82  double Br=Bspline4i(ir-er,ir);
83  ptBti=&Bti[0];
84  for(int it=-1;it<=2;it++)
85  {
86  *pt++ +=Br* *ptBti++ *val;
87  }
88  }
89 }
90 
91 double vpTemplateTrackerMIBSpline::Bspline3(double diff)
92 {
93  //double result;
94  double aDiff=std::fabs(diff);
95  // if(aDiff<0.5)
97  // return (-(aDiff * aDiff) + 0.75);
98  // else if(aDiff<1.5)
99  // return (0.5*(1.5-aDiff)*(1.5-aDiff));
100 
101  if(aDiff<1.5){
102  if(aDiff<0.5)
103  return (-(aDiff * aDiff) + 0.75);
104  return (0.5*(1.5-aDiff)*(1.5-aDiff));
105  }
106 
107  return 0;
108 }
109 
110 double vpTemplateTrackerMIBSpline::Bspline4i(double diff,int &interv)
111 {
112  switch(interv)
113  {
114  case -1:
115  return ((2.+diff)*(2.+diff)*(2.+diff)/6.);
116  case 0:
117  return (-diff*diff*diff/2.-diff*diff+4./6.);
118  case 1:
119  return (diff*diff*diff/2.-diff*diff+4./6.);
120  case 2:
121  return ((2.-diff)*(2.-diff)*(2.-diff)/6.);
122  default:
123  return 0;
124  }
125 }
126 
127 double vpTemplateTrackerMIBSpline::dBspline3(double diff)
128 {
129  if((diff>-1.5)&&(diff<=-0.5))
130  return diff+1.5;
131  else if((diff>-0.5)&&(diff<=0.5))
132  return -2.*diff;
133  else if((diff>0.5)&&(diff<=1.5))
134  return diff-1.5;
135 
136  // if(fabs(diff + 1.5) <= (-0.5 + 1.5))
137  // return diff+1.5;
138  // else if(fabs(diff + 0.5) <= (0.5 + 0.5))
139  // return -2.*diff;
140  // else if(fabs(diff - 0.5) <= (1.5 - 0.5))
141  // return diff-1.5;
142 
143  return 0;
144 }
145 
146 double vpTemplateTrackerMIBSpline::dBspline4(double diff)
147 {
148  if((diff>-2.)&&(diff<=-1.))
149  return (diff+2.)*(diff+2.)/2.;
150  else if((diff>-1.)&&(diff<=0.))
151  return -3.*diff*diff/2.-2.*diff;
152  else if((diff>0.)&&(diff<=1.))
153  return 3.*diff*diff/2.-2.*diff;
154  else if((diff>1.)&&(diff<=2.))
155  return -(diff-2.)*(diff-2.)/2.;
156  else
157  return 0;
158 }
159 
160 double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
161 {
162  if((diff>-1.5)&&(diff<=-0.5))
163  return 1.;
164  else if((diff>-0.5)&&(diff<=0.5))
165  return -2.;
166  else if((diff>0.5)&&(diff<=1.5))
167  return 1.;
168  else
169  return 0;
170 }
171 
172 double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
173 {
174  if((diff>-2.)&&(diff<=-1.))
175  return (diff+2.);
176  else if((diff>-1.)&&(diff<=0.))
177  return -3.*diff-2.;
178  else if((diff>0.)&&(diff<=1.))
179  return 3.*diff-2.;
180  else if((diff>1.)&&(diff<=2.))
181  return -(diff-2.);
182  else
183  return 0;
184 
185 }
186 
187 void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam, int &degree)
188 {
189  switch(degree)
190  {
191  case 4:
192  PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam); break;
193  default:
194  PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
195  }
196 }
197 
198 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)
199 {
200  switch(degree)
201  {
202  case 4:
203  PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam); break;
204  default:
205  PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
206  }
207 }
208 
209 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam)
210 {
211  short int sr=0;
212  short int st=0;
213  if(er>0.5){sr=1;er=er-1;}
214  if(et>0.5){st=1;et=et-1;}
215 
216  double Bti[3];
217  double dBti[3];
218  double d2Bti[3];
219 
220  double *ptBti=&Bti[0];
221  double *ptdBti=&dBti[0];
222  double *ptd2Bti=&d2Bti[0];
223 
224  for(short int it=1;it>=-1;it--)
225  {
226  *ptBti++ =Bspline3(it+et);
227  *ptdBti++ =dBspline3(it+et);
228  *ptd2Bti++ =d2Bspline3(it+et);
229  }
230 
231  double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+(int)(NbParam+NbParam*NbParam))];
232  for(short int ir=-1;ir<=1;++ir)
233  {
234  double Br=Bspline3(-ir+er);
235 
236  for(short unsigned int it=0;it<=2;++it)
237  {
238  *pt++ += Br * (Bti[it]);
239 
240  double v1 = Br * (dBti[it]);
241  for(short unsigned int ip=0;ip<NbParam;++ip)
242  {
243  *pt++ -= v1*val[ip];
244  double v2 = Br * (d2Bti[it]) * val[ip];
245  for(short unsigned int ip2=0;ip2<NbParam;++ip2)
246  *pt++ += v2*val[ip2];
247  }
248  }
249  }
250 }
251 
252 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam)
253 {
254  short int sr=0;
255  short int st=0;
256  if(er>0.5){sr=1;er=er-1;}
257  if(et>0.5){st=1;et=et-1;}
258 
259  double Bti[3];
260  double dBti[3];
261  double d2Bti[3];
262 
263  double *ptBti=&Bti[0];
264  double *ptdBti=&dBti[0];
265  double *ptd2Bti=&d2Bti[0];
266 
267  for(short int it=1;it>=-1;it--)
268  {
269  *ptBti++ =Bspline3(it+et);
270  *ptdBti++ =dBspline3(it+et);
271  *ptd2Bti++ =d2Bspline3(it+et);
272  }
273 
274  int NbParam_ = (int)NbParam;
275  for(short int ir=-1;ir<=1;++ir)
276  {
277  double Br=Bspline3(-ir+er);
278  short int irInd = ir + 1;
279  short int ind = (cr+sr+irInd)*Ncb;
280  for(short int it=0;it<=2;++it)
281  {
282  Prt[ind+(ct+st+it)] += Br * (Bti[it]);
283 
284  double v1 = Br * (dBti[it]);
285  int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
286  for (int ip=0;ip<NbParam_;++ip)
287  {
288  dPrt[ind1+ip]-= v1*val[ip];
289  double v2 = Br * (d2Bti[it]) * val[ip];
290  int ind2 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_*NbParam_+ip*NbParam_;
291  for(short int ip2=0;ip2<NbParam_;++ip2)
292  d2Prt[ind2+ip2] += v2*val[ip2];
293  }
294  }
295  }
296 }
297 
298 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
299 {
300  // double Br;
301  // for(short int ir=-1;ir<=1;++ir)
302  // {
303  // Br=Bspline3(-ir+er);
304 
305  // for(short unsigned int it=0;it < size;++it)
306  // *Prt++ += Br * bt[it];
307  // }
308 #define LSIZE 12
309 
310  double *bt0 = &bt[0];
311  if(er>0.5){er=er-1.0;}
312 
313  for(int ir=-1;ir<=1;++ir)
314  {
315  double Br=Bspline3(-ir+er);
316  const double *btend = bt0 + size;
317  bt = bt0;
318 
319  if (size >= LSIZE) {
320  btend -= LSIZE - 1;
321  for (; bt < btend; bt += LSIZE){
322  *Prt++ += Br * bt[0];
323  *Prt++ += Br * bt[1];
324  *Prt++ += Br * bt[2];
325  *Prt++ += Br * bt[3];
326  *Prt++ += Br * bt[4];
327  *Prt++ += Br * bt[5];
328  *Prt++ += Br * bt[6];
329  *Prt++ += Br * bt[7];
330  *Prt++ += Br * bt[8];
331  *Prt++ += Br * bt[9];
332  *Prt++ += Br * bt[10];
333  *Prt++ += Br * bt[11];
334  }
335  btend += LSIZE - 1;
336  }
337  for (; bt < btend; *Prt++ += Br * *bt++) {};
338 
339  }
340 #undef LSIZE
341 }
342 
343 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et,int Nc, double *val, unsigned int &NbParam)
344 {
345  double Bti[4];
346  double dBti[4];
347  double d2Bti[4];
348 
349  double *ptBti=&Bti[0];
350  double *ptdBti=&dBti[0];
351  double *ptd2Bti=&d2Bti[0];
352  for(char it=-1;it<=2;it++)
353  {
354  *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
355  *ptdBti++ =dBspline4(-it+et);
356  *ptd2Bti++ =d2Bspline4(-it+et);
357  }
358 
359  int NbParam_ = (int)NbParam;
360 
361  double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
362  for(char ir=-1;ir<=2;ir++)
363  {
364  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
365  ptBti=&Bti[0];
366  ptdBti=&dBti[0];
367  ptd2Bti=&d2Bti[0];
368  for(char it=-1;it<=2;it++)
369  {
370  *pt++ +=Br**ptBti;
371  for(short int ip=0;ip<NbParam_;ip++)
372  {
373  *pt++ -=Br**ptdBti*val[ip];
374  for(short int ip2=0;ip2<NbParam_;ip2++)
375  *pt++ +=Br**ptd2Bti*val[ip]*val[ip2];
376  }
377  ptBti++;
378  ptdBti++;
379  ptd2Bti++;
380  }
381  }
382 }
383 
384 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, double et,int Ncb, double *val, unsigned int &NbParam)
385 {
386  double Bti[4];
387  double dBti[4];
388  double d2Bti[4];
389 
390  double *ptBti=&Bti[0];
391  double *ptdBti=&dBti[0];
392  double *ptd2Bti=&d2Bti[0];
393  for(char it=-1;it<=2;it++)
394  {
395  *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
396  *ptdBti++ =dBspline4(-it+et);
397  *ptd2Bti++ =d2Bspline4(-it+et);
398  }
399 
400  int NbParam_ = (int)NbParam;
401 
402  for(int ir=-1;ir<=2;ir++)
403  {
404  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
405  int irInd = ir + 1;
406  int ind = (cr+irInd)*Ncb+ct;
407 
408  ptBti=&Bti[0];
409  ptdBti=&dBti[0];
410  ptd2Bti=&d2Bti[0];
411 
412  for(int it=-1;it<=2;it++)
413  {
414  Prt[ind+it] += Br * *ptBti;
415  int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
416  for(int ip=0;ip<NbParam_;ip++)
417  {
418  dPrt[ind1+ip]-= Br**ptdBti*val[ip];
419  int ind2 = ((cr+irInd)*Ncb+(ct+it))*NbParam_*NbParam_+ip*NbParam_;
420  for(int ip2=0;ip2<NbParam_;ip2++)
421  d2Prt[ind2+ip2] += Br**ptd2Bti*val[ip]*val[ip2];
422  }
423  ptBti++;
424  ptdBti++;
425  ptd2Bti++;
426  }
427  }
428 }
429 
430 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
431 {
432 #define LSIZE 12
433  double *bt0 = &bt[0];
434 
435  for(int ir=-1;ir<=2;++ir)
436  {
437  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
438  const double *btend = bt0 + size;
439  bt = bt0;
440 
441  if (size >= LSIZE) {
442  btend -= LSIZE - 1;
443  for (; bt < btend; bt += LSIZE){
444  *Prt++ += Br * bt[0];
445  *Prt++ += Br * bt[1];
446  *Prt++ += Br * bt[2];
447  *Prt++ += Br * bt[3];
448  *Prt++ += Br * bt[4];
449  *Prt++ += Br * bt[5];
450  *Prt++ += Br * bt[6];
451  *Prt++ += Br * bt[7];
452  *Prt++ += Br * bt[8];
453  *Prt++ += Br * bt[9];
454  *Prt++ += Br * bt[10];
455  *Prt++ += Br * bt[11];
456  }
457  btend += LSIZE - 1;
458  }
459  for (; bt < btend; *Prt++ += Br * *bt++) {};
460 
461  }
462 #undef LSIZE
463 }
464 
465 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam, int &degree)
466 {
467  switch(degree)
468  {
469  case 4:
470  PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam); break;
471  default:
472  PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
473  }
474 }
475 
476 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam, int &degree)
477 {
478  switch(degree)
479  {
480  case 4:
481  PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam); break;
482  default:
483  PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
484  }
485 }
486 
487 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam)
488 {
489  int sr=0;
490  int st=0;
491  if(er>0.5){sr=1;er=er-1;}
492  if(et>0.5){st=1;et=et-1;}
493 
494  double Bti[3];
495  double dBti[3];
496 
497  double *ptBti=&Bti[0];
498  double *ptdBti=&dBti[0];
499  for(char it=-1;it<=1;it++)
500  {
501  *ptBti++ =Bspline3(-it+et);
502  *ptdBti++ =dBspline3(-it+et);
503  }
504 
505  int NbParam_ = (int)NbParam;
506 
507  double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+NbParam_+NbParam_*NbParam_)];
508  for(char ir=-1;ir<=1;ir++)
509  {
510  double Br=Bspline3(-ir+er);
511  ptBti=&Bti[0];
512  ptdBti=&dBti[0];
513  for(char it=-1;it<=1;it++)
514  {
515  *pt++ +=Br**ptBti;
516  for(unsigned int ip=0;ip<NbParam;ip++)
517  {
518  *pt++ -=Br**ptdBti*val[ip];
519  pt=pt+NbParam; // Modif AY
520  }
521  // pt=pt+NbParam*NbParam; // Modif AY
522  ptBti++;
523  ptdBti++;
524  }
525  }
526 }
527 
528 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam)
529 {
530  int sr=0;
531  int st=0;
532  if(er>0.5){sr=1;er=er-1;}
533  if(et>0.5){st=1;et=et-1;}
534 
535  double Bti[3];
536  double dBti[3];
537 
538  double *ptBti=&Bti[0];
539  double *ptdBti=&dBti[0];
540  for(char it=-1;it<=1;it++)
541  {
542  *ptBti++ =Bspline3(-it+et);
543  *ptdBti++ =dBspline3(-it+et);
544  }
545 
546  int NbParam_ = (int)NbParam;
547 
548  for(char ir=-1;ir<=1;ir++)
549  {
550  double Br=Bspline3(-ir+er);
551 
552  int irInd = ir + 1;
553  int ind = (cr+sr+irInd)*Ncb;
554 
555  ptBti=&Bti[0];
556  ptdBti=&dBti[0];
557  for(char it=-1;it<=1;it++)
558  {
559  Prt[ind+(ct+st+it)] += Br**ptBti;
560  int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
561  for(short int ip=0;ip<NbParam_;ip++)
562  {
563  dPrt[ind1+ip]-= Br**ptdBti*val[ip];
564  }
565  ptBti++;
566  ptdBti++;
567  }
568  }
569 }
570 
571 
572 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam)
573 {
574  double Bti[4] = {0, 0, 0, 0};
575  double dBti[4] = {0, 0, 0, 0};
576 
577  for(char it=-1;it<=2;it++)
578  {
579  Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
580  dBti[it+1] = dBspline4(-it+et);
581  }
582 
583  int NbParam_ = (int)NbParam;
584 
585  double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
586  for(int ir=-1;ir<=2;ir++)
587  {
588  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
589  for(int it=0;it<=3;it++)
590  {
591  (*pt++) +=Br*Bti[it];
592 
593  for(int ip=0;ip<NbParam_;ip++)
594  {
595  (*pt++) -=Br*dBti[it]*val[ip];
596  pt=pt+NbParam_; // Modif AY
597  }
598  // pt=pt+NbParam*NbParam; // Modif AY
599  }
600  }
601 }
602 
603 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam)
604 {
605  double Bti[4] = {0, 0, 0, 0};
606  double dBti[4] = {0, 0, 0, 0};
607 
608  for(char it=-1;it<=2;it++)
609  {
610  Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
611  dBti[it+1] = dBspline4(-it+et);
612  }
613 
614  int NbParam_ = (int)NbParam;
615 
616  for(int ir=-1;ir<=2;ir++)
617  {
618  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
619  int irInd = ir + 1;
620  int ind = (cr+irInd)*Ncb+ct;
621 
622  for(int it=0;it<=3;it++)
623  {
624  Prt[ind+it] += Br*Bti[it];
625  int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
626 
627  for(int ip=0;ip<NbParam_;ip++)
628  {
629  dPrt[ind1+ip]-= Br*dBti[it]*val[ip];
630  }
631  }
632  }
633 }
634 
635 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam, int &degree)
636 {
637  switch(degree)
638  {
639  case 4:
640  PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam); break;
641  default:
642  PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
643  }
644 }
645 
646 void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
647 {
648  int sr=0;
649  int st=0;
650  if(er>0.5){sr=1;er=er-1;}
651  if(et>0.5){st=1;et=et-1;}
652 
653  double Bti[3] = {0, 0, 0};
654 
655  for(char it=-1;it<=1;it++)
656  {
657  Bti[it+1] = Bspline3(-it+et);
658  }
659 
660  int NbParam_ = (int)NbParam;
661  int NbParam_val = NbParam_+NbParam_*NbParam_;
662 
663  double *pt=&PrtTout[(unsigned int)(((cr+sr)*Nc+(ct+st))*9*(1+NbParam_val))];
664  for(int ir=-1;ir<=1;ir++)
665  {
666  double Br= Bspline3(-ir+er);
667  for(int it=0;it<=2;it++)
668  {
669  (*pt++) +=Br*Bti[it];
670  pt=pt+NbParam_val;
671  }
672  }
673 }
674 void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
675 {
676  double Bti[4] = {0, 0, 0, 0};
677 
678  for(char it=-1;it<=2;it++)
679  {
680  Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
681  }
682 
683  int NbParam_ = (int)NbParam;
684  int NbParam_val = NbParam_+NbParam_*NbParam_;
685  double *pt=&PrtTout[(unsigned int)((cr*Nc+ct)*16*(1+NbParam_val))];
686  for(int ir=-1;ir<=2;ir++)
687  {
688  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
689  for(int it=0;it<=3;it++)
690  {
691  (*pt++) +=Br*Bti[it];
692  pt=pt+NbParam_val;
693  }
694  }
695 }
696 
697 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb, unsigned int &NbParam, int &degree)
698 {
699  switch(degree)
700  {
701  case 4:
702  PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam); break;
703  default:
704  PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
705  }
706 }
707 
708 void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
709 {
710 
711  int sr=0;
712  int st=0;
713  if(er>0.5){sr=1;er=er-1;}
714  if(et>0.5){st=1;et=et-1;}
715 
716  double Bti[3] = {0, 0, 0};
717 
718  for(char it=-1;it<=1;it++)
719  {
720  Bti[it+1] = Bspline3(-it+et);
721  }
722 
723  for(int ir=-1;ir<=1;ir++)
724  {
725  double Br= Bspline3(-ir+er);
726 
727  int irInd = ir + 1;
728  int ind = (cr+sr+irInd)*Ncb;
729  for(int it=0;it<=2;it++)
730  {
731  Prt[ind+(ct+st+it)] += Br*Bti[it];
732  }
733  }
734 }
735 
736 
737 void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
738 {
739  double Bti[4] = {0, 0, 0, 0};
740 
741  for(char it=-1;it<=2;it++)
742  {
743  Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
744  }
745 
746  for(int ir=-1;ir<=2;ir++)
747  {
748  double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
749  int irInd = ir + 1;
750  int ind = (cr+irInd)*Ncb+ct;
751 
752  for(int it=0;it<=3;it++)
753  {
754  Prt[ind+it] += Br*Bti[it];
755  }
756  }
757 }
758 
759 #endif