39 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
41 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 void vpTemplateTrackerMIBSpline::PutPVBsplineD(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val,
const int °re)
48 PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
break;
50 PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);
54 void vpTemplateTrackerMIBSpline::PutPVBsplineD3(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
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++)
64 *pt++ +=Bspline3(ir-er)*Bspline3(it-et)*val;
69 void vpTemplateTrackerMIBSpline::PutPVBsplineD4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double val)
74 double *ptBti=&Bti[0];
75 for(
int it=-1;it<=2;it++)
77 *ptBti++ =Bspline4i(it-et,it);
80 double *pt=&Prt[(cr*Nc+ct)*16];
81 for(
int ir=-1;ir<=2;ir++)
83 Br=Bspline4i(ir-er,ir);
85 for(
int it=-1;it<=2;it++)
87 *pt++ +=Br* *ptBti++ *val;
92 double vpTemplateTrackerMIBSpline::Bspline3(
double diff)
95 double aDiff=std::fabs(diff);
104 return (-(aDiff * aDiff) + 0.75);
105 return (0.5*(1.5-aDiff)*(1.5-aDiff));
111 double vpTemplateTrackerMIBSpline::Bspline4i(
double diff,
int &interv)
116 return ((2.+diff)*(2.+diff)*(2.+diff)/6.);
118 return (-diff*diff*diff/2.-diff*diff+4./6.);
120 return (diff*diff*diff/2.-diff*diff+4./6.);
122 return ((2.-diff)*(2.-diff)*(2.-diff)/6.);
128 double vpTemplateTrackerMIBSpline::dBspline3(
double diff)
130 if((diff>-1.5)&&(diff<=-0.5))
132 else if((diff>-0.5)&&(diff<=0.5))
134 else if((diff>0.5)&&(diff<=1.5))
147 double vpTemplateTrackerMIBSpline::dBspline4(
double diff)
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.;
161 double vpTemplateTrackerMIBSpline::d2Bspline3(
double diff)
163 if((diff>-1.5)&&(diff<=-0.5))
165 else if((diff>-0.5)&&(diff<=0.5))
167 else if((diff>0.5)&&(diff<=1.5))
173 double vpTemplateTrackerMIBSpline::d2Bspline4(
double diff)
175 if((diff>-2.)&&(diff<=-1.))
177 else if((diff>-1.)&&(diff<=0.))
179 else if((diff>0.)&&(diff<=1.))
181 else if((diff>1.)&&(diff<=2.))
188 void vpTemplateTrackerMIBSpline::PutTotPVBspline(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
double *val,
unsigned int &NbParam,
int °ree)
193 PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
break;
195 PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
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 °ree)
204 PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
break;
206 PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
210 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
int cr,
double &er,
int ct,
double &et,
int Nc,
double *val,
unsigned int &NbParam)
214 if(er>0.5){sr=1;er=er-1;}
215 if(et>0.5){st=1;et=et-1;}
223 double *ptBti=&Bti[0];
224 double *ptdBti=&dBti[0];
225 double *ptd2Bti=&d2Bti[0];
227 for(
short int it=1;it>=-1;it--)
229 *ptBti++ =Bspline3(it+et);
230 *ptdBti++ =dBspline3(it+et);
231 *ptd2Bti++ =d2Bspline3(it+et);
234 double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+(
int)(NbParam+NbParam*NbParam))];
235 for(
short int ir=-1;ir<=1;++ir)
239 for(
short unsigned int it=0;it<=2;++it)
241 *pt++ += Br * (Bti[it]);
243 double v1 = Br * (dBti[it]);
244 for(
short unsigned int ip=0;ip<NbParam;++ip)
247 double v2 = Br * (d2Bti[it]) * val[ip];
248 for(
short unsigned int ip2=0;ip2<NbParam;++ip2)
249 *pt++ += v2*val[ip2];
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)
259 if(er>0.5){sr=1;er=er-1;}
260 if(et>0.5){st=1;et=et-1;}
268 double *ptBti=&Bti[0];
269 double *ptdBti=&dBti[0];
270 double *ptd2Bti=&d2Bti[0];
272 for(
short int it=1;it>=-1;it--)
274 *ptBti++ =Bspline3(it+et);
275 *ptdBti++ =dBspline3(it+et);
276 *ptd2Bti++ =d2Bspline3(it+et);
279 int NbParam_ = (int)NbParam;
280 for(
short int ir=-1;ir<=1;++ir)
283 short int irInd = ir + 1;
284 short int ind = (cr+sr+irInd)*Ncb;
285 for(
short int it=0;it<=2;++it)
287 Prt[ind+(ct+st+it)] += Br * (Bti[it]);
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)
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];
303 void vpTemplateTrackerMIBSpline::PutTotPVBspline3(
double *Prt,
double &er,
double *bt,
unsigned int size)
317 double *bt0 = &bt[0];
318 if(er>0.5){er=er-1.0;}
320 for(
int ir=-1;ir<=1;++ir)
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];
344 for (; bt < btend; *Prt++ += Br * *bt++) {};
350 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
int cr,
double er,
int ct,
double et,
int Nc,
double *val,
unsigned int &NbParam)
358 double *ptBti=&Bti[0];
359 double *ptdBti=&dBti[0];
360 double *ptd2Bti=&d2Bti[0];
361 for(
char it=-1;it<=2;it++)
363 *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
364 *ptdBti++ =dBspline4(-it+et);
365 *ptd2Bti++ =d2Bspline4(-it+et);
368 int NbParam_ = (int)NbParam;
370 double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
371 for(
char ir=-1;ir<=2;ir++)
373 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
377 for(
char it=-1;it<=2;it++)
380 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];
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)
401 double *ptBti=&Bti[0];
402 double *ptdBti=&dBti[0];
403 double *ptd2Bti=&d2Bti[0];
404 for(
char it=-1;it<=2;it++)
406 *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
407 *ptdBti++ =dBspline4(-it+et);
408 *ptd2Bti++ =d2Bspline4(-it+et);
411 int NbParam_ = (int)NbParam;
413 for(
int ir=-1;ir<=2;ir++)
415 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
417 int ind = (cr+irInd)*Ncb+ct;
423 for(
int it=-1;it<=2;it++)
425 Prt[ind+it] += Br * *ptBti;
426 int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
427 for(
int ip=0;ip<NbParam_;ip++)
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];
441 void vpTemplateTrackerMIBSpline::PutTotPVBspline4(
double *Prt,
double &er,
double *bt,
unsigned int size)
446 double *bt0 = &bt[0];
448 for(
int ir=-1;ir<=2;++ir)
450 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
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];
472 for (; bt < btend; *Prt++ += Br * *bt++) {};
478 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
double *val,
unsigned int &NbParam,
int °ree)
483 PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
break;
485 PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
489 void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
double *val,
unsigned int &NbParam,
int °ree)
494 PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
break;
496 PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
500 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
double *val,
unsigned int &NbParam)
504 if(er>0.5){sr=1;er=er-1;}
505 if(et>0.5){st=1;et=et-1;}
512 double *ptBti=&Bti[0];
513 double *ptdBti=&dBti[0];
514 for(
char it=-1;it<=1;it++)
516 *ptBti++ =Bspline3(-it+et);
517 *ptdBti++ =dBspline3(-it+et);
520 int NbParam_ = (int)NbParam;
522 double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+NbParam_+NbParam_*NbParam_)];
523 for(
char ir=-1;ir<=1;ir++)
528 for(
char it=-1;it<=1;it++)
531 for(
unsigned int ip=0;ip<NbParam;ip++)
533 *pt++ -=Br**ptdBti*val[ip];
543 void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
547 if(er>0.5){sr=1;er=er-1;}
548 if(et>0.5){st=1;et=et-1;}
555 double *ptBti=&Bti[0];
556 double *ptdBti=&dBti[0];
557 for(
char it=-1;it<=1;it++)
559 *ptBti++ =Bspline3(-it+et);
560 *ptdBti++ =dBspline3(-it+et);
563 int NbParam_ = (int)NbParam;
565 for(
char ir=-1;ir<=1;ir++)
570 int ind = (cr+sr+irInd)*Ncb;
574 for(
char it=-1;it<=1;it++)
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++)
580 dPrt[ind1+ip]-= Br**ptdBti*val[ip];
589 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
double *val,
unsigned int &NbParam)
596 for(
char it=-1;it<=2;it++)
598 Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
599 dBti[it+1] = dBspline4(-it+et);
602 int NbParam_ = (int)NbParam;
604 double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
605 for(
int ir=-1;ir<=2;ir++)
607 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
608 for(
int it=0;it<=3;it++)
610 (*pt++) +=Br*Bti[it];
612 for(
int ip=0;ip<NbParam_;ip++)
614 (*pt++) -=Br*dBti[it]*val[ip];
622 void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(
double *Prt,
double *dPrt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
double *val,
unsigned int &NbParam)
629 for(
char it=-1;it<=2;it++)
631 Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
632 dBti[it+1] = dBspline4(-it+et);
635 int NbParam_ = (int)NbParam;
637 for(
int ir=-1;ir<=2;ir++)
639 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
641 int ind = (cr+irInd)*Ncb+ct;
643 for(
int it=0;it<=3;it++)
645 Prt[ind+it] += Br*Bti[it];
646 int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
648 for(
int ip=0;ip<NbParam_;ip++)
650 dPrt[ind1+ip]-= Br*dBti[it]*val[ip];
656 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
unsigned int &NbParam,
int °ree)
661 PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
break;
663 PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
667 void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
unsigned int &NbParam)
672 if(er>0.5){sr=1;er=er-1;}
673 if(et>0.5){st=1;et=et-1;}
679 for(
char it=-1;it<=1;it++)
681 Bti[it+1] = Bspline3(-it+et);
684 int NbParam_ = (int)NbParam;
685 int NbParam_val = NbParam_+NbParam_*NbParam_;
687 double *pt=&PrtTout[(
unsigned int)(((cr+sr)*Nc+(ct+st))*9*(1+NbParam_val))];
688 for(
int ir=-1;ir<=1;ir++)
690 Br= Bspline3(-ir+er);
691 for(
int it=0;it<=2;it++)
693 (*pt++) +=Br*Bti[it];
698 void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(
double *PrtTout,
int &cr,
double &er,
int &ct,
double &et,
int &Nc,
unsigned int &NbParam)
705 for(
char it=-1;it<=2;it++)
707 Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
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++)
715 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
716 for(
int it=0;it<=3;it++)
718 (*pt++) +=Br*Bti[it];
724 void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb,
unsigned int &NbParam,
int °ree)
729 PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
break;
731 PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
735 void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
740 if(er>0.5){sr=1;er=er-1;}
741 if(et>0.5){st=1;et=et-1;}
747 for(
char it=-1;it<=1;it++)
749 Bti[it+1] = Bspline3(-it+et);
752 for(
int ir=-1;ir<=1;ir++)
754 Br= Bspline3(-ir+er);
757 int ind = (cr+sr+irInd)*Ncb;
758 for(
int it=0;it<=2;it++)
760 Prt[ind+(ct+st+it)] += Br*Bti[it];
766 void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(
double *Prt,
int &cr,
double &er,
int &ct,
double &et,
int &Ncb)
773 for(
char it=-1;it<=2;it++)
775 Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
778 for(
int ir=-1;ir<=2;ir++)
780 Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
782 int ind = (cr+irInd)*Ncb+ct;
784 for(
int it=0;it<=3;it++)
786 Prt[ind+it] += Br*Bti[it];