1 #define IKFAST_HAS_LIBRARY
43 #ifndef __PRETTY_FUNCTION__
44 #define __PRETTY_FUNCTION__ __FUNCDNAME__
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __func__
52 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
57 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
59 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
62 #define IK2PI ((IkReal)6.28318530717959)
63 #define IKPI ((IkReal)3.14159265358979)
64 #define IKPI_2 ((IkReal)1.57079632679490)
80 #ifdef IKFAST_NAMESPACE
81 namespace IKFAST_NAMESPACE {
84 inline float IKabs(
float f) {
return fabsf(
f); }
85 inline double IKabs(
double f) {
return fabs(
f); }
90 inline float IKlog(
float f) {
return logf(
f); }
91 inline double IKlog(
double f) {
return log(
f); }
94 #ifndef IKFAST_SINCOS_THRESH
95 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
99 #ifndef IKFAST_ATAN2_MAGTHRESH
100 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
104 #ifndef IKFAST_SOLUTION_THRESH
105 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
109 #ifndef IKFAST_EVALCOND_THRESH
110 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
117 if(
f <= -1 )
return float(-
IKPI_2);
118 else if(
f >= 1 )
return float(
IKPI_2);
125 else if(
f >= 1 )
return IKPI_2;
150 if(
f <= -1 )
return float(
IKPI);
151 else if(
f >= 1 )
return float(0);
157 if(
f <= -1 )
return IKPI;
158 else if(
f >= 1 )
return 0;
161 inline float IKsin(
float f) {
return sinf(
f); }
162 inline double IKsin(
double f) {
return sin(
f); }
163 inline float IKcos(
float f) {
return cosf(
f); }
164 inline double IKcos(
double f) {
return cos(
f); }
165 inline float IKtan(
float f) {
return tanf(
f); }
166 inline double IKtan(
double f) {
return tan(
f); }
167 inline float IKsqrt(
float f) {
if(
f <= 0.0f )
return 0.0f;
return sqrtf(
f); }
168 inline double IKsqrt(
double f) {
if(
f <= 0.0 )
return 0.0;
return sqrt(
f); }
170 return atan2f(fy,fx);
177 else if( isnan(fx) ) {
180 return atan2f(fy,fx);
190 else if( isnan(fx) ) {
196 template <
typename T>
203 template <
typename T>
209 if( !isnan(fy) && !isnan(fx) ) {
238 template <
typename T>
257 ret.
value = (T)1.0e30;
266 int num = n > 0 ? n : -n;
270 else if( num == 3 ) {
292 IKFAST_API
void ComputeFk(
const IkReal* j, IkReal* eetrans, IkReal* eerot) {
293 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46;
326 x32=((((-1.0)*x31))+x24);
327 x33=((((-1.0)*x24))+x31);
328 x34=((((1.0)*x23))+(((1.0)*x28)));
331 x37=(x17*(((((-1.0)*x24))+x26)));
332 x38=(x12*(((((-1.0)*x24))+x26)));
333 x39=(x17*(((((-1.0)*x23))+(((-1.0)*x28)))));
334 x40=(x12*(((((-1.0)*x23))+(((-1.0)*x28)))));
338 x44=(((x0*x6))+((x40*x5)));
339 x45=(x30+((x25*x40)));
341 eerot[0]=(((x9*(((((-1.0)*x12*x6))+x42))))+((x8*((((x10*x37))+((x11*(((((-1.0)*x27))+((x25*x39)))))))))));
342 eerot[1]=((((-1.0)*x9*((((x18*x37))+(((1.0)*x11*(((((-1.0)*x12*x5))+(((-1.0)*x13*x39))))))))))+((x8*((x42+((x25*x7)))))));
343 eerot[2]=(((x10*((x43+x27))))+((x11*x37)));
344 IkReal x47=((1.0)*x24);
345 eetrans[0]=(((x21*x26))+((x0*x19))+((x11*(((((-1.0)*x22*x47))+((x0*x15*x2))))))+(((-1.0)*x21*x47))+((x10*((((x20*x39))+((x16*x5)))))));
346 eerot[3]=(((x44*x9))+((x8*((x46+x41)))));
347 eerot[4]=(((x9*(((((-1.0)*x41))+(((-1.0)*x46))))))+((x44*x8)));
348 eerot[5]=(((x10*(((((-1.0)*x30))+((x40*x6))))))+((x11*x38)));
349 IkReal x48=((1.0)*x24);
350 eetrans[1]=(((x19*x7))+((x14*x26))+((x10*((((x20*x40))+(((-1.0)*x22*x5))))))+((x11*(((((-1.0)*x16*x48))+((x15*x2*x7))))))+(((-1.0)*x14*x48)));
351 eerot[6]=(((x8*((((x10*x34))+((x35*x6))))))+((x36*x9)));
352 eerot[7]=(((x9*(((((-1.0)*x18*x34))+(((-1.0)*x13*x35))))))+((x36*x8)));
353 eerot[8]=(((x29*x33))+((x11*x34)));
354 eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11*(((((0.084)*x23))+((x15*x4))))))+(((0.307)*x23))+(((0.307)*x28)));
367 IkReal j0,
cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,
py,npy,new_pz,pz,npz,pp;
368 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
371 unsigned char _ij100[2], _nj100;
373 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
374 for(
int dummyiter = 0; dummyiter < 1; ++dummyiter) {
385 px = eetrans[0];
py = eetrans[1]; pz = eetrans[2];
390 new_px=(px+(((-0.084)*r02)));
394 new_py=(
py+(((-0.084)*r12)));
398 new_pz=((-0.2604)+pz+(((-0.084)*r22)));
399 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px;
py = new_py; pz = new_pz;
400 IkReal x49=((1.0)*px);
401 IkReal x50=((1.0)*pz);
402 IkReal x51=((1.0)*
py);
403 pp=((px*px)+(
py*
py)+(pz*pz));
404 npx=(((px*r00))+((
py*r10))+((pz*r20)));
405 npy=(((px*r01))+((
py*r11))+((pz*r21)));
406 npz=(((px*r02))+((
py*r12))+((pz*r22)));
407 rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
408 rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
409 rxp0_2=((((-1.0)*r10*x49))+((
py*r00)));
410 rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
411 rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
412 rxp1_2=((((-1.0)*r11*x49))+((
py*r01)));
413 rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
414 rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
415 rxp2_2=((((-1.0)*r12*x49))+((
py*r02)));
417 IkReal j2array[2], cj2array[2], sj2array[2];
418 bool j2valid[2]={
false};
420 cj2array[0]=((-1.00860400186133)+(((4.65332712889716)*pp)));
423 j2valid[0] = j2valid[1] =
true;
424 j2array[0] =
IKacos(cj2array[0]);
425 sj2array[0] =
IKsin(j2array[0]);
426 cj2array[1] = cj2array[0];
427 j2array[1] = -j2array[0];
428 sj2array[1] = -sj2array[0];
430 else if( isnan(cj2array[0]) )
434 cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
436 for(
int ij2 = 0; ij2 < 2; ++ij2)
442 _ij2[0] = ij2; _ij2[1] = -1;
443 for(
int iij2 = ij2+1; iij2 < 2; ++iij2)
447 j2valid[iij2]=
false; _ij2[1] = iij2;
break;
450 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
455 if(
IKabs(j0eval[0]) < 0.0000010000000000 )
459 j1eval[0]=((
IKabs(sj2))+(((3.25732899022801)*(
IKabs(((0.35)+(((0.307)*cj2))))))));
460 j1eval[1]=((1.29974853844603)+(sj2*sj2)+(cj2*cj2)+(((2.28013029315961)*cj2)));
461 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
468 IkReal j1array[2], cj1array[2], sj1array[2];
469 bool j1valid[2]={
false};
471 IkReal x52=((0.35)+(((0.307)*cj2)));
476 IkReal x53=((1.0)*(x55.
value));
477 if((((((0.094249)*(sj2*sj2)))+(x52*x52))) < -0.00001)
486 j1array[0]=(x54+(((-1.0)*x53)));
487 sj1array[0]=
IKsin(j1array[0]);
488 cj1array[0]=
IKcos(j1array[0]);
489 j1array[1]=((3.14159265358979)+(((-1.0)*x53))+(((-1.0)*x54)));
490 sj1array[1]=
IKsin(j1array[1]);
491 cj1array[1]=
IKcos(j1array[1]);
492 if( j1array[0] >
IKPI )
496 else if( j1array[0] < -
IKPI )
500 if( j1array[1] >
IKPI )
504 else if( j1array[1] < -
IKPI )
508 for(
int ij1 = 0; ij1 < 2; ++ij1)
514 _ij1[0] = ij1; _ij1[1] = -1;
515 for(
int iij1 = ij1+1; iij1 < 2; ++iij1)
519 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
522 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
529 for(
int ij0=1; ij0 < _nj0; ++ij0)
531 j0=(IkReal)(ij0 - 1) * (3.14159265358979) / 2.;
535 rotationfunction0(solutions);
548 IkReal j0array[2], cj0array[2], sj0array[2];
549 bool j0valid[2]={
false};
555 IkReal x1690=x1691.
value;
556 j0array[0]=((-1.0)*x1690);
557 sj0array[0]=
IKsin(j0array[0]);
558 cj0array[0]=
IKcos(j0array[0]);
559 j0array[1]=((3.14159265358979)+(((-1.0)*x1690)));
560 sj0array[1]=
IKsin(j0array[1]);
561 cj0array[1]=
IKcos(j0array[1]);
562 if( j0array[0] >
IKPI )
566 else if( j0array[0] < -
IKPI )
570 if( j0array[1] >
IKPI )
574 else if( j0array[1] < -
IKPI )
578 for(
int ij0 = 0; ij0 < 2; ++ij0)
584 _ij0[0] = ij0; _ij0[1] = -1;
585 for(
int iij0 = ij0+1; iij0 < 2; ++iij0)
589 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
592 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
596 IkReal x1692=((307000.0)*cj2);
597 IkReal x1693=(cj0*px);
598 IkReal x1694=((307000.0)*sj2);
599 IkReal x1695=(
py*sj0);
600 j1eval[0]=((-1.00860400186133)+(((-1.0)*cj2)));
601 j1eval[1]=((
IKabs(((((-1.0)*pz*x1692))+(((-350000.0)*pz))+((x1693*x1694))+((x1694*x1695)))))+(
IKabs(((((-1.0)*x1692*x1693))+(((-1.0)*x1692*x1695))+(((-1.0)*pz*x1694))+(((-350000.0)*x1693))+(((-350000.0)*x1695))))));
602 j1eval[2]=
IKsign(((-216749.0)+(((-214900.0)*cj2))));
603 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
607 IkReal x1696=(
py*sj0);
608 IkReal x1697=((1000.0)*pz);
609 IkReal x1698=(pz*sj2);
610 IkReal x1699=(cj0*px);
611 IkReal x1700=(cj2*x1699);
612 j1eval[0]=((((-1.0)*x1700))+x1698+(((-1.0)*cj2*x1696))+(((-1.1400651465798)*x1696))+(((-1.1400651465798)*x1699)));
613 j1eval[1]=((
IKabs(((((-1.0)*x1696*x1697))+(((-1.0)*x1697*x1699))+(((94.249)*cj2*sj2))+(((107.45)*sj2)))))+(
IKabs(((-122.5)+(((-94.249)*(cj2*cj2)))+(((-214.9)*cj2))+((pz*x1697))))));
614 j1eval[2]=
IKsign(((((-307.0)*x1700))+(((-350.0)*x1699))+(((-350.0)*x1696))+(((-307.0)*cj2*x1696))+(((307.0)*x1698))));
615 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
619 IkReal x1701=(cj0*px);
620 IkReal x1702=((1.0)*cj2);
621 IkReal x1703=((7000.0)*pz);
622 IkReal x1704=(
py*sj0);
623 IkReal x1705=((2149.0)*cj2);
624 IkReal x1706=(pz*sj2);
625 IkReal x1707=((3070.0)*pp);
626 j1eval[0]=((((-1.1400651465798)*x1704))+(((-1.1400651465798)*x1701))+(((-1.0)*x1702*x1704))+x1706+(((-1.0)*x1701*x1702)));
627 j1eval[1]=((
IKabs(((-98.8785)+(((-86.73057)*cj2))+(((-1.0)*cj2*x1707))+(((-3500.0)*pp))+((pz*x1703)))))+(
IKabs(((((-1.0)*x1703*x1704))+((sj2*x1707))+(((-1.0)*x1701*x1703))+(((86.73057)*sj2))))));
628 j1eval[2]=
IKsign(((((2149.0)*x1706))+(((-2450.0)*x1704))+(((-2450.0)*x1701))+(((-1.0)*x1704*x1705))+(((-1.0)*x1701*x1705))));
629 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
636 IkReal j1array[1], cj1array[1], sj1array[1];
637 bool j1valid[1]={
false};
639 IkReal x1708=((7000.0)*pz);
640 IkReal x1709=(cj0*px);
641 IkReal x1710=(
py*sj0);
642 IkReal x1711=((2149.0)*cj2);
643 IkReal x1712=((3070.0)*pp);
652 j1array[0]=((-1.5707963267949)+(x1713.
value)+(((1.5707963267949)*(x1714.
value))));
653 sj1array[0]=
IKsin(j1array[0]);
654 cj1array[0]=
IKcos(j1array[0]);
655 if( j1array[0] >
IKPI )
659 else if( j1array[0] < -
IKPI )
663 for(
int ij1 = 0; ij1 < 1; ++ij1)
669 _ij1[0] = ij1; _ij1[1] = -1;
670 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
674 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
677 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
680 IkReal x1715=
IKcos(j1);
681 IkReal x1716=
IKsin(j1);
682 IkReal x1717=(cj0*px);
683 IkReal x1718=(
py*sj0);
684 IkReal x1719=((0.307)*x1715);
685 IkReal x1720=((1.0)*x1718);
686 IkReal x1721=(pz*x1715);
687 IkReal x1722=((0.7)*x1716);
688 IkReal x1723=((0.307)*x1716);
689 evalcond[0]=((((-1.0)*pz))+((cj2*x1719))+(((0.35)*x1715))+((sj2*x1723)));
690 evalcond[1]=((((-1.0)*x1715*x1720))+(((-1.0)*x1715*x1717))+(((-0.307)*sj2))+((pz*x1716)));
691 evalcond[2]=((((-0.35)*x1716))+x1717+x1718+(((-1.0)*cj2*x1723))+((sj2*x1719)));
692 evalcond[3]=((-0.028251)+((x1718*x1722))+((x1717*x1722))+(((-1.0)*pp))+(((0.7)*x1721)));
693 evalcond[4]=((0.35)+(((-1.0)*x1716*x1717))+(((-1.0)*x1721))+(((-1.0)*x1716*x1720))+(((0.307)*cj2)));
700 rotationfunction0(solutions);
711 IkReal j1array[1], cj1array[1], sj1array[1];
712 bool j1valid[1]={
false};
714 IkReal x1724=((307.0)*cj2);
715 IkReal x1725=(cj0*px);
716 IkReal x1726=(
py*sj0);
717 IkReal x1727=((1000.0)*pz);
726 j1array[0]=((-1.5707963267949)+(x1728.
value)+(((1.5707963267949)*(x1729.
value))));
727 sj1array[0]=
IKsin(j1array[0]);
728 cj1array[0]=
IKcos(j1array[0]);
729 if( j1array[0] >
IKPI )
733 else if( j1array[0] < -
IKPI )
737 for(
int ij1 = 0; ij1 < 1; ++ij1)
743 _ij1[0] = ij1; _ij1[1] = -1;
744 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
748 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
751 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
754 IkReal x1730=
IKcos(j1);
755 IkReal x1731=
IKsin(j1);
756 IkReal x1732=(cj0*px);
757 IkReal x1733=(
py*sj0);
758 IkReal x1734=((0.307)*x1730);
759 IkReal x1735=((1.0)*x1733);
760 IkReal x1736=(pz*x1730);
761 IkReal x1737=((0.7)*x1731);
762 IkReal x1738=((0.307)*x1731);
763 evalcond[0]=(((cj2*x1734))+(((0.35)*x1730))+(((-1.0)*pz))+((sj2*x1738)));
764 evalcond[1]=(((pz*x1731))+(((-0.307)*sj2))+(((-1.0)*x1730*x1735))+(((-1.0)*x1730*x1732)));
765 evalcond[2]=(x1733+x1732+(((-1.0)*cj2*x1738))+(((-0.35)*x1731))+((sj2*x1734)));
766 evalcond[3]=((-0.028251)+(((-1.0)*pp))+(((0.7)*x1736))+((x1732*x1737))+((x1733*x1737)));
767 evalcond[4]=((0.35)+(((-1.0)*x1731*x1735))+(((-1.0)*x1731*x1732))+(((-1.0)*x1736))+(((0.307)*cj2)));
774 rotationfunction0(solutions);
785 IkReal j1array[1], cj1array[1], sj1array[1];
786 bool j1valid[1]={
false};
788 IkReal x1739=((307000.0)*cj2);
789 IkReal x1740=(cj0*px);
790 IkReal x1741=((307000.0)*sj2);
791 IkReal x1742=(
py*sj0);
800 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1743.
value)))+(x1744.
value));
801 sj1array[0]=
IKsin(j1array[0]);
802 cj1array[0]=
IKcos(j1array[0]);
803 if( j1array[0] >
IKPI )
807 else if( j1array[0] < -
IKPI )
811 for(
int ij1 = 0; ij1 < 1; ++ij1)
817 _ij1[0] = ij1; _ij1[1] = -1;
818 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
822 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
825 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
828 IkReal x1745=
IKcos(j1);
829 IkReal x1746=
IKsin(j1);
830 IkReal x1747=(cj0*px);
831 IkReal x1748=(
py*sj0);
832 IkReal x1749=((0.307)*x1745);
833 IkReal x1750=((1.0)*x1748);
834 IkReal x1751=(pz*x1745);
835 IkReal x1752=((0.7)*x1746);
836 IkReal x1753=((0.307)*x1746);
837 evalcond[0]=((((0.35)*x1745))+((cj2*x1749))+(((-1.0)*pz))+((sj2*x1753)));
838 evalcond[1]=(((pz*x1746))+(((-0.307)*sj2))+(((-1.0)*x1745*x1750))+(((-1.0)*x1745*x1747)));
839 evalcond[2]=(x1748+x1747+(((-1.0)*cj2*x1753))+((sj2*x1749))+(((-0.35)*x1746)));
840 evalcond[3]=((-0.028251)+((x1747*x1752))+(((-1.0)*pp))+((x1748*x1752))+(((0.7)*x1751)));
841 evalcond[4]=((0.35)+(((-1.0)*x1746*x1747))+(((-1.0)*x1746*x1750))+(((-1.0)*x1751))+(((0.307)*cj2)));
848 rotationfunction0(solutions);
867 for(
int rotationiter = 0; rotationiter < 1; ++rotationiter) {
868 IkReal x70=(r11*sj0);
869 IkReal x71=(cj0*r02);
870 IkReal x72=(r10*sj0);
871 IkReal x73=((1.0)*sj0);
872 IkReal x74=(cj2*sj1);
873 IkReal x75=(cj1*sj2);
874 IkReal x76=(r12*sj0);
875 IkReal x77=(((sj1*sj2))+((cj1*cj2)));
876 IkReal x78=(x75+(((-1.0)*x74)));
877 IkReal x79=(x74+(((-1.0)*x75)));
878 IkReal x80=(sj0*x79);
879 IkReal x81=(cj0*x79);
880 IkReal x82=(cj0*x77);
881 new_r00=(((r00*x82))+((x72*x77))+((r20*x78)));
882 new_r01=(((r21*x78))+((r01*x82))+((x70*x77)));
883 new_r02=(((r22*x78))+((x76*x77))+((x71*x77)));
884 new_r10=((((-1.0)*r00*x73))+((cj0*r10)));
885 new_r11=((((-1.0)*r01*x73))+((cj0*r11)));
886 new_r12=((((-1.0)*r02*x73))+((cj0*r12)));
887 new_r20=(((r00*x81))+((x72*x79))+((r20*x77)));
888 new_r21=(((r21*x77))+((r01*x81))+((x70*x79)));
889 new_r22=(((r22*x77))+((x76*x79))+((x71*x79)));
891 IkReal j4array[2], cj4array[2], sj4array[2];
892 bool j4valid[2]={
false};
897 j4valid[0] = j4valid[1] =
true;
898 j4array[0] =
IKacos(cj4array[0]);
899 sj4array[0] =
IKsin(j4array[0]);
900 cj4array[1] = cj4array[0];
901 j4array[1] = -j4array[0];
902 sj4array[1] = -sj4array[0];
904 else if( isnan(cj4array[0]) )
908 cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
910 for(
int ij4 = 0; ij4 < 2; ++ij4)
916 _ij4[0] = ij4; _ij4[1] = -1;
917 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
921 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
924 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
930 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
931 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
937 j3eval[2]=((
IKabs(new_r12))+(
IKabs(new_r02)));
938 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
944 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
948 bool bgotonextstatement =
true;
951 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
956 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
958 bgotonextstatement=
false;
964 j3=
IKatan2(((-1.0)*new_r01), new_r00);
966 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
967 vinfos[0].jointtype = 1;
968 vinfos[0].foffset = j0;
969 vinfos[0].indices[0] = _ij0[0];
970 vinfos[0].indices[1] = _ij0[1];
971 vinfos[0].maxsolutions = _nj0;
972 vinfos[1].jointtype = 1;
973 vinfos[1].foffset = j1;
974 vinfos[1].indices[0] = _ij1[0];
975 vinfos[1].indices[1] = _ij1[1];
976 vinfos[1].maxsolutions = _nj1;
977 vinfos[2].jointtype = 1;
978 vinfos[2].foffset = j2;
979 vinfos[2].indices[0] = _ij2[0];
980 vinfos[2].indices[1] = _ij2[1];
981 vinfos[2].maxsolutions = _nj2;
982 vinfos[3].jointtype = 1;
983 vinfos[3].foffset = j3;
984 vinfos[3].fmul = j3mul;
985 vinfos[3].freeind = 0;
986 vinfos[3].maxsolutions = 0;
987 vinfos[4].jointtype = 1;
988 vinfos[4].foffset = j4;
989 vinfos[4].indices[0] = _ij4[0];
990 vinfos[4].indices[1] = _ij4[1];
991 vinfos[4].maxsolutions = _nj4;
992 vinfos[5].jointtype = 1;
993 vinfos[5].foffset = j5;
994 vinfos[5].fmul = j5mul;
995 vinfos[5].freeind = 0;
996 vinfos[5].maxsolutions = 0;
997 std::vector<int> vfree(1);
1004 if( bgotonextstatement )
1006 bool bgotonextstatement =
true;
1009 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
1010 evalcond[1]=new_r02;
1011 evalcond[2]=new_r12;
1012 evalcond[3]=new_r21;
1013 evalcond[4]=new_r20;
1014 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
1016 bgotonextstatement=
false;
1022 j3=
IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
1024 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1025 vinfos[0].jointtype = 1;
1026 vinfos[0].foffset = j0;
1027 vinfos[0].indices[0] = _ij0[0];
1028 vinfos[0].indices[1] = _ij0[1];
1029 vinfos[0].maxsolutions = _nj0;
1030 vinfos[1].jointtype = 1;
1031 vinfos[1].foffset = j1;
1032 vinfos[1].indices[0] = _ij1[0];
1033 vinfos[1].indices[1] = _ij1[1];
1034 vinfos[1].maxsolutions = _nj1;
1035 vinfos[2].jointtype = 1;
1036 vinfos[2].foffset = j2;
1037 vinfos[2].indices[0] = _ij2[0];
1038 vinfos[2].indices[1] = _ij2[1];
1039 vinfos[2].maxsolutions = _nj2;
1040 vinfos[3].jointtype = 1;
1041 vinfos[3].foffset = j3;
1042 vinfos[3].fmul = j3mul;
1043 vinfos[3].freeind = 0;
1044 vinfos[3].maxsolutions = 0;
1045 vinfos[4].jointtype = 1;
1046 vinfos[4].foffset = j4;
1047 vinfos[4].indices[0] = _ij4[0];
1048 vinfos[4].indices[1] = _ij4[1];
1049 vinfos[4].maxsolutions = _nj4;
1050 vinfos[5].jointtype = 1;
1051 vinfos[5].foffset = j5;
1052 vinfos[5].fmul = j5mul;
1053 vinfos[5].freeind = 0;
1054 vinfos[5].maxsolutions = 0;
1055 std::vector<int> vfree(1);
1062 if( bgotonextstatement )
1064 bool bgotonextstatement =
true;
1067 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
1068 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1070 bgotonextstatement=
false;
1077 IkReal x83=new_r22*new_r22;
1078 IkReal x84=((16.0)*new_r10);
1079 IkReal x85=((16.0)*new_r01);
1080 IkReal x86=((16.0)*new_r22);
1081 IkReal x87=((8.0)*new_r11);
1082 IkReal x88=((8.0)*new_r00);
1083 IkReal x89=(x83*x84);
1084 IkReal x90=(x83*x85);
1085 j3eval[0]=((
IKabs((x89+(((-1.0)*x84)))))+(
IKabs((((new_r22*x88))+(((-1.0)*x83*x87)))))+(
IKabs(((((16.0)*new_r00))+((new_r11*x86))+(((-32.0)*new_r00*x83)))))+(
IKabs((x85+(((-1.0)*x90)))))+(
IKabs((((new_r22*x87))+(((-1.0)*x88)))))+(
IKabs(((((16.0)*new_r11*x83))+((new_r00*x86))+(((-32.0)*new_r11)))))+(
IKabs((x90+(((-1.0)*x85)))))+(
IKabs((x84+(((-1.0)*x89))))));
1086 if(
IKabs(j3eval[0]) < 0.0000000100000000 )
1092 IkReal op[4+1], zeror[4];
1094 IkReal j3evalpoly[1];
1095 IkReal x91=new_r22*new_r22;
1096 IkReal x92=((16.0)*new_r10);
1097 IkReal x93=(new_r11*new_r22);
1098 IkReal x94=(x91*x92);
1099 IkReal x95=((((8.0)*x93))+(((-8.0)*new_r00)));
1101 op[1]=(x92+(((-1.0)*x94)));
1102 op[2]=((((16.0)*x93))+(((16.0)*new_r00))+(((-32.0)*new_r00*x91)));
1103 op[3]=(x94+(((-1.0)*x92)));
1105 polyroots4(op,zeror,numroots);
1106 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1107 int numsolutions = 0;
1108 for(
int ij3 = 0; ij3 < numroots; ++ij3)
1110 IkReal htj3 = zeror[ij3];
1111 tempj3array[0]=((2.0)*(atan(htj3)));
1112 for(
int kj3 = 0; kj3 < 1; ++kj3)
1114 j3array[numsolutions] = tempj3array[kj3];
1115 if( j3array[numsolutions] >
IKPI )
1117 j3array[numsolutions]-=
IK2PI;
1119 else if( j3array[numsolutions] < -
IKPI )
1121 j3array[numsolutions]+=
IK2PI;
1123 sj3array[numsolutions] =
IKsin(j3array[numsolutions]);
1124 cj3array[numsolutions] =
IKcos(j3array[numsolutions]);
1128 bool j3valid[4]={
true,
true,
true,
true};
1130 for(
int ij3 = 0; ij3 < numsolutions; ++ij3)
1136 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1139 IkReal x96=((16.0)*new_r01);
1140 IkReal x97=new_r22*new_r22;
1141 IkReal x98=(new_r00*new_r22);
1142 IkReal x99=((8.0)*x98);
1143 IkReal x100=(new_r11*x97);
1144 IkReal x101=(x96*x97);
1145 IkReal x102=((8.0)*x100);
1146 j3evalpoly[0]=((((-1.0)*x102))+(((htj3*htj3*htj3)*(((((-1.0)*x101))+x96))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x102))+x99))))+((htj3*((x101+(((-1.0)*x96))))))+(((htj3*htj3)*(((((16.0)*x98))+(((16.0)*x100))+(((-32.0)*new_r11))))))+x99);
1147 if(
IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1151 _ij3[0] = ij3; _ij3[1] = -1;
1152 for(
int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1156 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
1165 IkReal x103=cj3*cj3;
1166 IkReal x104=(cj3*new_r22);
1167 IkReal x105=((-1.0)+x103+(((-1.0)*x103*(new_r22*new_r22))));
1169 j5eval[1]=((
IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x104)))))+(
IKabs((((new_r00*sj3))+((new_r01*x104))))));
1171 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
1180 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1189 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1193 bool bgotonextstatement =
true;
1196 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
1197 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1199 bgotonextstatement=
false;
1201 IkReal j5array[1], cj5array[1], sj5array[1];
1202 bool j5valid[1]={
false};
1206 j5array[0]=
IKatan2(new_r10, new_r11);
1207 sj5array[0]=
IKsin(j5array[0]);
1208 cj5array[0]=
IKcos(j5array[0]);
1209 if( j5array[0] >
IKPI )
1213 else if( j5array[0] < -
IKPI )
1214 { j5array[0]+=
IK2PI;
1217 for(
int ij5 = 0; ij5 < 1; ++ij5)
1223 _ij5[0] = ij5; _ij5[1] = -1;
1224 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1228 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1231 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1234 IkReal x106=
IKsin(j5);
1235 IkReal x107=
IKcos(j5);
1237 evalcond[1]=((-1.0)*x106);
1238 evalcond[2]=(x106+(((-1.0)*new_r10)));
1239 evalcond[3]=(x107+(((-1.0)*new_r11)));
1247 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1248 vinfos[0].jointtype = 1;
1249 vinfos[0].foffset = j0;
1250 vinfos[0].indices[0] = _ij0[0];
1251 vinfos[0].indices[1] = _ij0[1];
1252 vinfos[0].maxsolutions = _nj0;
1253 vinfos[1].jointtype = 1;
1254 vinfos[1].foffset = j1;
1255 vinfos[1].indices[0] = _ij1[0];
1256 vinfos[1].indices[1] = _ij1[1];
1257 vinfos[1].maxsolutions = _nj1;
1258 vinfos[2].jointtype = 1;
1259 vinfos[2].foffset = j2;
1260 vinfos[2].indices[0] = _ij2[0];
1261 vinfos[2].indices[1] = _ij2[1];
1262 vinfos[2].maxsolutions = _nj2;
1263 vinfos[3].jointtype = 1;
1264 vinfos[3].foffset = j3;
1265 vinfos[3].indices[0] = _ij3[0];
1266 vinfos[3].indices[1] = _ij3[1];
1267 vinfos[3].maxsolutions = _nj3;
1268 vinfos[4].jointtype = 1;
1269 vinfos[4].foffset = j4;
1270 vinfos[4].indices[0] = _ij4[0];
1271 vinfos[4].indices[1] = _ij4[1];
1272 vinfos[4].maxsolutions = _nj4;
1273 vinfos[5].jointtype = 1;
1274 vinfos[5].foffset = j5;
1275 vinfos[5].indices[0] = _ij5[0];
1276 vinfos[5].indices[1] = _ij5[1];
1277 vinfos[5].maxsolutions = _nj5;
1278 std::vector<int> vfree(0);
1286 if( bgotonextstatement )
1288 bool bgotonextstatement =
true;
1291 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1292 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1294 bgotonextstatement=
false;
1296 IkReal j5array[1], cj5array[1], sj5array[1];
1297 bool j5valid[1]={
false};
1301 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1302 sj5array[0]=
IKsin(j5array[0]);
1303 cj5array[0]=
IKcos(j5array[0]);
1304 if( j5array[0] >
IKPI )
1308 else if( j5array[0] < -
IKPI )
1309 { j5array[0]+=
IK2PI;
1312 for(
int ij5 = 0; ij5 < 1; ++ij5)
1318 _ij5[0] = ij5; _ij5[1] = -1;
1319 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1323 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1326 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1329 IkReal x108=
IKcos(j5);
1330 IkReal x109=
IKsin(j5);
1332 evalcond[1]=(x109+new_r10);
1333 evalcond[2]=(x108+new_r11);
1334 evalcond[3]=((-1.0)*x109);
1342 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1343 vinfos[0].jointtype = 1;
1344 vinfos[0].foffset = j0;
1345 vinfos[0].indices[0] = _ij0[0];
1346 vinfos[0].indices[1] = _ij0[1];
1347 vinfos[0].maxsolutions = _nj0;
1348 vinfos[1].jointtype = 1;
1349 vinfos[1].foffset = j1;
1350 vinfos[1].indices[0] = _ij1[0];
1351 vinfos[1].indices[1] = _ij1[1];
1352 vinfos[1].maxsolutions = _nj1;
1353 vinfos[2].jointtype = 1;
1354 vinfos[2].foffset = j2;
1355 vinfos[2].indices[0] = _ij2[0];
1356 vinfos[2].indices[1] = _ij2[1];
1357 vinfos[2].maxsolutions = _nj2;
1358 vinfos[3].jointtype = 1;
1359 vinfos[3].foffset = j3;
1360 vinfos[3].indices[0] = _ij3[0];
1361 vinfos[3].indices[1] = _ij3[1];
1362 vinfos[3].maxsolutions = _nj3;
1363 vinfos[4].jointtype = 1;
1364 vinfos[4].foffset = j4;
1365 vinfos[4].indices[0] = _ij4[0];
1366 vinfos[4].indices[1] = _ij4[1];
1367 vinfos[4].maxsolutions = _nj4;
1368 vinfos[5].jointtype = 1;
1369 vinfos[5].foffset = j5;
1370 vinfos[5].indices[0] = _ij5[0];
1371 vinfos[5].indices[1] = _ij5[1];
1372 vinfos[5].maxsolutions = _nj5;
1373 std::vector<int> vfree(0);
1381 if( bgotonextstatement )
1383 bool bgotonextstatement =
true;
1390 if((x110.
value) < -0.00001)
1393 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj3)))))+(
IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1394 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1396 bgotonextstatement=
false;
1403 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1405 sj3=
IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))));
1414 if((x111.
value) < -0.00001)
1417 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1418 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1421 IkReal j5array[1], cj5array[1], sj5array[1];
1422 bool j5valid[1]={
false};
1424 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1432 j5array[0]=
IKatan2((((gconst18*new_r10))+(((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x112.
value)));
1433 sj5array[0]=
IKsin(j5array[0]);
1434 cj5array[0]=
IKcos(j5array[0]);
1435 if( j5array[0] >
IKPI )
1439 else if( j5array[0] < -
IKPI )
1440 { j5array[0]+=
IK2PI;
1443 for(
int ij5 = 0; ij5 < 1; ++ij5)
1449 _ij5[0] = ij5; _ij5[1] = -1;
1450 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1454 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1457 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1460 IkReal x113=
IKcos(j5);
1461 IkReal x114=
IKsin(j5);
1462 IkReal x115=((1.0)*gconst18);
1463 if((((1.0)+(((-1.0)*gconst18*x115)))) < -0.00001)
1465 IkReal x116=
IKsqrt(((1.0)+(((-1.0)*gconst18*x115))));
1467 evalcond[1]=((-1.0)*x114);
1468 evalcond[2]=((((-1.0)*x113*x115))+new_r11);
1469 evalcond[3]=((((-1.0)*x114*x115))+new_r10);
1470 evalcond[4]=(((x113*x116))+new_r01);
1471 evalcond[5]=(((x114*x116))+new_r00);
1472 evalcond[6]=((((-1.0)*new_r10*x115))+x114+((new_r00*x116)));
1473 evalcond[7]=((((-1.0)*new_r11*x115))+x113+((new_r01*x116)));
1481 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1482 vinfos[0].jointtype = 1;
1483 vinfos[0].foffset = j0;
1484 vinfos[0].indices[0] = _ij0[0];
1485 vinfos[0].indices[1] = _ij0[1];
1486 vinfos[0].maxsolutions = _nj0;
1487 vinfos[1].jointtype = 1;
1488 vinfos[1].foffset = j1;
1489 vinfos[1].indices[0] = _ij1[0];
1490 vinfos[1].indices[1] = _ij1[1];
1491 vinfos[1].maxsolutions = _nj1;
1492 vinfos[2].jointtype = 1;
1493 vinfos[2].foffset = j2;
1494 vinfos[2].indices[0] = _ij2[0];
1495 vinfos[2].indices[1] = _ij2[1];
1496 vinfos[2].maxsolutions = _nj2;
1497 vinfos[3].jointtype = 1;
1498 vinfos[3].foffset = j3;
1499 vinfos[3].indices[0] = _ij3[0];
1500 vinfos[3].indices[1] = _ij3[1];
1501 vinfos[3].maxsolutions = _nj3;
1502 vinfos[4].jointtype = 1;
1503 vinfos[4].foffset = j4;
1504 vinfos[4].indices[0] = _ij4[0];
1505 vinfos[4].indices[1] = _ij4[1];
1506 vinfos[4].maxsolutions = _nj4;
1507 vinfos[5].jointtype = 1;
1508 vinfos[5].foffset = j5;
1509 vinfos[5].indices[0] = _ij5[0];
1510 vinfos[5].indices[1] = _ij5[1];
1511 vinfos[5].maxsolutions = _nj5;
1512 std::vector<int> vfree(0);
1521 IkReal j5array[1], cj5array[1], sj5array[1];
1522 bool j5valid[1]={
false};
1532 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.
value)))+(x118.
value));
1533 sj5array[0]=
IKsin(j5array[0]);
1534 cj5array[0]=
IKcos(j5array[0]);
1535 if( j5array[0] >
IKPI )
1539 else if( j5array[0] < -
IKPI )
1540 { j5array[0]+=
IK2PI;
1543 for(
int ij5 = 0; ij5 < 1; ++ij5)
1549 _ij5[0] = ij5; _ij5[1] = -1;
1550 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1554 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1557 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1560 IkReal x119=
IKcos(j5);
1561 IkReal x120=
IKsin(j5);
1562 IkReal x121=((1.0)*gconst18);
1563 if((((1.0)+(((-1.0)*gconst18*x121)))) < -0.00001)
1565 IkReal x122=
IKsqrt(((1.0)+(((-1.0)*gconst18*x121))));
1567 evalcond[1]=((-1.0)*x120);
1568 evalcond[2]=((((-1.0)*x119*x121))+new_r11);
1569 evalcond[3]=((((-1.0)*x120*x121))+new_r10);
1570 evalcond[4]=(new_r01+((x119*x122)));
1571 evalcond[5]=(((x120*x122))+new_r00);
1572 evalcond[6]=((((-1.0)*new_r10*x121))+((new_r00*x122))+x120);
1573 evalcond[7]=(((new_r01*x122))+x119+(((-1.0)*new_r11*x121)));
1581 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1582 vinfos[0].jointtype = 1;
1583 vinfos[0].foffset = j0;
1584 vinfos[0].indices[0] = _ij0[0];
1585 vinfos[0].indices[1] = _ij0[1];
1586 vinfos[0].maxsolutions = _nj0;
1587 vinfos[1].jointtype = 1;
1588 vinfos[1].foffset = j1;
1589 vinfos[1].indices[0] = _ij1[0];
1590 vinfos[1].indices[1] = _ij1[1];
1591 vinfos[1].maxsolutions = _nj1;
1592 vinfos[2].jointtype = 1;
1593 vinfos[2].foffset = j2;
1594 vinfos[2].indices[0] = _ij2[0];
1595 vinfos[2].indices[1] = _ij2[1];
1596 vinfos[2].maxsolutions = _nj2;
1597 vinfos[3].jointtype = 1;
1598 vinfos[3].foffset = j3;
1599 vinfos[3].indices[0] = _ij3[0];
1600 vinfos[3].indices[1] = _ij3[1];
1601 vinfos[3].maxsolutions = _nj3;
1602 vinfos[4].jointtype = 1;
1603 vinfos[4].foffset = j4;
1604 vinfos[4].indices[0] = _ij4[0];
1605 vinfos[4].indices[1] = _ij4[1];
1606 vinfos[4].maxsolutions = _nj4;
1607 vinfos[5].jointtype = 1;
1608 vinfos[5].foffset = j5;
1609 vinfos[5].indices[0] = _ij5[0];
1610 vinfos[5].indices[1] = _ij5[1];
1611 vinfos[5].maxsolutions = _nj5;
1612 std::vector<int> vfree(0);
1624 if( bgotonextstatement )
1626 bool bgotonextstatement =
true;
1633 if((x123.
value) < -0.00001)
1636 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.0)+(
IKsign(sj3)))))+(
IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1637 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1639 bgotonextstatement=
false;
1646 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1648 sj3=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))));
1652 j3=((-1.0)*(
IKacos(gconst18)));
1657 if((x124.
value) < -0.00001)
1660 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1661 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1664 IkReal j5array[1], cj5array[1], sj5array[1];
1665 bool j5valid[1]={
false};
1667 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1675 j5array[0]=
IKatan2((((gconst18*new_r10))+((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x125.
value)));
1676 sj5array[0]=
IKsin(j5array[0]);
1677 cj5array[0]=
IKcos(j5array[0]);
1678 if( j5array[0] >
IKPI )
1682 else if( j5array[0] < -
IKPI )
1683 { j5array[0]+=
IK2PI;
1686 for(
int ij5 = 0; ij5 < 1; ++ij5)
1692 _ij5[0] = ij5; _ij5[1] = -1;
1693 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1697 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1700 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1703 IkReal x126=
IKcos(j5);
1704 IkReal x127=
IKsin(j5);
1705 IkReal x128=((1.0)*gconst18);
1706 if((((1.0)+(((-1.0)*gconst18*x128)))) < -0.00001)
1708 IkReal x129=
IKsqrt(((1.0)+(((-1.0)*gconst18*x128))));
1709 IkReal x130=((1.0)*x129);
1711 evalcond[1]=((-1.0)*x127);
1712 evalcond[2]=((((-1.0)*x126*x128))+new_r11);
1713 evalcond[3]=(new_r10+(((-1.0)*x127*x128)));
1714 evalcond[4]=((((-1.0)*x126*x130))+new_r01);
1715 evalcond[5]=(new_r00+(((-1.0)*x127*x130)));
1716 evalcond[6]=((((-1.0)*new_r10*x128))+(((-1.0)*new_r00*x130))+x127);
1717 evalcond[7]=((((-1.0)*new_r01*x130))+x126+(((-1.0)*new_r11*x128)));
1725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1726 vinfos[0].jointtype = 1;
1727 vinfos[0].foffset = j0;
1728 vinfos[0].indices[0] = _ij0[0];
1729 vinfos[0].indices[1] = _ij0[1];
1730 vinfos[0].maxsolutions = _nj0;
1731 vinfos[1].jointtype = 1;
1732 vinfos[1].foffset = j1;
1733 vinfos[1].indices[0] = _ij1[0];
1734 vinfos[1].indices[1] = _ij1[1];
1735 vinfos[1].maxsolutions = _nj1;
1736 vinfos[2].jointtype = 1;
1737 vinfos[2].foffset = j2;
1738 vinfos[2].indices[0] = _ij2[0];
1739 vinfos[2].indices[1] = _ij2[1];
1740 vinfos[2].maxsolutions = _nj2;
1741 vinfos[3].jointtype = 1;
1742 vinfos[3].foffset = j3;
1743 vinfos[3].indices[0] = _ij3[0];
1744 vinfos[3].indices[1] = _ij3[1];
1745 vinfos[3].maxsolutions = _nj3;
1746 vinfos[4].jointtype = 1;
1747 vinfos[4].foffset = j4;
1748 vinfos[4].indices[0] = _ij4[0];
1749 vinfos[4].indices[1] = _ij4[1];
1750 vinfos[4].maxsolutions = _nj4;
1751 vinfos[5].jointtype = 1;
1752 vinfos[5].foffset = j5;
1753 vinfos[5].indices[0] = _ij5[0];
1754 vinfos[5].indices[1] = _ij5[1];
1755 vinfos[5].maxsolutions = _nj5;
1756 std::vector<int> vfree(0);
1765 IkReal j5array[1], cj5array[1], sj5array[1];
1766 bool j5valid[1]={
false};
1776 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x131.
value)))+(x132.
value));
1777 sj5array[0]=
IKsin(j5array[0]);
1778 cj5array[0]=
IKcos(j5array[0]);
1779 if( j5array[0] >
IKPI )
1783 else if( j5array[0] < -
IKPI )
1784 { j5array[0]+=
IK2PI;
1787 for(
int ij5 = 0; ij5 < 1; ++ij5)
1793 _ij5[0] = ij5; _ij5[1] = -1;
1794 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1798 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1801 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1804 IkReal x133=
IKcos(j5);
1805 IkReal x134=
IKsin(j5);
1806 IkReal x135=((1.0)*gconst18);
1807 if((((1.0)+(((-1.0)*gconst18*x135)))) < -0.00001)
1809 IkReal x136=
IKsqrt(((1.0)+(((-1.0)*gconst18*x135))));
1810 IkReal x137=((1.0)*x136);
1812 evalcond[1]=((-1.0)*x134);
1813 evalcond[2]=((((-1.0)*x133*x135))+new_r11);
1814 evalcond[3]=((((-1.0)*x134*x135))+new_r10);
1815 evalcond[4]=((((-1.0)*x133*x137))+new_r01);
1816 evalcond[5]=((((-1.0)*x134*x137))+new_r00);
1817 evalcond[6]=((((-1.0)*new_r10*x135))+(((-1.0)*new_r00*x137))+x134);
1818 evalcond[7]=((((-1.0)*new_r11*x135))+(((-1.0)*new_r01*x137))+x133);
1826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1827 vinfos[0].jointtype = 1;
1828 vinfos[0].foffset = j0;
1829 vinfos[0].indices[0] = _ij0[0];
1830 vinfos[0].indices[1] = _ij0[1];
1831 vinfos[0].maxsolutions = _nj0;
1832 vinfos[1].jointtype = 1;
1833 vinfos[1].foffset = j1;
1834 vinfos[1].indices[0] = _ij1[0];
1835 vinfos[1].indices[1] = _ij1[1];
1836 vinfos[1].maxsolutions = _nj1;
1837 vinfos[2].jointtype = 1;
1838 vinfos[2].foffset = j2;
1839 vinfos[2].indices[0] = _ij2[0];
1840 vinfos[2].indices[1] = _ij2[1];
1841 vinfos[2].maxsolutions = _nj2;
1842 vinfos[3].jointtype = 1;
1843 vinfos[3].foffset = j3;
1844 vinfos[3].indices[0] = _ij3[0];
1845 vinfos[3].indices[1] = _ij3[1];
1846 vinfos[3].maxsolutions = _nj3;
1847 vinfos[4].jointtype = 1;
1848 vinfos[4].foffset = j4;
1849 vinfos[4].indices[0] = _ij4[0];
1850 vinfos[4].indices[1] = _ij4[1];
1851 vinfos[4].maxsolutions = _nj4;
1852 vinfos[5].jointtype = 1;
1853 vinfos[5].foffset = j5;
1854 vinfos[5].indices[0] = _ij5[0];
1855 vinfos[5].indices[1] = _ij5[1];
1856 vinfos[5].maxsolutions = _nj5;
1857 std::vector<int> vfree(0);
1869 if( bgotonextstatement )
1871 bool bgotonextstatement =
true;
1878 if((x138.
value) < -0.00001)
1881 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj3)))))+(
IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
1882 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1884 bgotonextstatement=
false;
1891 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1893 sj3=
IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))));
1902 if((x139.
value) < -0.00001)
1905 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1906 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1909 IkReal j5array[1], cj5array[1], sj5array[1];
1910 bool j5valid[1]={
false};
1912 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1920 j5array[0]=
IKatan2((((gconst19*new_r10))+(((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))), (new_r11*(x140.
value)));
1921 sj5array[0]=
IKsin(j5array[0]);
1922 cj5array[0]=
IKcos(j5array[0]);
1923 if( j5array[0] >
IKPI )
1927 else if( j5array[0] < -
IKPI )
1928 { j5array[0]+=
IK2PI;
1931 for(
int ij5 = 0; ij5 < 1; ++ij5)
1937 _ij5[0] = ij5; _ij5[1] = -1;
1938 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1942 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1945 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1948 IkReal x141=
IKcos(j5);
1949 IkReal x142=
IKsin(j5);
1950 IkReal x143=((1.0)*gconst19);
1951 if((((1.0)+(((-1.0)*gconst19*x143)))) < -0.00001)
1953 IkReal x144=
IKsqrt(((1.0)+(((-1.0)*gconst19*x143))));
1955 evalcond[1]=((-1.0)*x142);
1956 evalcond[2]=((((-1.0)*x141*x143))+new_r11);
1957 evalcond[3]=((((-1.0)*x142*x143))+new_r10);
1958 evalcond[4]=(((x141*x144))+new_r01);
1959 evalcond[5]=(((x142*x144))+new_r00);
1960 evalcond[6]=(((new_r00*x144))+x142+(((-1.0)*new_r10*x143)));
1961 evalcond[7]=(((new_r01*x144))+(((-1.0)*new_r11*x143))+x141);
1969 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1970 vinfos[0].jointtype = 1;
1971 vinfos[0].foffset = j0;
1972 vinfos[0].indices[0] = _ij0[0];
1973 vinfos[0].indices[1] = _ij0[1];
1974 vinfos[0].maxsolutions = _nj0;
1975 vinfos[1].jointtype = 1;
1976 vinfos[1].foffset = j1;
1977 vinfos[1].indices[0] = _ij1[0];
1978 vinfos[1].indices[1] = _ij1[1];
1979 vinfos[1].maxsolutions = _nj1;
1980 vinfos[2].jointtype = 1;
1981 vinfos[2].foffset = j2;
1982 vinfos[2].indices[0] = _ij2[0];
1983 vinfos[2].indices[1] = _ij2[1];
1984 vinfos[2].maxsolutions = _nj2;
1985 vinfos[3].jointtype = 1;
1986 vinfos[3].foffset = j3;
1987 vinfos[3].indices[0] = _ij3[0];
1988 vinfos[3].indices[1] = _ij3[1];
1989 vinfos[3].maxsolutions = _nj3;
1990 vinfos[4].jointtype = 1;
1991 vinfos[4].foffset = j4;
1992 vinfos[4].indices[0] = _ij4[0];
1993 vinfos[4].indices[1] = _ij4[1];
1994 vinfos[4].maxsolutions = _nj4;
1995 vinfos[5].jointtype = 1;
1996 vinfos[5].foffset = j5;
1997 vinfos[5].indices[0] = _ij5[0];
1998 vinfos[5].indices[1] = _ij5[1];
1999 vinfos[5].maxsolutions = _nj5;
2000 std::vector<int> vfree(0);
2009 IkReal j5array[1], cj5array[1], sj5array[1];
2010 bool j5valid[1]={
false};
2020 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x145.
value)))+(x146.
value));
2021 sj5array[0]=
IKsin(j5array[0]);
2022 cj5array[0]=
IKcos(j5array[0]);
2023 if( j5array[0] >
IKPI )
2027 else if( j5array[0] < -
IKPI )
2028 { j5array[0]+=
IK2PI;
2031 for(
int ij5 = 0; ij5 < 1; ++ij5)
2037 _ij5[0] = ij5; _ij5[1] = -1;
2038 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2042 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2045 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2048 IkReal x147=
IKcos(j5);
2049 IkReal x148=
IKsin(j5);
2050 IkReal x149=((1.0)*gconst19);
2051 if((((1.0)+(((-1.0)*gconst19*x149)))) < -0.00001)
2053 IkReal x150=
IKsqrt(((1.0)+(((-1.0)*gconst19*x149))));
2055 evalcond[1]=((-1.0)*x148);
2056 evalcond[2]=((((-1.0)*x147*x149))+new_r11);
2057 evalcond[3]=((((-1.0)*x148*x149))+new_r10);
2058 evalcond[4]=(((x147*x150))+new_r01);
2059 evalcond[5]=(((x148*x150))+new_r00);
2060 evalcond[6]=(((new_r00*x150))+x148+(((-1.0)*new_r10*x149)));
2061 evalcond[7]=(((new_r01*x150))+(((-1.0)*new_r11*x149))+x147);
2069 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2070 vinfos[0].jointtype = 1;
2071 vinfos[0].foffset = j0;
2072 vinfos[0].indices[0] = _ij0[0];
2073 vinfos[0].indices[1] = _ij0[1];
2074 vinfos[0].maxsolutions = _nj0;
2075 vinfos[1].jointtype = 1;
2076 vinfos[1].foffset = j1;
2077 vinfos[1].indices[0] = _ij1[0];
2078 vinfos[1].indices[1] = _ij1[1];
2079 vinfos[1].maxsolutions = _nj1;
2080 vinfos[2].jointtype = 1;
2081 vinfos[2].foffset = j2;
2082 vinfos[2].indices[0] = _ij2[0];
2083 vinfos[2].indices[1] = _ij2[1];
2084 vinfos[2].maxsolutions = _nj2;
2085 vinfos[3].jointtype = 1;
2086 vinfos[3].foffset = j3;
2087 vinfos[3].indices[0] = _ij3[0];
2088 vinfos[3].indices[1] = _ij3[1];
2089 vinfos[3].maxsolutions = _nj3;
2090 vinfos[4].jointtype = 1;
2091 vinfos[4].foffset = j4;
2092 vinfos[4].indices[0] = _ij4[0];
2093 vinfos[4].indices[1] = _ij4[1];
2094 vinfos[4].maxsolutions = _nj4;
2095 vinfos[5].jointtype = 1;
2096 vinfos[5].foffset = j5;
2097 vinfos[5].indices[0] = _ij5[0];
2098 vinfos[5].indices[1] = _ij5[1];
2099 vinfos[5].maxsolutions = _nj5;
2100 std::vector<int> vfree(0);
2112 if( bgotonextstatement )
2114 bool bgotonextstatement =
true;
2121 if((x151.
value) < -0.00001)
2124 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.0)+(
IKsign(sj3)))))+(
IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
2125 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2127 bgotonextstatement=
false;
2134 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2136 sj3=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))));
2140 j3=((-1.0)*(
IKacos(gconst19)));
2145 if((x152.
value) < -0.00001)
2148 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
2149 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
2152 IkReal j5array[1], cj5array[1], sj5array[1];
2153 bool j5valid[1]={
false};
2155 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2163 j5array[0]=
IKatan2((((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))), (new_r11*(x153.
value)));
2164 sj5array[0]=
IKsin(j5array[0]);
2165 cj5array[0]=
IKcos(j5array[0]);
2166 if( j5array[0] >
IKPI )
2170 else if( j5array[0] < -
IKPI )
2171 { j5array[0]+=
IK2PI;
2174 for(
int ij5 = 0; ij5 < 1; ++ij5)
2180 _ij5[0] = ij5; _ij5[1] = -1;
2181 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2185 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2188 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2191 IkReal x154=
IKcos(j5);
2192 IkReal x155=
IKsin(j5);
2193 IkReal x156=((1.0)*gconst19);
2194 if((((1.0)+(((-1.0)*gconst19*x156)))) < -0.00001)
2196 IkReal x157=
IKsqrt(((1.0)+(((-1.0)*gconst19*x156))));
2197 IkReal x158=((1.0)*x157);
2199 evalcond[1]=((-1.0)*x155);
2200 evalcond[2]=((((-1.0)*x154*x156))+new_r11);
2201 evalcond[3]=((((-1.0)*x155*x156))+new_r10);
2202 evalcond[4]=((((-1.0)*x154*x158))+new_r01);
2203 evalcond[5]=((((-1.0)*x155*x158))+new_r00);
2204 evalcond[6]=(x155+(((-1.0)*new_r10*x156))+(((-1.0)*new_r00*x158)));
2205 evalcond[7]=(x154+(((-1.0)*new_r11*x156))+(((-1.0)*new_r01*x158)));
2213 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2214 vinfos[0].jointtype = 1;
2215 vinfos[0].foffset = j0;
2216 vinfos[0].indices[0] = _ij0[0];
2217 vinfos[0].indices[1] = _ij0[1];
2218 vinfos[0].maxsolutions = _nj0;
2219 vinfos[1].jointtype = 1;
2220 vinfos[1].foffset = j1;
2221 vinfos[1].indices[0] = _ij1[0];
2222 vinfos[1].indices[1] = _ij1[1];
2223 vinfos[1].maxsolutions = _nj1;
2224 vinfos[2].jointtype = 1;
2225 vinfos[2].foffset = j2;
2226 vinfos[2].indices[0] = _ij2[0];
2227 vinfos[2].indices[1] = _ij2[1];
2228 vinfos[2].maxsolutions = _nj2;
2229 vinfos[3].jointtype = 1;
2230 vinfos[3].foffset = j3;
2231 vinfos[3].indices[0] = _ij3[0];
2232 vinfos[3].indices[1] = _ij3[1];
2233 vinfos[3].maxsolutions = _nj3;
2234 vinfos[4].jointtype = 1;
2235 vinfos[4].foffset = j4;
2236 vinfos[4].indices[0] = _ij4[0];
2237 vinfos[4].indices[1] = _ij4[1];
2238 vinfos[4].maxsolutions = _nj4;
2239 vinfos[5].jointtype = 1;
2240 vinfos[5].foffset = j5;
2241 vinfos[5].indices[0] = _ij5[0];
2242 vinfos[5].indices[1] = _ij5[1];
2243 vinfos[5].maxsolutions = _nj5;
2244 std::vector<int> vfree(0);
2253 IkReal j5array[1], cj5array[1], sj5array[1];
2254 bool j5valid[1]={
false};
2264 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x159.
value)))+(x160.
value));
2265 sj5array[0]=
IKsin(j5array[0]);
2266 cj5array[0]=
IKcos(j5array[0]);
2267 if( j5array[0] >
IKPI )
2271 else if( j5array[0] < -
IKPI )
2272 { j5array[0]+=
IK2PI;
2275 for(
int ij5 = 0; ij5 < 1; ++ij5)
2281 _ij5[0] = ij5; _ij5[1] = -1;
2282 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2286 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2289 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2292 IkReal x161=
IKcos(j5);
2293 IkReal x162=
IKsin(j5);
2294 IkReal x163=((1.0)*gconst19);
2295 if((((1.0)+(((-1.0)*gconst19*x163)))) < -0.00001)
2297 IkReal x164=
IKsqrt(((1.0)+(((-1.0)*gconst19*x163))));
2298 IkReal x165=((1.0)*x164);
2300 evalcond[1]=((-1.0)*x162);
2301 evalcond[2]=((((-1.0)*x161*x163))+new_r11);
2302 evalcond[3]=((((-1.0)*x162*x163))+new_r10);
2303 evalcond[4]=((((-1.0)*x161*x165))+new_r01);
2304 evalcond[5]=((((-1.0)*x162*x165))+new_r00);
2305 evalcond[6]=((((-1.0)*new_r00*x165))+(((-1.0)*new_r10*x163))+x162);
2306 evalcond[7]=((((-1.0)*new_r11*x163))+x161+(((-1.0)*new_r01*x165)));
2314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2315 vinfos[0].jointtype = 1;
2316 vinfos[0].foffset = j0;
2317 vinfos[0].indices[0] = _ij0[0];
2318 vinfos[0].indices[1] = _ij0[1];
2319 vinfos[0].maxsolutions = _nj0;
2320 vinfos[1].jointtype = 1;
2321 vinfos[1].foffset = j1;
2322 vinfos[1].indices[0] = _ij1[0];
2323 vinfos[1].indices[1] = _ij1[1];
2324 vinfos[1].maxsolutions = _nj1;
2325 vinfos[2].jointtype = 1;
2326 vinfos[2].foffset = j2;
2327 vinfos[2].indices[0] = _ij2[0];
2328 vinfos[2].indices[1] = _ij2[1];
2329 vinfos[2].maxsolutions = _nj2;
2330 vinfos[3].jointtype = 1;
2331 vinfos[3].foffset = j3;
2332 vinfos[3].indices[0] = _ij3[0];
2333 vinfos[3].indices[1] = _ij3[1];
2334 vinfos[3].maxsolutions = _nj3;
2335 vinfos[4].jointtype = 1;
2336 vinfos[4].foffset = j4;
2337 vinfos[4].indices[0] = _ij4[0];
2338 vinfos[4].indices[1] = _ij4[1];
2339 vinfos[4].maxsolutions = _nj4;
2340 vinfos[5].jointtype = 1;
2341 vinfos[5].foffset = j5;
2342 vinfos[5].indices[0] = _ij5[0];
2343 vinfos[5].indices[1] = _ij5[1];
2344 vinfos[5].maxsolutions = _nj5;
2345 std::vector<int> vfree(0);
2357 if( bgotonextstatement )
2359 bool bgotonextstatement =
true;
2364 bgotonextstatement=
false;
2369 if( bgotonextstatement )
2383 IkReal j5array[1], cj5array[1], sj5array[1];
2384 bool j5valid[1]={
false};
2386 IkReal x166=(new_r00*sj3);
2393 j5array[0]=
IKatan2(((((-1.0)*x166))+((cj3*new_r10))), ((x167.
value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2394 sj5array[0]=
IKsin(j5array[0]);
2395 cj5array[0]=
IKcos(j5array[0]);
2396 if( j5array[0] >
IKPI )
2400 else if( j5array[0] < -
IKPI )
2401 { j5array[0]+=
IK2PI;
2404 for(
int ij5 = 0; ij5 < 1; ++ij5)
2410 _ij5[0] = ij5; _ij5[1] = -1;
2411 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2415 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2418 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2420 IkReal evalcond[10];
2421 IkReal x168=
IKsin(j5);
2422 IkReal x169=
IKcos(j5);
2423 IkReal x170=((1.0)*new_r11);
2424 IkReal x171=(new_r22*sj3);
2425 IkReal x172=((1.0)*new_r10);
2426 IkReal x173=((1.0)*cj3*new_r22);
2427 IkReal x174=(sj3*x168);
2428 IkReal x175=(new_r22*x168);
2429 IkReal x176=((1.0)*x169);
2430 IkReal x177=((1.0)*x168);
2431 evalcond[0]=(((new_r00*sj3))+x168+(((-1.0)*cj3*x172)));
2432 evalcond[1]=(((new_r01*sj3))+x169+(((-1.0)*cj3*x170)));
2433 evalcond[2]=(((new_r11*sj3))+x175+((cj3*new_r01)));
2434 evalcond[3]=(((sj3*x169))+((cj3*x175))+new_r01);
2435 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x176))+((cj3*new_r00)));
2436 evalcond[5]=(x174+new_r00+(((-1.0)*x169*x173)));
2437 evalcond[6]=(((x168*x171))+(((-1.0)*cj3*x176))+new_r11);
2438 evalcond[7]=(x169+(((-1.0)*x171*x172))+(((-1.0)*new_r00*x173)));
2439 evalcond[8]=((((-1.0)*cj3*x177))+(((-1.0)*x171*x176))+new_r10);
2440 evalcond[9]=((((-1.0)*x177))+(((-1.0)*x170*x171))+(((-1.0)*new_r01*x173)));
2448 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2449 vinfos[0].jointtype = 1;
2450 vinfos[0].foffset = j0;
2451 vinfos[0].indices[0] = _ij0[0];
2452 vinfos[0].indices[1] = _ij0[1];
2453 vinfos[0].maxsolutions = _nj0;
2454 vinfos[1].jointtype = 1;
2455 vinfos[1].foffset = j1;
2456 vinfos[1].indices[0] = _ij1[0];
2457 vinfos[1].indices[1] = _ij1[1];
2458 vinfos[1].maxsolutions = _nj1;
2459 vinfos[2].jointtype = 1;
2460 vinfos[2].foffset = j2;
2461 vinfos[2].indices[0] = _ij2[0];
2462 vinfos[2].indices[1] = _ij2[1];
2463 vinfos[2].maxsolutions = _nj2;
2464 vinfos[3].jointtype = 1;
2465 vinfos[3].foffset = j3;
2466 vinfos[3].indices[0] = _ij3[0];
2467 vinfos[3].indices[1] = _ij3[1];
2468 vinfos[3].maxsolutions = _nj3;
2469 vinfos[4].jointtype = 1;
2470 vinfos[4].foffset = j4;
2471 vinfos[4].indices[0] = _ij4[0];
2472 vinfos[4].indices[1] = _ij4[1];
2473 vinfos[4].maxsolutions = _nj4;
2474 vinfos[5].jointtype = 1;
2475 vinfos[5].foffset = j5;
2476 vinfos[5].indices[0] = _ij5[0];
2477 vinfos[5].indices[1] = _ij5[1];
2478 vinfos[5].maxsolutions = _nj5;
2479 std::vector<int> vfree(0);
2492 IkReal j5array[1], cj5array[1], sj5array[1];
2493 bool j5valid[1]={
false};
2495 IkReal x178=((1.0)*new_r01);
2502 j5array[0]=
IKatan2(((x179.
value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))), (((cj3*new_r11))+(((-1.0)*sj3*x178))));
2503 sj5array[0]=
IKsin(j5array[0]);
2504 cj5array[0]=
IKcos(j5array[0]);
2505 if( j5array[0] >
IKPI )
2509 else if( j5array[0] < -
IKPI )
2510 { j5array[0]+=
IK2PI;
2513 for(
int ij5 = 0; ij5 < 1; ++ij5)
2519 _ij5[0] = ij5; _ij5[1] = -1;
2520 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2524 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2527 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2529 IkReal evalcond[10];
2530 IkReal x180=
IKsin(j5);
2531 IkReal x181=
IKcos(j5);
2532 IkReal x182=((1.0)*new_r11);
2533 IkReal x183=(new_r22*sj3);
2534 IkReal x184=((1.0)*new_r10);
2535 IkReal x185=((1.0)*cj3*new_r22);
2536 IkReal x186=(sj3*x180);
2537 IkReal x187=(new_r22*x180);
2538 IkReal x188=((1.0)*x181);
2539 IkReal x189=((1.0)*x180);
2540 evalcond[0]=(((new_r00*sj3))+x180+(((-1.0)*cj3*x184)));
2541 evalcond[1]=(((new_r01*sj3))+x181+(((-1.0)*cj3*x182)));
2542 evalcond[2]=(((new_r11*sj3))+x187+((cj3*new_r01)));
2543 evalcond[3]=(((sj3*x181))+((cj3*x187))+new_r01);
2544 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x188))+((cj3*new_r00)));
2545 evalcond[5]=(x186+new_r00+(((-1.0)*x181*x185)));
2546 evalcond[6]=(((x180*x183))+new_r11+(((-1.0)*cj3*x188)));
2547 evalcond[7]=(x181+(((-1.0)*new_r00*x185))+(((-1.0)*x183*x184)));
2548 evalcond[8]=(new_r10+(((-1.0)*x183*x188))+(((-1.0)*cj3*x189)));
2549 evalcond[9]=((((-1.0)*x189))+(((-1.0)*new_r01*x185))+(((-1.0)*x182*x183)));
2557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2558 vinfos[0].jointtype = 1;
2559 vinfos[0].foffset = j0;
2560 vinfos[0].indices[0] = _ij0[0];
2561 vinfos[0].indices[1] = _ij0[1];
2562 vinfos[0].maxsolutions = _nj0;
2563 vinfos[1].jointtype = 1;
2564 vinfos[1].foffset = j1;
2565 vinfos[1].indices[0] = _ij1[0];
2566 vinfos[1].indices[1] = _ij1[1];
2567 vinfos[1].maxsolutions = _nj1;
2568 vinfos[2].jointtype = 1;
2569 vinfos[2].foffset = j2;
2570 vinfos[2].indices[0] = _ij2[0];
2571 vinfos[2].indices[1] = _ij2[1];
2572 vinfos[2].maxsolutions = _nj2;
2573 vinfos[3].jointtype = 1;
2574 vinfos[3].foffset = j3;
2575 vinfos[3].indices[0] = _ij3[0];
2576 vinfos[3].indices[1] = _ij3[1];
2577 vinfos[3].maxsolutions = _nj3;
2578 vinfos[4].jointtype = 1;
2579 vinfos[4].foffset = j4;
2580 vinfos[4].indices[0] = _ij4[0];
2581 vinfos[4].indices[1] = _ij4[1];
2582 vinfos[4].maxsolutions = _nj4;
2583 vinfos[5].jointtype = 1;
2584 vinfos[5].foffset = j5;
2585 vinfos[5].indices[0] = _ij5[0];
2586 vinfos[5].indices[1] = _ij5[1];
2587 vinfos[5].maxsolutions = _nj5;
2588 std::vector<int> vfree(0);
2601 IkReal j5array[1], cj5array[1], sj5array[1];
2602 bool j5valid[1]={
false};
2604 IkReal x190=cj3*cj3;
2605 IkReal x191=(cj3*new_r22);
2614 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.
value)))+(x193.
value));
2615 sj5array[0]=
IKsin(j5array[0]);
2616 cj5array[0]=
IKcos(j5array[0]);
2617 if( j5array[0] >
IKPI )
2621 else if( j5array[0] < -
IKPI )
2622 { j5array[0]+=
IK2PI;
2625 for(
int ij5 = 0; ij5 < 1; ++ij5)
2631 _ij5[0] = ij5; _ij5[1] = -1;
2632 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2636 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2639 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2641 IkReal evalcond[10];
2642 IkReal x194=
IKsin(j5);
2643 IkReal x195=
IKcos(j5);
2644 IkReal x196=((1.0)*new_r11);
2645 IkReal x197=(new_r22*sj3);
2646 IkReal x198=((1.0)*new_r10);
2647 IkReal x199=((1.0)*cj3*new_r22);
2648 IkReal x200=(sj3*x194);
2649 IkReal x201=(new_r22*x194);
2650 IkReal x202=((1.0)*x195);
2651 IkReal x203=((1.0)*x194);
2652 evalcond[0]=(((new_r00*sj3))+x194+(((-1.0)*cj3*x198)));
2653 evalcond[1]=(((new_r01*sj3))+x195+(((-1.0)*cj3*x196)));
2654 evalcond[2]=(((new_r11*sj3))+x201+((cj3*new_r01)));
2655 evalcond[3]=(((sj3*x195))+((cj3*x201))+new_r01);
2656 evalcond[4]=((((-1.0)*new_r22*x202))+((new_r10*sj3))+((cj3*new_r00)));
2657 evalcond[5]=((((-1.0)*x195*x199))+x200+new_r00);
2658 evalcond[6]=(((x194*x197))+new_r11+(((-1.0)*cj3*x202)));
2659 evalcond[7]=((((-1.0)*x197*x198))+x195+(((-1.0)*new_r00*x199)));
2660 evalcond[8]=((((-1.0)*x197*x202))+new_r10+(((-1.0)*cj3*x203)));
2661 evalcond[9]=((((-1.0)*x196*x197))+(((-1.0)*x203))+(((-1.0)*new_r01*x199)));
2669 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2670 vinfos[0].jointtype = 1;
2671 vinfos[0].foffset = j0;
2672 vinfos[0].indices[0] = _ij0[0];
2673 vinfos[0].indices[1] = _ij0[1];
2674 vinfos[0].maxsolutions = _nj0;
2675 vinfos[1].jointtype = 1;
2676 vinfos[1].foffset = j1;
2677 vinfos[1].indices[0] = _ij1[0];
2678 vinfos[1].indices[1] = _ij1[1];
2679 vinfos[1].maxsolutions = _nj1;
2680 vinfos[2].jointtype = 1;
2681 vinfos[2].foffset = j2;
2682 vinfos[2].indices[0] = _ij2[0];
2683 vinfos[2].indices[1] = _ij2[1];
2684 vinfos[2].maxsolutions = _nj2;
2685 vinfos[3].jointtype = 1;
2686 vinfos[3].foffset = j3;
2687 vinfos[3].indices[0] = _ij3[0];
2688 vinfos[3].indices[1] = _ij3[1];
2689 vinfos[3].maxsolutions = _nj3;
2690 vinfos[4].jointtype = 1;
2691 vinfos[4].foffset = j4;
2692 vinfos[4].indices[0] = _ij4[0];
2693 vinfos[4].indices[1] = _ij4[1];
2694 vinfos[4].maxsolutions = _nj4;
2695 vinfos[5].jointtype = 1;
2696 vinfos[5].foffset = j5;
2697 vinfos[5].indices[0] = _ij5[0];
2698 vinfos[5].indices[1] = _ij5[1];
2699 vinfos[5].maxsolutions = _nj5;
2700 std::vector<int> vfree(0);
2717 if( bgotonextstatement )
2719 bool bgotonextstatement =
true;
2724 bgotonextstatement=
false;
2729 if( bgotonextstatement )
2740 IkReal j3array[1], cj3array[1], sj3array[1];
2741 bool j3valid[1]={
false};
2747 IkReal x204=x205.
value;
2754 j3array[0]=
IKatan2((x204*(x206.
value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x204));
2755 sj3array[0]=
IKsin(j3array[0]);
2756 cj3array[0]=
IKcos(j3array[0]);
2757 if( j3array[0] >
IKPI )
2761 else if( j3array[0] < -
IKPI )
2762 { j3array[0]+=
IK2PI;
2765 for(
int ij3 = 0; ij3 < 1; ++ij3)
2771 _ij3[0] = ij3; _ij3[1] = -1;
2772 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
2776 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
2779 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2782 IkReal x207=
IKcos(j3);
2783 IkReal x208=
IKsin(j3);
2784 IkReal x209=((1.0)*cj4);
2785 IkReal x210=(sj4*x208);
2786 IkReal x211=(new_r12*x208);
2787 IkReal x212=(sj4*x207);
2788 IkReal x213=(new_r02*x207);
2789 evalcond[0]=(x212+new_r02);
2790 evalcond[1]=(x210+new_r12);
2791 evalcond[2]=((((-1.0)*new_r02*x208))+((new_r12*x207)));
2792 evalcond[3]=(sj4+x211+x213);
2793 evalcond[4]=((((-1.0)*new_r20*x209))+((new_r00*x212))+((new_r10*x210)));
2794 evalcond[5]=((((-1.0)*new_r21*x209))+((new_r01*x212))+((new_r11*x210)));
2795 evalcond[6]=((1.0)+(((-1.0)*new_r22*x209))+((new_r02*x212))+((new_r12*x210)));
2796 evalcond[7]=((((-1.0)*new_r22*sj4))+(((-1.0)*x209*x213))+(((-1.0)*x209*x211)));
2807 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
2808 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
2814 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
2820 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
2824 bool bgotonextstatement =
true;
2827 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
2828 evalcond[1]=new_r02;
2829 evalcond[2]=new_r12;
2830 evalcond[3]=new_r21;
2831 evalcond[4]=new_r20;
2832 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
2834 bgotonextstatement=
false;
2836 IkReal j5array[1], cj5array[1], sj5array[1];
2837 bool j5valid[1]={
false};
2839 IkReal x214=((1.0)*new_r01);
2842 j5array[0]=
IKatan2(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x214))+((cj3*new_r00))));
2843 sj5array[0]=
IKsin(j5array[0]);
2844 cj5array[0]=
IKcos(j5array[0]);
2845 if( j5array[0] >
IKPI )
2849 else if( j5array[0] < -
IKPI )
2850 { j5array[0]+=
IK2PI;
2853 for(
int ij5 = 0; ij5 < 1; ++ij5)
2859 _ij5[0] = ij5; _ij5[1] = -1;
2860 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2864 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2867 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2870 IkReal x215=
IKsin(j5);
2871 IkReal x216=
IKcos(j5);
2872 IkReal x217=((1.0)*cj3);
2873 IkReal x218=(sj3*x215);
2874 IkReal x219=((1.0)*x216);
2875 IkReal x220=(x216*x217);
2876 evalcond[0]=(((new_r11*sj3))+x215+((cj3*new_r01)));
2877 evalcond[1]=(((new_r00*sj3))+(((-1.0)*new_r10*x217))+x215);
2878 evalcond[2]=((((-1.0)*new_r11*x217))+((new_r01*sj3))+x216);
2879 evalcond[3]=(((sj3*x216))+((cj3*x215))+new_r01);
2880 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x219)));
2881 evalcond[5]=(x218+new_r00+(((-1.0)*x220)));
2882 evalcond[6]=(x218+new_r11+(((-1.0)*x220)));
2883 evalcond[7]=((((-1.0)*sj3*x219))+new_r10+(((-1.0)*x215*x217)));
2891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2892 vinfos[0].jointtype = 1;
2893 vinfos[0].foffset = j0;
2894 vinfos[0].indices[0] = _ij0[0];
2895 vinfos[0].indices[1] = _ij0[1];
2896 vinfos[0].maxsolutions = _nj0;
2897 vinfos[1].jointtype = 1;
2898 vinfos[1].foffset = j1;
2899 vinfos[1].indices[0] = _ij1[0];
2900 vinfos[1].indices[1] = _ij1[1];
2901 vinfos[1].maxsolutions = _nj1;
2902 vinfos[2].jointtype = 1;
2903 vinfos[2].foffset = j2;
2904 vinfos[2].indices[0] = _ij2[0];
2905 vinfos[2].indices[1] = _ij2[1];
2906 vinfos[2].maxsolutions = _nj2;
2907 vinfos[3].jointtype = 1;
2908 vinfos[3].foffset = j3;
2909 vinfos[3].indices[0] = _ij3[0];
2910 vinfos[3].indices[1] = _ij3[1];
2911 vinfos[3].maxsolutions = _nj3;
2912 vinfos[4].jointtype = 1;
2913 vinfos[4].foffset = j4;
2914 vinfos[4].indices[0] = _ij4[0];
2915 vinfos[4].indices[1] = _ij4[1];
2916 vinfos[4].maxsolutions = _nj4;
2917 vinfos[5].jointtype = 1;
2918 vinfos[5].foffset = j5;
2919 vinfos[5].indices[0] = _ij5[0];
2920 vinfos[5].indices[1] = _ij5[1];
2921 vinfos[5].maxsolutions = _nj5;
2922 std::vector<int> vfree(0);
2930 if( bgotonextstatement )
2932 bool bgotonextstatement =
true;
2935 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2936 evalcond[1]=new_r02;
2937 evalcond[2]=new_r12;
2938 evalcond[3]=new_r21;
2939 evalcond[4]=new_r20;
2940 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
2942 bgotonextstatement=
false;
2944 IkReal j5array[1], cj5array[1], sj5array[1];
2945 bool j5valid[1]={
false};
2947 IkReal x221=((1.0)*sj3);
2950 j5array[0]=
IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x221))), ((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))));
2951 sj5array[0]=
IKsin(j5array[0]);
2952 cj5array[0]=
IKcos(j5array[0]);
2953 if( j5array[0] >
IKPI )
2957 else if( j5array[0] < -
IKPI )
2958 { j5array[0]+=
IK2PI;
2961 for(
int ij5 = 0; ij5 < 1; ++ij5)
2967 _ij5[0] = ij5; _ij5[1] = -1;
2968 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2972 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2975 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2978 IkReal x222=
IKcos(j5);
2979 IkReal x223=
IKsin(j5);
2980 IkReal x224=((1.0)*cj3);
2981 IkReal x225=(sj3*x222);
2982 IkReal x226=((1.0)*x223);
2983 IkReal x227=(x223*x224);
2984 evalcond[0]=(((new_r10*sj3))+x222+((cj3*new_r00)));
2985 evalcond[1]=((((-1.0)*new_r10*x224))+((new_r00*sj3))+x223);
2986 evalcond[2]=((((-1.0)*new_r11*x224))+((new_r01*sj3))+x222);
2987 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x226)));
2988 evalcond[4]=(((cj3*x222))+((sj3*x223))+new_r00);
2989 evalcond[5]=(x225+new_r01+(((-1.0)*x227)));
2990 evalcond[6]=(x225+new_r10+(((-1.0)*x227)));
2991 evalcond[7]=((((-1.0)*x222*x224))+(((-1.0)*sj3*x226))+new_r11);
2999 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3000 vinfos[0].jointtype = 1;
3001 vinfos[0].foffset = j0;
3002 vinfos[0].indices[0] = _ij0[0];
3003 vinfos[0].indices[1] = _ij0[1];
3004 vinfos[0].maxsolutions = _nj0;
3005 vinfos[1].jointtype = 1;
3006 vinfos[1].foffset = j1;
3007 vinfos[1].indices[0] = _ij1[0];
3008 vinfos[1].indices[1] = _ij1[1];
3009 vinfos[1].maxsolutions = _nj1;
3010 vinfos[2].jointtype = 1;
3011 vinfos[2].foffset = j2;
3012 vinfos[2].indices[0] = _ij2[0];
3013 vinfos[2].indices[1] = _ij2[1];
3014 vinfos[2].maxsolutions = _nj2;
3015 vinfos[3].jointtype = 1;
3016 vinfos[3].foffset = j3;
3017 vinfos[3].indices[0] = _ij3[0];
3018 vinfos[3].indices[1] = _ij3[1];
3019 vinfos[3].maxsolutions = _nj3;
3020 vinfos[4].jointtype = 1;
3021 vinfos[4].foffset = j4;
3022 vinfos[4].indices[0] = _ij4[0];
3023 vinfos[4].indices[1] = _ij4[1];
3024 vinfos[4].maxsolutions = _nj4;
3025 vinfos[5].jointtype = 1;
3026 vinfos[5].foffset = j5;
3027 vinfos[5].indices[0] = _ij5[0];
3028 vinfos[5].indices[1] = _ij5[1];
3029 vinfos[5].maxsolutions = _nj5;
3030 std::vector<int> vfree(0);
3038 if( bgotonextstatement )
3040 bool bgotonextstatement =
true;
3043 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3044 evalcond[1]=new_r02;
3045 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3047 bgotonextstatement=
false;
3049 IkReal j5array[1], cj5array[1], sj5array[1];
3050 bool j5valid[1]={
false};
3054 j5array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3055 sj5array[0]=
IKsin(j5array[0]);
3056 cj5array[0]=
IKcos(j5array[0]);
3057 if( j5array[0] >
IKPI )
3061 else if( j5array[0] < -
IKPI )
3062 { j5array[0]+=
IK2PI;
3065 for(
int ij5 = 0; ij5 < 1; ++ij5)
3071 _ij5[0] = ij5; _ij5[1] = -1;
3072 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3076 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3079 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3082 IkReal x228=
IKsin(j5);
3083 IkReal x229=
IKcos(j5);
3084 IkReal x230=((1.0)*cj4);
3085 IkReal x231=((1.0)*sj4);
3086 evalcond[0]=(x228+new_r00);
3087 evalcond[1]=(x229+new_r01);
3088 evalcond[2]=(((sj4*x228))+new_r21);
3089 evalcond[3]=(((cj4*x228))+new_r11);
3090 evalcond[4]=(new_r20+(((-1.0)*x229*x231)));
3091 evalcond[5]=(new_r10+(((-1.0)*x229*x230)));
3092 evalcond[6]=((((-1.0)*new_r20*x231))+x229+(((-1.0)*new_r10*x230)));
3093 evalcond[7]=((((-1.0)*new_r21*x231))+(((-1.0)*new_r11*x230))+(((-1.0)*x228)));
3101 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3102 vinfos[0].jointtype = 1;
3103 vinfos[0].foffset = j0;
3104 vinfos[0].indices[0] = _ij0[0];
3105 vinfos[0].indices[1] = _ij0[1];
3106 vinfos[0].maxsolutions = _nj0;
3107 vinfos[1].jointtype = 1;
3108 vinfos[1].foffset = j1;
3109 vinfos[1].indices[0] = _ij1[0];
3110 vinfos[1].indices[1] = _ij1[1];
3111 vinfos[1].maxsolutions = _nj1;
3112 vinfos[2].jointtype = 1;
3113 vinfos[2].foffset = j2;
3114 vinfos[2].indices[0] = _ij2[0];
3115 vinfos[2].indices[1] = _ij2[1];
3116 vinfos[2].maxsolutions = _nj2;
3117 vinfos[3].jointtype = 1;
3118 vinfos[3].foffset = j3;
3119 vinfos[3].indices[0] = _ij3[0];
3120 vinfos[3].indices[1] = _ij3[1];
3121 vinfos[3].maxsolutions = _nj3;
3122 vinfos[4].jointtype = 1;
3123 vinfos[4].foffset = j4;
3124 vinfos[4].indices[0] = _ij4[0];
3125 vinfos[4].indices[1] = _ij4[1];
3126 vinfos[4].maxsolutions = _nj4;
3127 vinfos[5].jointtype = 1;
3128 vinfos[5].foffset = j5;
3129 vinfos[5].indices[0] = _ij5[0];
3130 vinfos[5].indices[1] = _ij5[1];
3131 vinfos[5].maxsolutions = _nj5;
3132 std::vector<int> vfree(0);
3140 if( bgotonextstatement )
3142 bool bgotonextstatement =
true;
3145 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3146 evalcond[1]=new_r02;
3147 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3149 bgotonextstatement=
false;
3151 IkReal j5array[1], cj5array[1], sj5array[1];
3152 bool j5valid[1]={
false};
3156 j5array[0]=
IKatan2(new_r00, new_r01);
3157 sj5array[0]=
IKsin(j5array[0]);
3158 cj5array[0]=
IKcos(j5array[0]);
3159 if( j5array[0] >
IKPI )
3163 else if( j5array[0] < -
IKPI )
3164 { j5array[0]+=
IK2PI;
3167 for(
int ij5 = 0; ij5 < 1; ++ij5)
3173 _ij5[0] = ij5; _ij5[1] = -1;
3174 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3178 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3181 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3184 IkReal x232=
IKsin(j5);
3185 IkReal x233=
IKcos(j5);
3186 IkReal x234=((1.0)*sj4);
3187 IkReal x235=((1.0)*x233);
3188 evalcond[0]=(((sj4*x232))+new_r21);
3189 evalcond[1]=(x232+(((-1.0)*new_r00)));
3190 evalcond[2]=(x233+(((-1.0)*new_r01)));
3191 evalcond[3]=((((-1.0)*x233*x234))+new_r20);
3192 evalcond[4]=(((cj4*x232))+(((-1.0)*new_r11)));
3193 evalcond[5]=((((-1.0)*cj4*x235))+(((-1.0)*new_r10)));
3194 evalcond[6]=((((-1.0)*new_r20*x234))+((cj4*new_r10))+x233);
3195 evalcond[7]=((((-1.0)*new_r21*x234))+((cj4*new_r11))+(((-1.0)*x232)));
3203 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3204 vinfos[0].jointtype = 1;
3205 vinfos[0].foffset = j0;
3206 vinfos[0].indices[0] = _ij0[0];
3207 vinfos[0].indices[1] = _ij0[1];
3208 vinfos[0].maxsolutions = _nj0;
3209 vinfos[1].jointtype = 1;
3210 vinfos[1].foffset = j1;
3211 vinfos[1].indices[0] = _ij1[0];
3212 vinfos[1].indices[1] = _ij1[1];
3213 vinfos[1].maxsolutions = _nj1;
3214 vinfos[2].jointtype = 1;
3215 vinfos[2].foffset = j2;
3216 vinfos[2].indices[0] = _ij2[0];
3217 vinfos[2].indices[1] = _ij2[1];
3218 vinfos[2].maxsolutions = _nj2;
3219 vinfos[3].jointtype = 1;
3220 vinfos[3].foffset = j3;
3221 vinfos[3].indices[0] = _ij3[0];
3222 vinfos[3].indices[1] = _ij3[1];
3223 vinfos[3].maxsolutions = _nj3;
3224 vinfos[4].jointtype = 1;
3225 vinfos[4].foffset = j4;
3226 vinfos[4].indices[0] = _ij4[0];
3227 vinfos[4].indices[1] = _ij4[1];
3228 vinfos[4].maxsolutions = _nj4;
3229 vinfos[5].jointtype = 1;
3230 vinfos[5].foffset = j5;
3231 vinfos[5].indices[0] = _ij5[0];
3232 vinfos[5].indices[1] = _ij5[1];
3233 vinfos[5].maxsolutions = _nj5;
3234 std::vector<int> vfree(0);
3242 if( bgotonextstatement )
3244 bool bgotonextstatement =
true;
3247 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
3248 evalcond[1]=new_r12;
3249 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3251 bgotonextstatement=
false;
3253 IkReal j5array[1], cj5array[1], sj5array[1];
3254 bool j5valid[1]={
false};
3258 j5array[0]=
IKatan2(new_r10, new_r11);
3259 sj5array[0]=
IKsin(j5array[0]);
3260 cj5array[0]=
IKcos(j5array[0]);
3261 if( j5array[0] >
IKPI )
3265 else if( j5array[0] < -
IKPI )
3266 { j5array[0]+=
IK2PI;
3269 for(
int ij5 = 0; ij5 < 1; ++ij5)
3275 _ij5[0] = ij5; _ij5[1] = -1;
3276 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3280 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3283 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3286 IkReal x236=
IKcos(j5);
3287 IkReal x237=
IKsin(j5);
3288 IkReal x238=((1.0)*cj4);
3289 IkReal x239=((1.0)*sj4);
3290 IkReal x240=((1.0)*x237);
3291 evalcond[0]=(((new_r02*x236))+new_r20);
3292 evalcond[1]=(x237+(((-1.0)*new_r10)));
3293 evalcond[2]=(x236+(((-1.0)*new_r11)));
3294 evalcond[3]=(((cj4*x237))+new_r01);
3295 evalcond[4]=((((-1.0)*new_r02*x240))+new_r21);
3296 evalcond[5]=((((-1.0)*x236*x238))+new_r00);
3297 evalcond[6]=((((-1.0)*new_r20*x239))+x236+(((-1.0)*new_r00*x238)));
3298 evalcond[7]=((((-1.0)*new_r21*x239))+(((-1.0)*x240))+(((-1.0)*new_r01*x238)));
3306 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3307 vinfos[0].jointtype = 1;
3308 vinfos[0].foffset = j0;
3309 vinfos[0].indices[0] = _ij0[0];
3310 vinfos[0].indices[1] = _ij0[1];
3311 vinfos[0].maxsolutions = _nj0;
3312 vinfos[1].jointtype = 1;
3313 vinfos[1].foffset = j1;
3314 vinfos[1].indices[0] = _ij1[0];
3315 vinfos[1].indices[1] = _ij1[1];
3316 vinfos[1].maxsolutions = _nj1;
3317 vinfos[2].jointtype = 1;
3318 vinfos[2].foffset = j2;
3319 vinfos[2].indices[0] = _ij2[0];
3320 vinfos[2].indices[1] = _ij2[1];
3321 vinfos[2].maxsolutions = _nj2;
3322 vinfos[3].jointtype = 1;
3323 vinfos[3].foffset = j3;
3324 vinfos[3].indices[0] = _ij3[0];
3325 vinfos[3].indices[1] = _ij3[1];
3326 vinfos[3].maxsolutions = _nj3;
3327 vinfos[4].jointtype = 1;
3328 vinfos[4].foffset = j4;
3329 vinfos[4].indices[0] = _ij4[0];
3330 vinfos[4].indices[1] = _ij4[1];
3331 vinfos[4].maxsolutions = _nj4;
3332 vinfos[5].jointtype = 1;
3333 vinfos[5].foffset = j5;
3334 vinfos[5].indices[0] = _ij5[0];
3335 vinfos[5].indices[1] = _ij5[1];
3336 vinfos[5].maxsolutions = _nj5;
3337 std::vector<int> vfree(0);
3345 if( bgotonextstatement )
3347 bool bgotonextstatement =
true;
3350 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3351 evalcond[1]=new_r12;
3352 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3354 bgotonextstatement=
false;
3356 IkReal j5array[1], cj5array[1], sj5array[1];
3357 bool j5valid[1]={
false};
3361 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3362 sj5array[0]=
IKsin(j5array[0]);
3363 cj5array[0]=
IKcos(j5array[0]);
3364 if( j5array[0] >
IKPI )
3368 else if( j5array[0] < -
IKPI )
3369 { j5array[0]+=
IK2PI;
3372 for(
int ij5 = 0; ij5 < 1; ++ij5)
3378 _ij5[0] = ij5; _ij5[1] = -1;
3379 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3383 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3386 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3389 IkReal x241=
IKsin(j5);
3390 IkReal x242=
IKcos(j5);
3391 IkReal x243=((1.0)*sj4);
3392 IkReal x244=((1.0)*x242);
3393 evalcond[0]=(x241+new_r10);
3394 evalcond[1]=(x242+new_r11);
3395 evalcond[2]=(new_r21+((new_r02*x241)));
3396 evalcond[3]=((((-1.0)*new_r02*x244))+new_r20);
3397 evalcond[4]=(((cj4*x241))+(((-1.0)*new_r01)));
3398 evalcond[5]=((((-1.0)*cj4*x244))+(((-1.0)*new_r00)));
3399 evalcond[6]=(((cj4*new_r00))+x242+(((-1.0)*new_r20*x243)));
3400 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x241))+(((-1.0)*new_r21*x243)));
3408 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3409 vinfos[0].jointtype = 1;
3410 vinfos[0].foffset = j0;
3411 vinfos[0].indices[0] = _ij0[0];
3412 vinfos[0].indices[1] = _ij0[1];
3413 vinfos[0].maxsolutions = _nj0;
3414 vinfos[1].jointtype = 1;
3415 vinfos[1].foffset = j1;
3416 vinfos[1].indices[0] = _ij1[0];
3417 vinfos[1].indices[1] = _ij1[1];
3418 vinfos[1].maxsolutions = _nj1;
3419 vinfos[2].jointtype = 1;
3420 vinfos[2].foffset = j2;
3421 vinfos[2].indices[0] = _ij2[0];
3422 vinfos[2].indices[1] = _ij2[1];
3423 vinfos[2].maxsolutions = _nj2;
3424 vinfos[3].jointtype = 1;
3425 vinfos[3].foffset = j3;
3426 vinfos[3].indices[0] = _ij3[0];
3427 vinfos[3].indices[1] = _ij3[1];
3428 vinfos[3].maxsolutions = _nj3;
3429 vinfos[4].jointtype = 1;
3430 vinfos[4].foffset = j4;
3431 vinfos[4].indices[0] = _ij4[0];
3432 vinfos[4].indices[1] = _ij4[1];
3433 vinfos[4].maxsolutions = _nj4;
3434 vinfos[5].jointtype = 1;
3435 vinfos[5].foffset = j5;
3436 vinfos[5].indices[0] = _ij5[0];
3437 vinfos[5].indices[1] = _ij5[1];
3438 vinfos[5].maxsolutions = _nj5;
3439 std::vector<int> vfree(0);
3447 if( bgotonextstatement )
3449 bool bgotonextstatement =
true;
3452 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
3453 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3455 bgotonextstatement=
false;
3463 if(
IKabs(j5eval[0]) < 0.0000000100000000 )
3469 IkReal op[2+1], zeror[2];
3474 polyroots2(op,zeror,numroots);
3475 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3476 int numsolutions = 0;
3477 for(
int ij5 = 0; ij5 < numroots; ++ij5)
3479 IkReal htj5 = zeror[ij5];
3480 tempj5array[0]=((2.0)*(atan(htj5)));
3481 for(
int kj5 = 0; kj5 < 1; ++kj5)
3483 j5array[numsolutions] = tempj5array[kj5];
3484 if( j5array[numsolutions] >
IKPI )
3486 j5array[numsolutions]-=
IK2PI;
3488 else if( j5array[numsolutions] < -
IKPI )
3490 j5array[numsolutions]+=
IK2PI;
3492 sj5array[numsolutions] =
IKsin(j5array[numsolutions]);
3493 cj5array[numsolutions] =
IKcos(j5array[numsolutions]);
3497 bool j5valid[2]={
true,
true};
3499 for(
int ij5 = 0; ij5 < numsolutions; ++ij5)
3505 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3508 _ij5[0] = ij5; _ij5[1] = -1;
3509 for(
int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3513 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3517 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3518 vinfos[0].jointtype = 1;
3519 vinfos[0].foffset = j0;
3520 vinfos[0].indices[0] = _ij0[0];
3521 vinfos[0].indices[1] = _ij0[1];
3522 vinfos[0].maxsolutions = _nj0;
3523 vinfos[1].jointtype = 1;
3524 vinfos[1].foffset = j1;
3525 vinfos[1].indices[0] = _ij1[0];
3526 vinfos[1].indices[1] = _ij1[1];
3527 vinfos[1].maxsolutions = _nj1;
3528 vinfos[2].jointtype = 1;
3529 vinfos[2].foffset = j2;
3530 vinfos[2].indices[0] = _ij2[0];
3531 vinfos[2].indices[1] = _ij2[1];
3532 vinfos[2].maxsolutions = _nj2;
3533 vinfos[3].jointtype = 1;
3534 vinfos[3].foffset = j3;
3535 vinfos[3].indices[0] = _ij3[0];
3536 vinfos[3].indices[1] = _ij3[1];
3537 vinfos[3].maxsolutions = _nj3;
3538 vinfos[4].jointtype = 1;
3539 vinfos[4].foffset = j4;
3540 vinfos[4].indices[0] = _ij4[0];
3541 vinfos[4].indices[1] = _ij4[1];
3542 vinfos[4].maxsolutions = _nj4;
3543 vinfos[5].jointtype = 1;
3544 vinfos[5].foffset = j5;
3545 vinfos[5].indices[0] = _ij5[0];
3546 vinfos[5].indices[1] = _ij5[1];
3547 vinfos[5].maxsolutions = _nj5;
3548 std::vector<int> vfree(0);
3559 if( bgotonextstatement )
3561 bool bgotonextstatement =
true;
3566 bgotonextstatement=
false;
3571 if( bgotonextstatement )
3586 IkReal j5array[1], cj5array[1], sj5array[1];
3587 bool j5valid[1]={
false};
3593 IkReal x245=x246.
value;
3600 j5array[0]=
IKatan2(((-1.0)*new_r21*x245), (x245*(x247.
value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3601 sj5array[0]=
IKsin(j5array[0]);
3602 cj5array[0]=
IKcos(j5array[0]);
3603 if( j5array[0] >
IKPI )
3607 else if( j5array[0] < -
IKPI )
3608 { j5array[0]+=
IK2PI;
3611 for(
int ij5 = 0; ij5 < 1; ++ij5)
3617 _ij5[0] = ij5; _ij5[1] = -1;
3618 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3622 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3625 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3627 IkReal evalcond[12];
3628 IkReal x248=
IKsin(j5);
3629 IkReal x249=
IKcos(j5);
3630 IkReal x250=(cj3*new_r00);
3631 IkReal x251=(cj3*cj4);
3632 IkReal x252=((1.0)*cj3);
3633 IkReal x253=(new_r11*sj3);
3634 IkReal x254=((1.0)*cj4);
3635 IkReal x255=(new_r10*sj3);
3636 IkReal x256=((1.0)*sj4);
3637 IkReal x257=(sj3*x248);
3638 IkReal x258=((1.0)*x249);
3639 IkReal x259=(sj3*x249);
3640 evalcond[0]=(new_r21+((sj4*x248)));
3641 evalcond[1]=((((-1.0)*x249*x256))+new_r20);
3642 evalcond[2]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x248);
3643 evalcond[3]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x249);
3644 evalcond[4]=(((cj4*x248))+x253+((cj3*new_r01)));
3645 evalcond[5]=(((x248*x251))+x259+new_r01);
3646 evalcond[6]=(x255+x250+(((-1.0)*x249*x254)));
3647 evalcond[7]=((((-1.0)*x251*x258))+x257+new_r00);
3648 evalcond[8]=((((-1.0)*x249*x252))+new_r11+((cj4*x257)));
3649 evalcond[9]=((((-1.0)*x254*x259))+(((-1.0)*x248*x252))+new_r10);
3650 evalcond[10]=((((-1.0)*x250*x254))+(((-1.0)*x254*x255))+x249+(((-1.0)*new_r20*x256)));
3651 evalcond[11]=((((-1.0)*new_r21*x256))+(((-1.0)*x248))+(((-1.0)*x253*x254))+(((-1.0)*new_r01*x251)));
3659 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3660 vinfos[0].jointtype = 1;
3661 vinfos[0].foffset = j0;
3662 vinfos[0].indices[0] = _ij0[0];
3663 vinfos[0].indices[1] = _ij0[1];
3664 vinfos[0].maxsolutions = _nj0;
3665 vinfos[1].jointtype = 1;
3666 vinfos[1].foffset = j1;
3667 vinfos[1].indices[0] = _ij1[0];
3668 vinfos[1].indices[1] = _ij1[1];
3669 vinfos[1].maxsolutions = _nj1;
3670 vinfos[2].jointtype = 1;
3671 vinfos[2].foffset = j2;
3672 vinfos[2].indices[0] = _ij2[0];
3673 vinfos[2].indices[1] = _ij2[1];
3674 vinfos[2].maxsolutions = _nj2;
3675 vinfos[3].jointtype = 1;
3676 vinfos[3].foffset = j3;
3677 vinfos[3].indices[0] = _ij3[0];
3678 vinfos[3].indices[1] = _ij3[1];
3679 vinfos[3].maxsolutions = _nj3;
3680 vinfos[4].jointtype = 1;
3681 vinfos[4].foffset = j4;
3682 vinfos[4].indices[0] = _ij4[0];
3683 vinfos[4].indices[1] = _ij4[1];
3684 vinfos[4].maxsolutions = _nj4;
3685 vinfos[5].jointtype = 1;
3686 vinfos[5].foffset = j5;
3687 vinfos[5].indices[0] = _ij5[0];
3688 vinfos[5].indices[1] = _ij5[1];
3689 vinfos[5].maxsolutions = _nj5;
3690 std::vector<int> vfree(0);
3703 IkReal j5array[1], cj5array[1], sj5array[1];
3704 bool j5valid[1]={
false};
3710 IkReal x260=x261.
value;
3717 j5array[0]=
IKatan2(((-1.0)*new_r21*x260), (x260*(x262.
value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3718 sj5array[0]=
IKsin(j5array[0]);
3719 cj5array[0]=
IKcos(j5array[0]);
3720 if( j5array[0] >
IKPI )
3724 else if( j5array[0] < -
IKPI )
3725 { j5array[0]+=
IK2PI;
3728 for(
int ij5 = 0; ij5 < 1; ++ij5)
3734 _ij5[0] = ij5; _ij5[1] = -1;
3735 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3739 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3742 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3744 IkReal evalcond[12];
3745 IkReal x263=
IKsin(j5);
3746 IkReal x264=
IKcos(j5);
3747 IkReal x265=(cj3*new_r00);
3748 IkReal x266=(cj3*cj4);
3749 IkReal x267=((1.0)*cj3);
3750 IkReal x268=(new_r11*sj3);
3751 IkReal x269=((1.0)*cj4);
3752 IkReal x270=(new_r10*sj3);
3753 IkReal x271=((1.0)*sj4);
3754 IkReal x272=(sj3*x263);
3755 IkReal x273=((1.0)*x264);
3756 IkReal x274=(sj3*x264);
3757 evalcond[0]=(new_r21+((sj4*x263)));
3758 evalcond[1]=(new_r20+(((-1.0)*x264*x271)));
3759 evalcond[2]=(((new_r00*sj3))+x263+(((-1.0)*new_r10*x267)));
3760 evalcond[3]=(((new_r01*sj3))+x264+(((-1.0)*new_r11*x267)));
3761 evalcond[4]=(((cj4*x263))+x268+((cj3*new_r01)));
3762 evalcond[5]=(((x263*x266))+x274+new_r01);
3763 evalcond[6]=(x265+x270+(((-1.0)*x264*x269)));
3764 evalcond[7]=(x272+(((-1.0)*x266*x273))+new_r00);
3765 evalcond[8]=(((cj4*x272))+new_r11+(((-1.0)*x264*x267)));
3766 evalcond[9]=((((-1.0)*x263*x267))+(((-1.0)*x269*x274))+new_r10);
3767 evalcond[10]=((((-1.0)*x269*x270))+x264+(((-1.0)*new_r20*x271))+(((-1.0)*x265*x269)));
3768 evalcond[11]=((((-1.0)*x263))+(((-1.0)*new_r01*x266))+(((-1.0)*new_r21*x271))+(((-1.0)*x268*x269)));
3776 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3777 vinfos[0].jointtype = 1;
3778 vinfos[0].foffset = j0;
3779 vinfos[0].indices[0] = _ij0[0];
3780 vinfos[0].indices[1] = _ij0[1];
3781 vinfos[0].maxsolutions = _nj0;
3782 vinfos[1].jointtype = 1;
3783 vinfos[1].foffset = j1;
3784 vinfos[1].indices[0] = _ij1[0];
3785 vinfos[1].indices[1] = _ij1[1];
3786 vinfos[1].maxsolutions = _nj1;
3787 vinfos[2].jointtype = 1;
3788 vinfos[2].foffset = j2;
3789 vinfos[2].indices[0] = _ij2[0];
3790 vinfos[2].indices[1] = _ij2[1];
3791 vinfos[2].maxsolutions = _nj2;
3792 vinfos[3].jointtype = 1;
3793 vinfos[3].foffset = j3;
3794 vinfos[3].indices[0] = _ij3[0];
3795 vinfos[3].indices[1] = _ij3[1];
3796 vinfos[3].maxsolutions = _nj3;
3797 vinfos[4].jointtype = 1;
3798 vinfos[4].foffset = j4;
3799 vinfos[4].indices[0] = _ij4[0];
3800 vinfos[4].indices[1] = _ij4[1];
3801 vinfos[4].maxsolutions = _nj4;
3802 vinfos[5].jointtype = 1;
3803 vinfos[5].foffset = j5;
3804 vinfos[5].indices[0] = _ij5[0];
3805 vinfos[5].indices[1] = _ij5[1];
3806 vinfos[5].maxsolutions = _nj5;
3807 std::vector<int> vfree(0);
3820 IkReal j5array[1], cj5array[1], sj5array[1];
3821 bool j5valid[1]={
false};
3831 j5array[0]=((-1.5707963267949)+(x275.
value)+(((1.5707963267949)*(x276.
value))));
3832 sj5array[0]=
IKsin(j5array[0]);
3833 cj5array[0]=
IKcos(j5array[0]);
3834 if( j5array[0] >
IKPI )
3838 else if( j5array[0] < -
IKPI )
3839 { j5array[0]+=
IK2PI;
3842 for(
int ij5 = 0; ij5 < 1; ++ij5)
3848 _ij5[0] = ij5; _ij5[1] = -1;
3849 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3853 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3856 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3858 IkReal evalcond[12];
3859 IkReal x277=
IKsin(j5);
3860 IkReal x278=
IKcos(j5);
3861 IkReal x279=(cj3*new_r00);
3862 IkReal x280=(cj3*cj4);
3863 IkReal x281=((1.0)*cj3);
3864 IkReal x282=(new_r11*sj3);
3865 IkReal x283=((1.0)*cj4);
3866 IkReal x284=(new_r10*sj3);
3867 IkReal x285=((1.0)*sj4);
3868 IkReal x286=(sj3*x277);
3869 IkReal x287=((1.0)*x278);
3870 IkReal x288=(sj3*x278);
3871 evalcond[0]=(new_r21+((sj4*x277)));
3872 evalcond[1]=(new_r20+(((-1.0)*x278*x285)));
3873 evalcond[2]=(((new_r00*sj3))+x277+(((-1.0)*new_r10*x281)));
3874 evalcond[3]=(((new_r01*sj3))+x278+(((-1.0)*new_r11*x281)));
3875 evalcond[4]=(((cj4*x277))+x282+((cj3*new_r01)));
3876 evalcond[5]=(x288+new_r01+((x277*x280)));
3877 evalcond[6]=(x279+x284+(((-1.0)*x278*x283)));
3878 evalcond[7]=((((-1.0)*x280*x287))+x286+new_r00);
3879 evalcond[8]=(new_r11+((cj4*x286))+(((-1.0)*x278*x281)));
3880 evalcond[9]=((((-1.0)*x277*x281))+new_r10+(((-1.0)*x283*x288)));
3881 evalcond[10]=(x278+(((-1.0)*new_r20*x285))+(((-1.0)*x279*x283))+(((-1.0)*x283*x284)));
3882 evalcond[11]=((((-1.0)*x277))+(((-1.0)*x282*x283))+(((-1.0)*new_r01*x280))+(((-1.0)*new_r21*x285)));
3890 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3891 vinfos[0].jointtype = 1;
3892 vinfos[0].foffset = j0;
3893 vinfos[0].indices[0] = _ij0[0];
3894 vinfos[0].indices[1] = _ij0[1];
3895 vinfos[0].maxsolutions = _nj0;
3896 vinfos[1].jointtype = 1;
3897 vinfos[1].foffset = j1;
3898 vinfos[1].indices[0] = _ij1[0];
3899 vinfos[1].indices[1] = _ij1[1];
3900 vinfos[1].maxsolutions = _nj1;
3901 vinfos[2].jointtype = 1;
3902 vinfos[2].foffset = j2;
3903 vinfos[2].indices[0] = _ij2[0];
3904 vinfos[2].indices[1] = _ij2[1];
3905 vinfos[2].maxsolutions = _nj2;
3906 vinfos[3].jointtype = 1;
3907 vinfos[3].foffset = j3;
3908 vinfos[3].indices[0] = _ij3[0];
3909 vinfos[3].indices[1] = _ij3[1];
3910 vinfos[3].maxsolutions = _nj3;
3911 vinfos[4].jointtype = 1;
3912 vinfos[4].foffset = j4;
3913 vinfos[4].indices[0] = _ij4[0];
3914 vinfos[4].indices[1] = _ij4[1];
3915 vinfos[4].maxsolutions = _nj4;
3916 vinfos[5].jointtype = 1;
3917 vinfos[5].foffset = j5;
3918 vinfos[5].indices[0] = _ij5[0];
3919 vinfos[5].indices[1] = _ij5[1];
3920 vinfos[5].maxsolutions = _nj5;
3921 std::vector<int> vfree(0);
3940 IkReal j3array[1], cj3array[1], sj3array[1];
3941 bool j3valid[1]={
false};
3951 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x289.
value)))+(x290.
value));
3952 sj3array[0]=
IKsin(j3array[0]);
3953 cj3array[0]=
IKcos(j3array[0]);
3954 if( j3array[0] >
IKPI )
3958 else if( j3array[0] < -
IKPI )
3959 { j3array[0]+=
IK2PI;
3962 for(
int ij3 = 0; ij3 < 1; ++ij3)
3968 _ij3[0] = ij3; _ij3[1] = -1;
3969 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
3973 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
3976 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3979 IkReal x291=
IKcos(j3);
3980 IkReal x292=
IKsin(j3);
3981 IkReal x293=((1.0)*cj4);
3982 IkReal x294=(sj4*x292);
3983 IkReal x295=(new_r12*x292);
3984 IkReal x296=(sj4*x291);
3985 IkReal x297=(new_r02*x291);
3986 evalcond[0]=(x296+new_r02);
3987 evalcond[1]=(x294+new_r12);
3988 evalcond[2]=(((new_r12*x291))+(((-1.0)*new_r02*x292)));
3989 evalcond[3]=(sj4+x295+x297);
3990 evalcond[4]=((((-1.0)*new_r20*x293))+((new_r10*x294))+((new_r00*x296)));
3991 evalcond[5]=((((-1.0)*new_r21*x293))+((new_r11*x294))+((new_r01*x296)));
3992 evalcond[6]=((1.0)+((new_r02*x296))+((new_r12*x294))+(((-1.0)*new_r22*x293)));
3993 evalcond[7]=((((-1.0)*x293*x297))+(((-1.0)*x293*x295))+(((-1.0)*new_r22*sj4)));
4004 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
4005 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
4011 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
4017 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
4021 bool bgotonextstatement =
true;
4024 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
4025 evalcond[1]=new_r02;
4026 evalcond[2]=new_r12;
4027 evalcond[3]=new_r21;
4028 evalcond[4]=new_r20;
4029 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
4031 bgotonextstatement=
false;
4033 IkReal j5array[1], cj5array[1], sj5array[1];
4034 bool j5valid[1]={
false};
4036 IkReal x298=((1.0)*new_r01);
4039 j5array[0]=
IKatan2(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x298))+((cj3*new_r00))));
4040 sj5array[0]=
IKsin(j5array[0]);
4041 cj5array[0]=
IKcos(j5array[0]);
4042 if( j5array[0] >
IKPI )
4046 else if( j5array[0] < -
IKPI )
4047 { j5array[0]+=
IK2PI;
4050 for(
int ij5 = 0; ij5 < 1; ++ij5)
4056 _ij5[0] = ij5; _ij5[1] = -1;
4057 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4061 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4064 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4067 IkReal x299=
IKsin(j5);
4068 IkReal x300=
IKcos(j5);
4069 IkReal x301=((1.0)*cj3);
4070 IkReal x302=(sj3*x299);
4071 IkReal x303=((1.0)*x300);
4072 IkReal x304=(x300*x301);
4073 evalcond[0]=(((new_r11*sj3))+x299+((cj3*new_r01)));
4074 evalcond[1]=(((new_r00*sj3))+x299+(((-1.0)*new_r10*x301)));
4075 evalcond[2]=(((new_r01*sj3))+x300+(((-1.0)*new_r11*x301)));
4076 evalcond[3]=(((cj3*x299))+((sj3*x300))+new_r01);
4077 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x303)));
4078 evalcond[5]=(x302+new_r00+(((-1.0)*x304)));
4079 evalcond[6]=(x302+new_r11+(((-1.0)*x304)));
4080 evalcond[7]=((((-1.0)*sj3*x303))+(((-1.0)*x299*x301))+new_r10);
4088 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4089 vinfos[0].jointtype = 1;
4090 vinfos[0].foffset = j0;
4091 vinfos[0].indices[0] = _ij0[0];
4092 vinfos[0].indices[1] = _ij0[1];
4093 vinfos[0].maxsolutions = _nj0;
4094 vinfos[1].jointtype = 1;
4095 vinfos[1].foffset = j1;
4096 vinfos[1].indices[0] = _ij1[0];
4097 vinfos[1].indices[1] = _ij1[1];
4098 vinfos[1].maxsolutions = _nj1;
4099 vinfos[2].jointtype = 1;
4100 vinfos[2].foffset = j2;
4101 vinfos[2].indices[0] = _ij2[0];
4102 vinfos[2].indices[1] = _ij2[1];
4103 vinfos[2].maxsolutions = _nj2;
4104 vinfos[3].jointtype = 1;
4105 vinfos[3].foffset = j3;
4106 vinfos[3].indices[0] = _ij3[0];
4107 vinfos[3].indices[1] = _ij3[1];
4108 vinfos[3].maxsolutions = _nj3;
4109 vinfos[4].jointtype = 1;
4110 vinfos[4].foffset = j4;
4111 vinfos[4].indices[0] = _ij4[0];
4112 vinfos[4].indices[1] = _ij4[1];
4113 vinfos[4].maxsolutions = _nj4;
4114 vinfos[5].jointtype = 1;
4115 vinfos[5].foffset = j5;
4116 vinfos[5].indices[0] = _ij5[0];
4117 vinfos[5].indices[1] = _ij5[1];
4118 vinfos[5].maxsolutions = _nj5;
4119 std::vector<int> vfree(0);
4127 if( bgotonextstatement )
4129 bool bgotonextstatement =
true;
4132 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4133 evalcond[1]=new_r02;
4134 evalcond[2]=new_r12;
4135 evalcond[3]=new_r21;
4136 evalcond[4]=new_r20;
4137 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
4139 bgotonextstatement=
false;
4141 IkReal j5array[1], cj5array[1], sj5array[1];
4142 bool j5valid[1]={
false};
4144 IkReal x305=((1.0)*sj3);
4147 j5array[0]=
IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x305))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))));
4148 sj5array[0]=
IKsin(j5array[0]);
4149 cj5array[0]=
IKcos(j5array[0]);
4150 if( j5array[0] >
IKPI )
4154 else if( j5array[0] < -
IKPI )
4155 { j5array[0]+=
IK2PI;
4158 for(
int ij5 = 0; ij5 < 1; ++ij5)
4164 _ij5[0] = ij5; _ij5[1] = -1;
4165 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4169 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4172 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4175 IkReal x306=
IKcos(j5);
4176 IkReal x307=
IKsin(j5);
4177 IkReal x308=((1.0)*cj3);
4178 IkReal x309=(sj3*x306);
4179 IkReal x310=((1.0)*x307);
4180 IkReal x311=(x307*x308);
4181 evalcond[0]=(((new_r10*sj3))+x306+((cj3*new_r00)));
4182 evalcond[1]=(((new_r00*sj3))+x307+(((-1.0)*new_r10*x308)));
4183 evalcond[2]=(((new_r01*sj3))+x306+(((-1.0)*new_r11*x308)));
4184 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x310))+((cj3*new_r01)));
4185 evalcond[4]=(((sj3*x307))+((cj3*x306))+new_r00);
4186 evalcond[5]=(x309+(((-1.0)*x311))+new_r01);
4187 evalcond[6]=(x309+(((-1.0)*x311))+new_r10);
4188 evalcond[7]=((((-1.0)*sj3*x310))+(((-1.0)*x306*x308))+new_r11);
4196 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4197 vinfos[0].jointtype = 1;
4198 vinfos[0].foffset = j0;
4199 vinfos[0].indices[0] = _ij0[0];
4200 vinfos[0].indices[1] = _ij0[1];
4201 vinfos[0].maxsolutions = _nj0;
4202 vinfos[1].jointtype = 1;
4203 vinfos[1].foffset = j1;
4204 vinfos[1].indices[0] = _ij1[0];
4205 vinfos[1].indices[1] = _ij1[1];
4206 vinfos[1].maxsolutions = _nj1;
4207 vinfos[2].jointtype = 1;
4208 vinfos[2].foffset = j2;
4209 vinfos[2].indices[0] = _ij2[0];
4210 vinfos[2].indices[1] = _ij2[1];
4211 vinfos[2].maxsolutions = _nj2;
4212 vinfos[3].jointtype = 1;
4213 vinfos[3].foffset = j3;
4214 vinfos[3].indices[0] = _ij3[0];
4215 vinfos[3].indices[1] = _ij3[1];
4216 vinfos[3].maxsolutions = _nj3;
4217 vinfos[4].jointtype = 1;
4218 vinfos[4].foffset = j4;
4219 vinfos[4].indices[0] = _ij4[0];
4220 vinfos[4].indices[1] = _ij4[1];
4221 vinfos[4].maxsolutions = _nj4;
4222 vinfos[5].jointtype = 1;
4223 vinfos[5].foffset = j5;
4224 vinfos[5].indices[0] = _ij5[0];
4225 vinfos[5].indices[1] = _ij5[1];
4226 vinfos[5].maxsolutions = _nj5;
4227 std::vector<int> vfree(0);
4235 if( bgotonextstatement )
4237 bool bgotonextstatement =
true;
4240 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4241 evalcond[1]=new_r02;
4242 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4244 bgotonextstatement=
false;
4246 IkReal j5array[1], cj5array[1], sj5array[1];
4247 bool j5valid[1]={
false};
4251 j5array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4252 sj5array[0]=
IKsin(j5array[0]);
4253 cj5array[0]=
IKcos(j5array[0]);
4254 if( j5array[0] >
IKPI )
4258 else if( j5array[0] < -
IKPI )
4259 { j5array[0]+=
IK2PI;
4262 for(
int ij5 = 0; ij5 < 1; ++ij5)
4268 _ij5[0] = ij5; _ij5[1] = -1;
4269 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4273 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4276 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4279 IkReal x312=
IKsin(j5);
4280 IkReal x313=
IKcos(j5);
4281 IkReal x314=((1.0)*cj4);
4282 IkReal x315=((1.0)*sj4);
4283 evalcond[0]=(x312+new_r00);
4284 evalcond[1]=(x313+new_r01);
4285 evalcond[2]=(((sj4*x312))+new_r21);
4286 evalcond[3]=(((cj4*x312))+new_r11);
4287 evalcond[4]=((((-1.0)*x313*x315))+new_r20);
4288 evalcond[5]=((((-1.0)*x313*x314))+new_r10);
4289 evalcond[6]=((((-1.0)*new_r20*x315))+x313+(((-1.0)*new_r10*x314)));
4290 evalcond[7]=((((-1.0)*new_r21*x315))+(((-1.0)*new_r11*x314))+(((-1.0)*x312)));
4298 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4299 vinfos[0].jointtype = 1;
4300 vinfos[0].foffset = j0;
4301 vinfos[0].indices[0] = _ij0[0];
4302 vinfos[0].indices[1] = _ij0[1];
4303 vinfos[0].maxsolutions = _nj0;
4304 vinfos[1].jointtype = 1;
4305 vinfos[1].foffset = j1;
4306 vinfos[1].indices[0] = _ij1[0];
4307 vinfos[1].indices[1] = _ij1[1];
4308 vinfos[1].maxsolutions = _nj1;
4309 vinfos[2].jointtype = 1;
4310 vinfos[2].foffset = j2;
4311 vinfos[2].indices[0] = _ij2[0];
4312 vinfos[2].indices[1] = _ij2[1];
4313 vinfos[2].maxsolutions = _nj2;
4314 vinfos[3].jointtype = 1;
4315 vinfos[3].foffset = j3;
4316 vinfos[3].indices[0] = _ij3[0];
4317 vinfos[3].indices[1] = _ij3[1];
4318 vinfos[3].maxsolutions = _nj3;
4319 vinfos[4].jointtype = 1;
4320 vinfos[4].foffset = j4;
4321 vinfos[4].indices[0] = _ij4[0];
4322 vinfos[4].indices[1] = _ij4[1];
4323 vinfos[4].maxsolutions = _nj4;
4324 vinfos[5].jointtype = 1;
4325 vinfos[5].foffset = j5;
4326 vinfos[5].indices[0] = _ij5[0];
4327 vinfos[5].indices[1] = _ij5[1];
4328 vinfos[5].maxsolutions = _nj5;
4329 std::vector<int> vfree(0);
4337 if( bgotonextstatement )
4339 bool bgotonextstatement =
true;
4342 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4343 evalcond[1]=new_r02;
4344 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4346 bgotonextstatement=
false;
4348 IkReal j5array[1], cj5array[1], sj5array[1];
4349 bool j5valid[1]={
false};
4353 j5array[0]=
IKatan2(new_r00, new_r01);
4354 sj5array[0]=
IKsin(j5array[0]);
4355 cj5array[0]=
IKcos(j5array[0]);
4356 if( j5array[0] >
IKPI )
4360 else if( j5array[0] < -
IKPI )
4361 { j5array[0]+=
IK2PI;
4364 for(
int ij5 = 0; ij5 < 1; ++ij5)
4370 _ij5[0] = ij5; _ij5[1] = -1;
4371 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4375 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4378 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4381 IkReal x316=
IKsin(j5);
4382 IkReal x317=
IKcos(j5);
4383 IkReal x318=((1.0)*sj4);
4384 IkReal x319=((1.0)*x317);
4385 evalcond[0]=(((sj4*x316))+new_r21);
4386 evalcond[1]=(x316+(((-1.0)*new_r00)));
4387 evalcond[2]=(x317+(((-1.0)*new_r01)));
4388 evalcond[3]=(new_r20+(((-1.0)*x317*x318)));
4389 evalcond[4]=(((cj4*x316))+(((-1.0)*new_r11)));
4390 evalcond[5]=((((-1.0)*cj4*x319))+(((-1.0)*new_r10)));
4391 evalcond[6]=((((-1.0)*new_r20*x318))+((cj4*new_r10))+x317);
4392 evalcond[7]=((((-1.0)*new_r21*x318))+((cj4*new_r11))+(((-1.0)*x316)));
4400 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4401 vinfos[0].jointtype = 1;
4402 vinfos[0].foffset = j0;
4403 vinfos[0].indices[0] = _ij0[0];
4404 vinfos[0].indices[1] = _ij0[1];
4405 vinfos[0].maxsolutions = _nj0;
4406 vinfos[1].jointtype = 1;
4407 vinfos[1].foffset = j1;
4408 vinfos[1].indices[0] = _ij1[0];
4409 vinfos[1].indices[1] = _ij1[1];
4410 vinfos[1].maxsolutions = _nj1;
4411 vinfos[2].jointtype = 1;
4412 vinfos[2].foffset = j2;
4413 vinfos[2].indices[0] = _ij2[0];
4414 vinfos[2].indices[1] = _ij2[1];
4415 vinfos[2].maxsolutions = _nj2;
4416 vinfos[3].jointtype = 1;
4417 vinfos[3].foffset = j3;
4418 vinfos[3].indices[0] = _ij3[0];
4419 vinfos[3].indices[1] = _ij3[1];
4420 vinfos[3].maxsolutions = _nj3;
4421 vinfos[4].jointtype = 1;
4422 vinfos[4].foffset = j4;
4423 vinfos[4].indices[0] = _ij4[0];
4424 vinfos[4].indices[1] = _ij4[1];
4425 vinfos[4].maxsolutions = _nj4;
4426 vinfos[5].jointtype = 1;
4427 vinfos[5].foffset = j5;
4428 vinfos[5].indices[0] = _ij5[0];
4429 vinfos[5].indices[1] = _ij5[1];
4430 vinfos[5].maxsolutions = _nj5;
4431 std::vector<int> vfree(0);
4439 if( bgotonextstatement )
4441 bool bgotonextstatement =
true;
4444 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
4445 evalcond[1]=new_r12;
4446 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4448 bgotonextstatement=
false;
4450 IkReal j5array[1], cj5array[1], sj5array[1];
4451 bool j5valid[1]={
false};
4455 j5array[0]=
IKatan2(new_r10, new_r11);
4456 sj5array[0]=
IKsin(j5array[0]);
4457 cj5array[0]=
IKcos(j5array[0]);
4458 if( j5array[0] >
IKPI )
4462 else if( j5array[0] < -
IKPI )
4463 { j5array[0]+=
IK2PI;
4466 for(
int ij5 = 0; ij5 < 1; ++ij5)
4472 _ij5[0] = ij5; _ij5[1] = -1;
4473 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4477 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4480 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4483 IkReal x320=
IKcos(j5);
4484 IkReal x321=
IKsin(j5);
4485 IkReal x322=((1.0)*cj4);
4486 IkReal x323=((1.0)*sj4);
4487 IkReal x324=((1.0)*x321);
4488 evalcond[0]=(new_r20+((new_r02*x320)));
4489 evalcond[1]=(x321+(((-1.0)*new_r10)));
4490 evalcond[2]=(x320+(((-1.0)*new_r11)));
4491 evalcond[3]=(((cj4*x321))+new_r01);
4492 evalcond[4]=((((-1.0)*new_r02*x324))+new_r21);
4493 evalcond[5]=((((-1.0)*x320*x322))+new_r00);
4494 evalcond[6]=(x320+(((-1.0)*new_r00*x322))+(((-1.0)*new_r20*x323)));
4495 evalcond[7]=((((-1.0)*x324))+(((-1.0)*new_r01*x322))+(((-1.0)*new_r21*x323)));
4503 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4504 vinfos[0].jointtype = 1;
4505 vinfos[0].foffset = j0;
4506 vinfos[0].indices[0] = _ij0[0];
4507 vinfos[0].indices[1] = _ij0[1];
4508 vinfos[0].maxsolutions = _nj0;
4509 vinfos[1].jointtype = 1;
4510 vinfos[1].foffset = j1;
4511 vinfos[1].indices[0] = _ij1[0];
4512 vinfos[1].indices[1] = _ij1[1];
4513 vinfos[1].maxsolutions = _nj1;
4514 vinfos[2].jointtype = 1;
4515 vinfos[2].foffset = j2;
4516 vinfos[2].indices[0] = _ij2[0];
4517 vinfos[2].indices[1] = _ij2[1];
4518 vinfos[2].maxsolutions = _nj2;
4519 vinfos[3].jointtype = 1;
4520 vinfos[3].foffset = j3;
4521 vinfos[3].indices[0] = _ij3[0];
4522 vinfos[3].indices[1] = _ij3[1];
4523 vinfos[3].maxsolutions = _nj3;
4524 vinfos[4].jointtype = 1;
4525 vinfos[4].foffset = j4;
4526 vinfos[4].indices[0] = _ij4[0];
4527 vinfos[4].indices[1] = _ij4[1];
4528 vinfos[4].maxsolutions = _nj4;
4529 vinfos[5].jointtype = 1;
4530 vinfos[5].foffset = j5;
4531 vinfos[5].indices[0] = _ij5[0];
4532 vinfos[5].indices[1] = _ij5[1];
4533 vinfos[5].maxsolutions = _nj5;
4534 std::vector<int> vfree(0);
4542 if( bgotonextstatement )
4544 bool bgotonextstatement =
true;
4547 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4548 evalcond[1]=new_r12;
4549 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4551 bgotonextstatement=
false;
4553 IkReal j5array[1], cj5array[1], sj5array[1];
4554 bool j5valid[1]={
false};
4558 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4559 sj5array[0]=
IKsin(j5array[0]);
4560 cj5array[0]=
IKcos(j5array[0]);
4561 if( j5array[0] >
IKPI )
4565 else if( j5array[0] < -
IKPI )
4566 { j5array[0]+=
IK2PI;
4569 for(
int ij5 = 0; ij5 < 1; ++ij5)
4575 _ij5[0] = ij5; _ij5[1] = -1;
4576 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4580 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4583 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4586 IkReal x325=
IKsin(j5);
4587 IkReal x326=
IKcos(j5);
4588 IkReal x327=((1.0)*sj4);
4589 IkReal x328=((1.0)*x326);
4590 evalcond[0]=(x325+new_r10);
4591 evalcond[1]=(x326+new_r11);
4592 evalcond[2]=(new_r21+((new_r02*x325)));
4593 evalcond[3]=((((-1.0)*new_r02*x328))+new_r20);
4594 evalcond[4]=(((cj4*x325))+(((-1.0)*new_r01)));
4595 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x328)));
4596 evalcond[6]=(((cj4*new_r00))+x326+(((-1.0)*new_r20*x327)));
4597 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x325))+(((-1.0)*new_r21*x327)));
4605 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4606 vinfos[0].jointtype = 1;
4607 vinfos[0].foffset = j0;
4608 vinfos[0].indices[0] = _ij0[0];
4609 vinfos[0].indices[1] = _ij0[1];
4610 vinfos[0].maxsolutions = _nj0;
4611 vinfos[1].jointtype = 1;
4612 vinfos[1].foffset = j1;
4613 vinfos[1].indices[0] = _ij1[0];
4614 vinfos[1].indices[1] = _ij1[1];
4615 vinfos[1].maxsolutions = _nj1;
4616 vinfos[2].jointtype = 1;
4617 vinfos[2].foffset = j2;
4618 vinfos[2].indices[0] = _ij2[0];
4619 vinfos[2].indices[1] = _ij2[1];
4620 vinfos[2].maxsolutions = _nj2;
4621 vinfos[3].jointtype = 1;
4622 vinfos[3].foffset = j3;
4623 vinfos[3].indices[0] = _ij3[0];
4624 vinfos[3].indices[1] = _ij3[1];
4625 vinfos[3].maxsolutions = _nj3;
4626 vinfos[4].jointtype = 1;
4627 vinfos[4].foffset = j4;
4628 vinfos[4].indices[0] = _ij4[0];
4629 vinfos[4].indices[1] = _ij4[1];
4630 vinfos[4].maxsolutions = _nj4;
4631 vinfos[5].jointtype = 1;
4632 vinfos[5].foffset = j5;
4633 vinfos[5].indices[0] = _ij5[0];
4634 vinfos[5].indices[1] = _ij5[1];
4635 vinfos[5].maxsolutions = _nj5;
4636 std::vector<int> vfree(0);
4644 if( bgotonextstatement )
4646 bool bgotonextstatement =
true;
4649 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
4650 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4652 bgotonextstatement=
false;
4660 if(
IKabs(j5eval[0]) < 0.0000000100000000 )
4666 IkReal op[2+1], zeror[2];
4671 polyroots2(op,zeror,numroots);
4672 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4673 int numsolutions = 0;
4674 for(
int ij5 = 0; ij5 < numroots; ++ij5)
4676 IkReal htj5 = zeror[ij5];
4677 tempj5array[0]=((2.0)*(atan(htj5)));
4678 for(
int kj5 = 0; kj5 < 1; ++kj5)
4680 j5array[numsolutions] = tempj5array[kj5];
4681 if( j5array[numsolutions] >
IKPI )
4683 j5array[numsolutions]-=
IK2PI;
4685 else if( j5array[numsolutions] < -
IKPI )
4687 j5array[numsolutions]+=
IK2PI;
4689 sj5array[numsolutions] =
IKsin(j5array[numsolutions]);
4690 cj5array[numsolutions] =
IKcos(j5array[numsolutions]);
4694 bool j5valid[2]={
true,
true};
4696 for(
int ij5 = 0; ij5 < numsolutions; ++ij5)
4702 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4705 _ij5[0] = ij5; _ij5[1] = -1;
4706 for(
int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4710 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4714 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4715 vinfos[0].jointtype = 1;
4716 vinfos[0].foffset = j0;
4717 vinfos[0].indices[0] = _ij0[0];
4718 vinfos[0].indices[1] = _ij0[1];
4719 vinfos[0].maxsolutions = _nj0;
4720 vinfos[1].jointtype = 1;
4721 vinfos[1].foffset = j1;
4722 vinfos[1].indices[0] = _ij1[0];
4723 vinfos[1].indices[1] = _ij1[1];
4724 vinfos[1].maxsolutions = _nj1;
4725 vinfos[2].jointtype = 1;
4726 vinfos[2].foffset = j2;
4727 vinfos[2].indices[0] = _ij2[0];
4728 vinfos[2].indices[1] = _ij2[1];
4729 vinfos[2].maxsolutions = _nj2;
4730 vinfos[3].jointtype = 1;
4731 vinfos[3].foffset = j3;
4732 vinfos[3].indices[0] = _ij3[0];
4733 vinfos[3].indices[1] = _ij3[1];
4734 vinfos[3].maxsolutions = _nj3;
4735 vinfos[4].jointtype = 1;
4736 vinfos[4].foffset = j4;
4737 vinfos[4].indices[0] = _ij4[0];
4738 vinfos[4].indices[1] = _ij4[1];
4739 vinfos[4].maxsolutions = _nj4;
4740 vinfos[5].jointtype = 1;
4741 vinfos[5].foffset = j5;
4742 vinfos[5].indices[0] = _ij5[0];
4743 vinfos[5].indices[1] = _ij5[1];
4744 vinfos[5].maxsolutions = _nj5;
4745 std::vector<int> vfree(0);
4756 if( bgotonextstatement )
4758 bool bgotonextstatement =
true;
4763 bgotonextstatement=
false;
4768 if( bgotonextstatement )
4783 IkReal j5array[1], cj5array[1], sj5array[1];
4784 bool j5valid[1]={
false};
4790 IkReal x329=x330.
value;
4797 j5array[0]=
IKatan2(((-1.0)*new_r21*x329), (x329*(x331.
value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4798 sj5array[0]=
IKsin(j5array[0]);
4799 cj5array[0]=
IKcos(j5array[0]);
4800 if( j5array[0] >
IKPI )
4804 else if( j5array[0] < -
IKPI )
4805 { j5array[0]+=
IK2PI;
4808 for(
int ij5 = 0; ij5 < 1; ++ij5)
4814 _ij5[0] = ij5; _ij5[1] = -1;
4815 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4819 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4822 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4824 IkReal evalcond[12];
4825 IkReal x332=
IKsin(j5);
4826 IkReal x333=
IKcos(j5);
4827 IkReal x334=(cj3*new_r00);
4828 IkReal x335=(cj3*cj4);
4829 IkReal x336=((1.0)*cj3);
4830 IkReal x337=(new_r11*sj3);
4831 IkReal x338=((1.0)*cj4);
4832 IkReal x339=(new_r10*sj3);
4833 IkReal x340=((1.0)*sj4);
4834 IkReal x341=(sj3*x332);
4835 IkReal x342=((1.0)*x333);
4836 IkReal x343=(sj3*x333);
4837 evalcond[0]=(((sj4*x332))+new_r21);
4838 evalcond[1]=(new_r20+(((-1.0)*x333*x340)));
4839 evalcond[2]=((((-1.0)*new_r10*x336))+((new_r00*sj3))+x332);
4840 evalcond[3]=((((-1.0)*new_r11*x336))+((new_r01*sj3))+x333);
4841 evalcond[4]=(x337+((cj4*x332))+((cj3*new_r01)));
4842 evalcond[5]=(((x332*x335))+x343+new_r01);
4843 evalcond[6]=((((-1.0)*x333*x338))+x339+x334);
4844 evalcond[7]=((((-1.0)*x335*x342))+x341+new_r00);
4845 evalcond[8]=((((-1.0)*x333*x336))+((cj4*x341))+new_r11);
4846 evalcond[9]=((((-1.0)*x332*x336))+new_r10+(((-1.0)*x338*x343)));
4847 evalcond[10]=((((-1.0)*x338*x339))+x333+(((-1.0)*new_r20*x340))+(((-1.0)*x334*x338)));
4848 evalcond[11]=((((-1.0)*x337*x338))+(((-1.0)*new_r01*x335))+(((-1.0)*x332))+(((-1.0)*new_r21*x340)));
4856 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4857 vinfos[0].jointtype = 1;
4858 vinfos[0].foffset = j0;
4859 vinfos[0].indices[0] = _ij0[0];
4860 vinfos[0].indices[1] = _ij0[1];
4861 vinfos[0].maxsolutions = _nj0;
4862 vinfos[1].jointtype = 1;
4863 vinfos[1].foffset = j1;
4864 vinfos[1].indices[0] = _ij1[0];
4865 vinfos[1].indices[1] = _ij1[1];
4866 vinfos[1].maxsolutions = _nj1;
4867 vinfos[2].jointtype = 1;
4868 vinfos[2].foffset = j2;
4869 vinfos[2].indices[0] = _ij2[0];
4870 vinfos[2].indices[1] = _ij2[1];
4871 vinfos[2].maxsolutions = _nj2;
4872 vinfos[3].jointtype = 1;
4873 vinfos[3].foffset = j3;
4874 vinfos[3].indices[0] = _ij3[0];
4875 vinfos[3].indices[1] = _ij3[1];
4876 vinfos[3].maxsolutions = _nj3;
4877 vinfos[4].jointtype = 1;
4878 vinfos[4].foffset = j4;
4879 vinfos[4].indices[0] = _ij4[0];
4880 vinfos[4].indices[1] = _ij4[1];
4881 vinfos[4].maxsolutions = _nj4;
4882 vinfos[5].jointtype = 1;
4883 vinfos[5].foffset = j5;
4884 vinfos[5].indices[0] = _ij5[0];
4885 vinfos[5].indices[1] = _ij5[1];
4886 vinfos[5].maxsolutions = _nj5;
4887 std::vector<int> vfree(0);
4900 IkReal j5array[1], cj5array[1], sj5array[1];
4901 bool j5valid[1]={
false};
4907 IkReal x344=x345.
value;
4914 j5array[0]=
IKatan2(((-1.0)*new_r21*x344), (x344*(x346.
value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4915 sj5array[0]=
IKsin(j5array[0]);
4916 cj5array[0]=
IKcos(j5array[0]);
4917 if( j5array[0] >
IKPI )
4921 else if( j5array[0] < -
IKPI )
4922 { j5array[0]+=
IK2PI;
4925 for(
int ij5 = 0; ij5 < 1; ++ij5)
4931 _ij5[0] = ij5; _ij5[1] = -1;
4932 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4936 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4939 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4941 IkReal evalcond[12];
4942 IkReal x347=
IKsin(j5);
4943 IkReal x348=
IKcos(j5);
4944 IkReal x349=(cj3*new_r00);
4945 IkReal x350=(cj3*cj4);
4946 IkReal x351=((1.0)*cj3);
4947 IkReal x352=(new_r11*sj3);
4948 IkReal x353=((1.0)*cj4);
4949 IkReal x354=(new_r10*sj3);
4950 IkReal x355=((1.0)*sj4);
4951 IkReal x356=(sj3*x347);
4952 IkReal x357=((1.0)*x348);
4953 IkReal x358=(sj3*x348);
4954 evalcond[0]=(((sj4*x347))+new_r21);
4955 evalcond[1]=((((-1.0)*x348*x355))+new_r20);
4956 evalcond[2]=(((new_r00*sj3))+x347+(((-1.0)*new_r10*x351)));
4957 evalcond[3]=(((new_r01*sj3))+x348+(((-1.0)*new_r11*x351)));
4958 evalcond[4]=(x352+((cj4*x347))+((cj3*new_r01)));
4959 evalcond[5]=(x358+((x347*x350))+new_r01);
4960 evalcond[6]=((((-1.0)*x348*x353))+x354+x349);
4961 evalcond[7]=(x356+new_r00+(((-1.0)*x350*x357)));
4962 evalcond[8]=((((-1.0)*x348*x351))+((cj4*x356))+new_r11);
4963 evalcond[9]=((((-1.0)*x347*x351))+(((-1.0)*x353*x358))+new_r10);
4964 evalcond[10]=((((-1.0)*x349*x353))+x348+(((-1.0)*x353*x354))+(((-1.0)*new_r20*x355)));
4965 evalcond[11]=((((-1.0)*new_r01*x350))+(((-1.0)*new_r21*x355))+(((-1.0)*x352*x353))+(((-1.0)*x347)));
4973 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4974 vinfos[0].jointtype = 1;
4975 vinfos[0].foffset = j0;
4976 vinfos[0].indices[0] = _ij0[0];
4977 vinfos[0].indices[1] = _ij0[1];
4978 vinfos[0].maxsolutions = _nj0;
4979 vinfos[1].jointtype = 1;
4980 vinfos[1].foffset = j1;
4981 vinfos[1].indices[0] = _ij1[0];
4982 vinfos[1].indices[1] = _ij1[1];
4983 vinfos[1].maxsolutions = _nj1;
4984 vinfos[2].jointtype = 1;
4985 vinfos[2].foffset = j2;
4986 vinfos[2].indices[0] = _ij2[0];
4987 vinfos[2].indices[1] = _ij2[1];
4988 vinfos[2].maxsolutions = _nj2;
4989 vinfos[3].jointtype = 1;
4990 vinfos[3].foffset = j3;
4991 vinfos[3].indices[0] = _ij3[0];
4992 vinfos[3].indices[1] = _ij3[1];
4993 vinfos[3].maxsolutions = _nj3;
4994 vinfos[4].jointtype = 1;
4995 vinfos[4].foffset = j4;
4996 vinfos[4].indices[0] = _ij4[0];
4997 vinfos[4].indices[1] = _ij4[1];
4998 vinfos[4].maxsolutions = _nj4;
4999 vinfos[5].jointtype = 1;
5000 vinfos[5].foffset = j5;
5001 vinfos[5].indices[0] = _ij5[0];
5002 vinfos[5].indices[1] = _ij5[1];
5003 vinfos[5].maxsolutions = _nj5;
5004 std::vector<int> vfree(0);
5017 IkReal j5array[1], cj5array[1], sj5array[1];
5018 bool j5valid[1]={
false};
5028 j5array[0]=((-1.5707963267949)+(x359.
value)+(((1.5707963267949)*(x360.
value))));
5029 sj5array[0]=
IKsin(j5array[0]);
5030 cj5array[0]=
IKcos(j5array[0]);
5031 if( j5array[0] >
IKPI )
5035 else if( j5array[0] < -
IKPI )
5036 { j5array[0]+=
IK2PI;
5039 for(
int ij5 = 0; ij5 < 1; ++ij5)
5045 _ij5[0] = ij5; _ij5[1] = -1;
5046 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
5050 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
5053 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5055 IkReal evalcond[12];
5056 IkReal x361=
IKsin(j5);
5057 IkReal x362=
IKcos(j5);
5058 IkReal x363=(cj3*new_r00);
5059 IkReal x364=(cj3*cj4);
5060 IkReal x365=((1.0)*cj3);
5061 IkReal x366=(new_r11*sj3);
5062 IkReal x367=((1.0)*cj4);
5063 IkReal x368=(new_r10*sj3);
5064 IkReal x369=((1.0)*sj4);
5065 IkReal x370=(sj3*x361);
5066 IkReal x371=((1.0)*x362);
5067 IkReal x372=(sj3*x362);
5068 evalcond[0]=(((sj4*x361))+new_r21);
5069 evalcond[1]=((((-1.0)*x362*x369))+new_r20);
5070 evalcond[2]=(((new_r00*sj3))+x361+(((-1.0)*new_r10*x365)));
5071 evalcond[3]=(((new_r01*sj3))+x362+(((-1.0)*new_r11*x365)));
5072 evalcond[4]=(((cj4*x361))+x366+((cj3*new_r01)));
5073 evalcond[5]=(((x361*x364))+x372+new_r01);
5074 evalcond[6]=((((-1.0)*x362*x367))+x368+x363);
5075 evalcond[7]=(x370+new_r00+(((-1.0)*x364*x371)));
5076 evalcond[8]=((((-1.0)*x362*x365))+((cj4*x370))+new_r11);
5077 evalcond[9]=((((-1.0)*x361*x365))+(((-1.0)*x367*x372))+new_r10);
5078 evalcond[10]=((((-1.0)*x363*x367))+(((-1.0)*new_r20*x369))+(((-1.0)*x367*x368))+x362);
5079 evalcond[11]=((((-1.0)*x361))+(((-1.0)*x366*x367))+(((-1.0)*new_r01*x364))+(((-1.0)*new_r21*x369)));
5087 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5088 vinfos[0].jointtype = 1;
5089 vinfos[0].foffset = j0;
5090 vinfos[0].indices[0] = _ij0[0];
5091 vinfos[0].indices[1] = _ij0[1];
5092 vinfos[0].maxsolutions = _nj0;
5093 vinfos[1].jointtype = 1;
5094 vinfos[1].foffset = j1;
5095 vinfos[1].indices[0] = _ij1[0];
5096 vinfos[1].indices[1] = _ij1[1];
5097 vinfos[1].maxsolutions = _nj1;
5098 vinfos[2].jointtype = 1;
5099 vinfos[2].foffset = j2;
5100 vinfos[2].indices[0] = _ij2[0];
5101 vinfos[2].indices[1] = _ij2[1];
5102 vinfos[2].maxsolutions = _nj2;
5103 vinfos[3].jointtype = 1;
5104 vinfos[3].foffset = j3;
5105 vinfos[3].indices[0] = _ij3[0];
5106 vinfos[3].indices[1] = _ij3[1];
5107 vinfos[3].maxsolutions = _nj3;
5108 vinfos[4].jointtype = 1;
5109 vinfos[4].foffset = j4;
5110 vinfos[4].indices[0] = _ij4[0];
5111 vinfos[4].indices[1] = _ij4[1];
5112 vinfos[4].maxsolutions = _nj4;
5113 vinfos[5].jointtype = 1;
5114 vinfos[5].foffset = j5;
5115 vinfos[5].indices[0] = _ij5[0];
5116 vinfos[5].indices[1] = _ij5[1];
5117 vinfos[5].maxsolutions = _nj5;
5118 std::vector<int> vfree(0);
5137 IkReal j5array[1], cj5array[1], sj5array[1];
5138 bool j5valid[1]={
false};
5148 j5array[0]=((-1.5707963267949)+(x373.
value)+(((1.5707963267949)*(x374.
value))));
5149 sj5array[0]=
IKsin(j5array[0]);
5150 cj5array[0]=
IKcos(j5array[0]);
5151 if( j5array[0] >
IKPI )
5155 else if( j5array[0] < -
IKPI )
5156 { j5array[0]+=
IK2PI;
5159 for(
int ij5 = 0; ij5 < 1; ++ij5)
5165 _ij5[0] = ij5; _ij5[1] = -1;
5166 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
5170 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
5173 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5176 evalcond[0]=(((sj4*(
IKsin(j5))))+new_r21);
5177 evalcond[1]=((((-1.0)*sj4*(
IKcos(j5))))+new_r20);
5188 j3eval[2]=((
IKabs(new_r12))+(
IKabs(new_r02)));
5189 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5195 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
5199 bool bgotonextstatement =
true;
5202 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
5203 evalcond[1]=new_r02;
5204 evalcond[2]=new_r12;
5205 evalcond[3]=new_r21;
5206 evalcond[4]=new_r20;
5207 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
5209 bgotonextstatement=
false;
5215 IkReal x375=((1.0)*new_r11);
5216 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5218 j3eval[1]=((
IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(
IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5220 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5227 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5229 j3eval[1]=((
IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(
IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5231 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5238 IkReal x378=((1.0)*new_r11);
5239 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5242 j3eval[2]=((
IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(
IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5243 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5247 bool bgotonextstatement =
true;
5250 IkReal x380=((-1.0)*new_r01);
5251 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5255 IkReal x381=pow(x382,-0.5);
5260 IkReal gconst0=((-1.0)*(x383.
value));
5261 IkReal gconst1=(new_r11*x381);
5262 IkReal gconst2=(x380*x381);
5267 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((x384.
value)+j5)))), 6.28318530717959)));
5268 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5270 bgotonextstatement=
false;
5273 IkReal x385=((-1.0)*new_r01);
5278 IkReal x386=((-1.0)*(x388.
value));
5286 IkReal gconst0=x386;
5287 IkReal gconst1=(new_r11*x387);
5288 IkReal gconst2=(x385*x387);
5289 IkReal x389=new_r11*new_r11;
5290 IkReal x390=(new_r10*new_r11);
5291 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5293 IkReal x393=(new_r11*x392);
5295 j3eval[1]=((
IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(
IKabs((((new_r01*x393))+((x390*x392))))));
5297 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5301 IkReal x394=((-1.0)*new_r01);
5306 IkReal x395=((-1.0)*(x397.
value));
5314 IkReal gconst0=x395;
5315 IkReal gconst1=(new_r11*x396);
5316 IkReal gconst2=(x394*x396);
5317 IkReal x398=new_r11*new_r11;
5322 IkReal x399=x401.
value;
5323 IkReal x400=(x398*x399);
5324 j3eval[0]=((
IKabs((((new_r01*new_r10))+x400)))+(
IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5325 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5329 IkReal x402=((-1.0)*new_r01);
5334 IkReal x403=((-1.0)*(x405.
value));
5342 IkReal gconst0=x403;
5343 IkReal gconst1=(new_r11*x404);
5344 IkReal gconst2=(x402*x404);
5345 IkReal x406=new_r01*new_r01;
5346 IkReal x407=new_r11*new_r11;
5351 IkReal x408=x414.
value;
5352 IkReal x409=(x407*x408);
5357 IkReal x410=x415.
value;
5358 IkReal x411=((1.0)*x410);
5359 IkReal x412=(new_r11*x411);
5360 IkReal x413=(new_r01*x411);
5361 j3eval[0]=((
IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(
IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5362 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5366 bool bgotonextstatement =
true;
5369 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
5370 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5372 bgotonextstatement=
false;
5374 IkReal j3array[2], cj3array[2], sj3array[2];
5375 bool j3valid[2]={
false};
5381 sj3array[0]=(new_r10*(x416.
value));
5384 j3valid[0] = j3valid[1] =
true;
5385 j3array[0] =
IKasin(sj3array[0]);
5386 cj3array[0] =
IKcos(j3array[0]);
5387 sj3array[1] = sj3array[0];
5388 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
5389 cj3array[1] = -cj3array[0];
5391 else if( isnan(sj3array[0]) )
5395 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5397 for(
int ij3 = 0; ij3 < 2; ++ij3)
5403 _ij3[0] = ij3; _ij3[1] = -1;
5404 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
5408 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5411 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5414 IkReal x417=
IKcos(j3);
5415 IkReal x418=
IKsin(j3);
5416 IkReal x419=((-1.0)*x417);
5417 evalcond[0]=(new_r01*x417);
5418 evalcond[1]=(new_r10*x419);
5419 evalcond[2]=(gconst2*x419);
5420 evalcond[3]=(gconst2+((new_r01*x418)));
5421 evalcond[4]=(((gconst2*x418))+new_r01);
5422 evalcond[5]=((((-1.0)*gconst2))+((new_r10*x418)));
5430 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5431 vinfos[0].jointtype = 1;
5432 vinfos[0].foffset = j0;
5433 vinfos[0].indices[0] = _ij0[0];
5434 vinfos[0].indices[1] = _ij0[1];
5435 vinfos[0].maxsolutions = _nj0;
5436 vinfos[1].jointtype = 1;
5437 vinfos[1].foffset = j1;
5438 vinfos[1].indices[0] = _ij1[0];
5439 vinfos[1].indices[1] = _ij1[1];
5440 vinfos[1].maxsolutions = _nj1;
5441 vinfos[2].jointtype = 1;
5442 vinfos[2].foffset = j2;
5443 vinfos[2].indices[0] = _ij2[0];
5444 vinfos[2].indices[1] = _ij2[1];
5445 vinfos[2].maxsolutions = _nj2;
5446 vinfos[3].jointtype = 1;
5447 vinfos[3].foffset = j3;
5448 vinfos[3].indices[0] = _ij3[0];
5449 vinfos[3].indices[1] = _ij3[1];
5450 vinfos[3].maxsolutions = _nj3;
5451 vinfos[4].jointtype = 1;
5452 vinfos[4].foffset = j4;
5453 vinfos[4].indices[0] = _ij4[0];
5454 vinfos[4].indices[1] = _ij4[1];
5455 vinfos[4].maxsolutions = _nj4;
5456 vinfos[5].jointtype = 1;
5457 vinfos[5].foffset = j5;
5458 vinfos[5].indices[0] = _ij5[0];
5459 vinfos[5].indices[1] = _ij5[1];
5460 vinfos[5].maxsolutions = _nj5;
5461 std::vector<int> vfree(0);
5469 if( bgotonextstatement )
5471 bool bgotonextstatement =
true;
5474 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
5475 evalcond[1]=gconst1;
5476 evalcond[2]=gconst2;
5477 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
5479 bgotonextstatement=
false;
5482 IkReal x420=((-1.0)*new_r01);
5487 IkReal x421=((-1.0)*(x422.
value));
5498 IkReal gconst0=x421;
5499 IkReal gconst1=new_r11;
5500 IkReal gconst2=x420;
5503 j3eval[2]=((
IKabs((new_r01*new_r11)))+(
IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5504 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5508 IkReal x423=((-1.0)*new_r01);
5513 IkReal x424=((-1.0)*(x425.
value));
5524 IkReal gconst0=x424;
5525 IkReal gconst1=new_r11;
5526 IkReal gconst2=x423;
5528 j3eval[1]=((
IKabs(new_r01*new_r01))+(
IKabs((new_r01*new_r11))));
5530 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5534 IkReal x426=((-1.0)*new_r01);
5539 IkReal x427=((-1.0)*(x428.
value));
5550 IkReal gconst0=x427;
5551 IkReal gconst1=new_r11;
5552 IkReal gconst2=x426;
5555 j3eval[2]=((((0.5)*(
IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(
IKabs((new_r01*new_r11))));
5556 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5563 IkReal j3array[1], cj3array[1], sj3array[1];
5564 bool j3valid[1]={
false};
5566 IkReal x429=((1.0)*new_r01);
5575 j3array[0]=((-1.5707963267949)+(x430.
value)+(((1.5707963267949)*(x431.
value))));
5576 sj3array[0]=
IKsin(j3array[0]);
5577 cj3array[0]=
IKcos(j3array[0]);
5578 if( j3array[0] >
IKPI )
5582 else if( j3array[0] < -
IKPI )
5583 { j3array[0]+=
IK2PI;
5586 for(
int ij3 = 0; ij3 < 1; ++ij3)
5592 _ij3[0] = ij3; _ij3[1] = -1;
5593 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5597 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5600 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5603 IkReal x432=
IKcos(j3);
5604 IkReal x433=
IKsin(j3);
5605 IkReal x434=(gconst1*x433);
5606 IkReal x435=(gconst2*x433);
5607 IkReal x436=((1.0)*x432);
5608 IkReal x437=(gconst2*x436);
5609 evalcond[0]=(((new_r01*x432))+gconst1+((new_r11*x433)));
5610 evalcond[1]=(((gconst1*x432))+x435+new_r01);
5611 evalcond[2]=((((-1.0)*x437))+x434);
5612 evalcond[3]=(((new_r01*x433))+gconst2+(((-1.0)*new_r11*x436)));
5613 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5614 evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst1*x436)));
5622 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5623 vinfos[0].jointtype = 1;
5624 vinfos[0].foffset = j0;
5625 vinfos[0].indices[0] = _ij0[0];
5626 vinfos[0].indices[1] = _ij0[1];
5627 vinfos[0].maxsolutions = _nj0;
5628 vinfos[1].jointtype = 1;
5629 vinfos[1].foffset = j1;
5630 vinfos[1].indices[0] = _ij1[0];
5631 vinfos[1].indices[1] = _ij1[1];
5632 vinfos[1].maxsolutions = _nj1;
5633 vinfos[2].jointtype = 1;
5634 vinfos[2].foffset = j2;
5635 vinfos[2].indices[0] = _ij2[0];
5636 vinfos[2].indices[1] = _ij2[1];
5637 vinfos[2].maxsolutions = _nj2;
5638 vinfos[3].jointtype = 1;
5639 vinfos[3].foffset = j3;
5640 vinfos[3].indices[0] = _ij3[0];
5641 vinfos[3].indices[1] = _ij3[1];
5642 vinfos[3].maxsolutions = _nj3;
5643 vinfos[4].jointtype = 1;
5644 vinfos[4].foffset = j4;
5645 vinfos[4].indices[0] = _ij4[0];
5646 vinfos[4].indices[1] = _ij4[1];
5647 vinfos[4].maxsolutions = _nj4;
5648 vinfos[5].jointtype = 1;
5649 vinfos[5].foffset = j5;
5650 vinfos[5].indices[0] = _ij5[0];
5651 vinfos[5].indices[1] = _ij5[1];
5652 vinfos[5].maxsolutions = _nj5;
5653 std::vector<int> vfree(0);
5666 IkReal j3array[1], cj3array[1], sj3array[1];
5667 bool j3valid[1]={
false};
5677 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.
value)))+(x439.
value));
5678 sj3array[0]=
IKsin(j3array[0]);
5679 cj3array[0]=
IKcos(j3array[0]);
5680 if( j3array[0] >
IKPI )
5684 else if( j3array[0] < -
IKPI )
5685 { j3array[0]+=
IK2PI;
5688 for(
int ij3 = 0; ij3 < 1; ++ij3)
5694 _ij3[0] = ij3; _ij3[1] = -1;
5695 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5699 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5702 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5705 IkReal x440=
IKcos(j3);
5706 IkReal x441=
IKsin(j3);
5707 IkReal x442=(gconst1*x441);
5708 IkReal x443=(gconst2*x441);
5709 IkReal x444=((1.0)*x440);
5710 IkReal x445=(gconst2*x444);
5711 evalcond[0]=(((new_r01*x440))+gconst1+((new_r11*x441)));
5712 evalcond[1]=(((gconst1*x440))+x443+new_r01);
5713 evalcond[2]=((((-1.0)*x445))+x442);
5714 evalcond[3]=(((new_r01*x441))+gconst2+(((-1.0)*new_r11*x444)));
5715 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5716 evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst1*x444)));
5724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5725 vinfos[0].jointtype = 1;
5726 vinfos[0].foffset = j0;
5727 vinfos[0].indices[0] = _ij0[0];
5728 vinfos[0].indices[1] = _ij0[1];
5729 vinfos[0].maxsolutions = _nj0;
5730 vinfos[1].jointtype = 1;
5731 vinfos[1].foffset = j1;
5732 vinfos[1].indices[0] = _ij1[0];
5733 vinfos[1].indices[1] = _ij1[1];
5734 vinfos[1].maxsolutions = _nj1;
5735 vinfos[2].jointtype = 1;
5736 vinfos[2].foffset = j2;
5737 vinfos[2].indices[0] = _ij2[0];
5738 vinfos[2].indices[1] = _ij2[1];
5739 vinfos[2].maxsolutions = _nj2;
5740 vinfos[3].jointtype = 1;
5741 vinfos[3].foffset = j3;
5742 vinfos[3].indices[0] = _ij3[0];
5743 vinfos[3].indices[1] = _ij3[1];
5744 vinfos[3].maxsolutions = _nj3;
5745 vinfos[4].jointtype = 1;
5746 vinfos[4].foffset = j4;
5747 vinfos[4].indices[0] = _ij4[0];
5748 vinfos[4].indices[1] = _ij4[1];
5749 vinfos[4].maxsolutions = _nj4;
5750 vinfos[5].jointtype = 1;
5751 vinfos[5].foffset = j5;
5752 vinfos[5].indices[0] = _ij5[0];
5753 vinfos[5].indices[1] = _ij5[1];
5754 vinfos[5].maxsolutions = _nj5;
5755 std::vector<int> vfree(0);
5768 IkReal j3array[1], cj3array[1], sj3array[1];
5769 bool j3valid[1]={
false};
5779 j3array[0]=((-1.5707963267949)+(x446.
value)+(((1.5707963267949)*(x447.
value))));
5780 sj3array[0]=
IKsin(j3array[0]);
5781 cj3array[0]=
IKcos(j3array[0]);
5782 if( j3array[0] >
IKPI )
5786 else if( j3array[0] < -
IKPI )
5787 { j3array[0]+=
IK2PI;
5790 for(
int ij3 = 0; ij3 < 1; ++ij3)
5796 _ij3[0] = ij3; _ij3[1] = -1;
5797 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5801 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5804 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5807 IkReal x448=
IKcos(j3);
5808 IkReal x449=
IKsin(j3);
5809 IkReal x450=(gconst1*x449);
5810 IkReal x451=(gconst2*x449);
5811 IkReal x452=((1.0)*x448);
5812 IkReal x453=(gconst2*x452);
5813 evalcond[0]=(((new_r01*x448))+gconst1+((new_r11*x449)));
5814 evalcond[1]=(((gconst1*x448))+x451+new_r01);
5815 evalcond[2]=((((-1.0)*x453))+x450);
5816 evalcond[3]=(((new_r01*x449))+gconst2+(((-1.0)*new_r11*x452)));
5817 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5818 evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst1*x452)));
5826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5827 vinfos[0].jointtype = 1;
5828 vinfos[0].foffset = j0;
5829 vinfos[0].indices[0] = _ij0[0];
5830 vinfos[0].indices[1] = _ij0[1];
5831 vinfos[0].maxsolutions = _nj0;
5832 vinfos[1].jointtype = 1;
5833 vinfos[1].foffset = j1;
5834 vinfos[1].indices[0] = _ij1[0];
5835 vinfos[1].indices[1] = _ij1[1];
5836 vinfos[1].maxsolutions = _nj1;
5837 vinfos[2].jointtype = 1;
5838 vinfos[2].foffset = j2;
5839 vinfos[2].indices[0] = _ij2[0];
5840 vinfos[2].indices[1] = _ij2[1];
5841 vinfos[2].maxsolutions = _nj2;
5842 vinfos[3].jointtype = 1;
5843 vinfos[3].foffset = j3;
5844 vinfos[3].indices[0] = _ij3[0];
5845 vinfos[3].indices[1] = _ij3[1];
5846 vinfos[3].maxsolutions = _nj3;
5847 vinfos[4].jointtype = 1;
5848 vinfos[4].foffset = j4;
5849 vinfos[4].indices[0] = _ij4[0];
5850 vinfos[4].indices[1] = _ij4[1];
5851 vinfos[4].maxsolutions = _nj4;
5852 vinfos[5].jointtype = 1;
5853 vinfos[5].foffset = j5;
5854 vinfos[5].indices[0] = _ij5[0];
5855 vinfos[5].indices[1] = _ij5[1];
5856 vinfos[5].maxsolutions = _nj5;
5857 std::vector<int> vfree(0);
5869 if( bgotonextstatement )
5871 bool bgotonextstatement =
true;
5874 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
5875 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5877 bgotonextstatement=
false;
5884 IkReal x454=((-1.0)*(x455.
value));
5893 IkReal gconst0=x454;
5894 IkReal x456 = new_r11*new_r11;
5898 IkReal gconst1=(new_r11*(pow(x456,-0.5)));
5901 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5909 IkReal x457=((-1.0)*(x458.
value));
5918 IkReal gconst0=x457;
5919 IkReal x459 = new_r11*new_r11;
5923 IkReal gconst1=(new_r11*(pow(x459,-0.5)));
5926 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5929 IkReal j3array[2], cj3array[2], sj3array[2];
5930 bool j3valid[2]={
false};
5936 sj3array[0]=((-1.0)*new_r00*(x460.
value));
5939 j3valid[0] = j3valid[1] =
true;
5940 j3array[0] =
IKasin(sj3array[0]);
5941 cj3array[0] =
IKcos(j3array[0]);
5942 sj3array[1] = sj3array[0];
5943 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
5944 cj3array[1] = -cj3array[0];
5946 else if( isnan(sj3array[0]) )
5950 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5952 for(
int ij3 = 0; ij3 < 2; ++ij3)
5958 _ij3[0] = ij3; _ij3[1] = -1;
5959 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
5963 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5966 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5969 IkReal x461=
IKcos(j3);
5970 IkReal x462=
IKsin(j3);
5971 evalcond[0]=(gconst1*x461);
5972 evalcond[1]=(new_r00*x461);
5973 evalcond[2]=((-1.0)*new_r11*x461);
5974 evalcond[3]=(((new_r00*x462))+gconst1);
5975 evalcond[4]=(((new_r11*x462))+gconst1);
5976 evalcond[5]=(((gconst1*x462))+new_r11);
5984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5985 vinfos[0].jointtype = 1;
5986 vinfos[0].foffset = j0;
5987 vinfos[0].indices[0] = _ij0[0];
5988 vinfos[0].indices[1] = _ij0[1];
5989 vinfos[0].maxsolutions = _nj0;
5990 vinfos[1].jointtype = 1;
5991 vinfos[1].foffset = j1;
5992 vinfos[1].indices[0] = _ij1[0];
5993 vinfos[1].indices[1] = _ij1[1];
5994 vinfos[1].maxsolutions = _nj1;
5995 vinfos[2].jointtype = 1;
5996 vinfos[2].foffset = j2;
5997 vinfos[2].indices[0] = _ij2[0];
5998 vinfos[2].indices[1] = _ij2[1];
5999 vinfos[2].maxsolutions = _nj2;
6000 vinfos[3].jointtype = 1;
6001 vinfos[3].foffset = j3;
6002 vinfos[3].indices[0] = _ij3[0];
6003 vinfos[3].indices[1] = _ij3[1];
6004 vinfos[3].maxsolutions = _nj3;
6005 vinfos[4].jointtype = 1;
6006 vinfos[4].foffset = j4;
6007 vinfos[4].indices[0] = _ij4[0];
6008 vinfos[4].indices[1] = _ij4[1];
6009 vinfos[4].maxsolutions = _nj4;
6010 vinfos[5].jointtype = 1;
6011 vinfos[5].foffset = j5;
6012 vinfos[5].indices[0] = _ij5[0];
6013 vinfos[5].indices[1] = _ij5[1];
6014 vinfos[5].maxsolutions = _nj5;
6015 std::vector<int> vfree(0);
6024 IkReal j3array[2], cj3array[2], sj3array[2];
6025 bool j3valid[2]={
false};
6031 sj3array[0]=((-1.0)*gconst1*(x463.
value));
6034 j3valid[0] = j3valid[1] =
true;
6035 j3array[0] =
IKasin(sj3array[0]);
6036 cj3array[0] =
IKcos(j3array[0]);
6037 sj3array[1] = sj3array[0];
6038 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
6039 cj3array[1] = -cj3array[0];
6041 else if( isnan(sj3array[0]) )
6045 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6047 for(
int ij3 = 0; ij3 < 2; ++ij3)
6053 _ij3[0] = ij3; _ij3[1] = -1;
6054 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
6058 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6061 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6064 IkReal x464=
IKcos(j3);
6065 IkReal x465=
IKsin(j3);
6066 IkReal x466=(gconst1*x465);
6067 evalcond[0]=(gconst1*x464);
6068 evalcond[1]=(new_r00*x464);
6069 evalcond[2]=((-1.0)*new_r11*x464);
6070 evalcond[3]=(((new_r00*x465))+gconst1);
6071 evalcond[4]=(x466+new_r00);
6072 evalcond[5]=(x466+new_r11);
6080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6081 vinfos[0].jointtype = 1;
6082 vinfos[0].foffset = j0;
6083 vinfos[0].indices[0] = _ij0[0];
6084 vinfos[0].indices[1] = _ij0[1];
6085 vinfos[0].maxsolutions = _nj0;
6086 vinfos[1].jointtype = 1;
6087 vinfos[1].foffset = j1;
6088 vinfos[1].indices[0] = _ij1[0];
6089 vinfos[1].indices[1] = _ij1[1];
6090 vinfos[1].maxsolutions = _nj1;
6091 vinfos[2].jointtype = 1;
6092 vinfos[2].foffset = j2;
6093 vinfos[2].indices[0] = _ij2[0];
6094 vinfos[2].indices[1] = _ij2[1];
6095 vinfos[2].maxsolutions = _nj2;
6096 vinfos[3].jointtype = 1;
6097 vinfos[3].foffset = j3;
6098 vinfos[3].indices[0] = _ij3[0];
6099 vinfos[3].indices[1] = _ij3[1];
6100 vinfos[3].maxsolutions = _nj3;
6101 vinfos[4].jointtype = 1;
6102 vinfos[4].foffset = j4;
6103 vinfos[4].indices[0] = _ij4[0];
6104 vinfos[4].indices[1] = _ij4[1];
6105 vinfos[4].maxsolutions = _nj4;
6106 vinfos[5].jointtype = 1;
6107 vinfos[5].foffset = j5;
6108 vinfos[5].indices[0] = _ij5[0];
6109 vinfos[5].indices[1] = _ij5[1];
6110 vinfos[5].maxsolutions = _nj5;
6111 std::vector<int> vfree(0);
6124 IkReal j3array[2], cj3array[2], sj3array[2];
6125 bool j3valid[2]={
false};
6131 sj3array[0]=((-1.0)*gconst1*(x467.
value));
6134 j3valid[0] = j3valid[1] =
true;
6135 j3array[0] =
IKasin(sj3array[0]);
6136 cj3array[0] =
IKcos(j3array[0]);
6137 sj3array[1] = sj3array[0];
6138 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
6139 cj3array[1] = -cj3array[0];
6141 else if( isnan(sj3array[0]) )
6145 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6147 for(
int ij3 = 0; ij3 < 2; ++ij3)
6153 _ij3[0] = ij3; _ij3[1] = -1;
6154 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
6158 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6161 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6164 IkReal x468=
IKcos(j3);
6165 IkReal x469=
IKsin(j3);
6166 IkReal x470=(gconst1*x469);
6167 evalcond[0]=(gconst1*x468);
6168 evalcond[1]=(new_r00*x468);
6169 evalcond[2]=((-1.0)*new_r11*x468);
6170 evalcond[3]=(((new_r11*x469))+gconst1);
6171 evalcond[4]=(x470+new_r00);
6172 evalcond[5]=(x470+new_r11);
6180 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6181 vinfos[0].jointtype = 1;
6182 vinfos[0].foffset = j0;
6183 vinfos[0].indices[0] = _ij0[0];
6184 vinfos[0].indices[1] = _ij0[1];
6185 vinfos[0].maxsolutions = _nj0;
6186 vinfos[1].jointtype = 1;
6187 vinfos[1].foffset = j1;
6188 vinfos[1].indices[0] = _ij1[0];
6189 vinfos[1].indices[1] = _ij1[1];
6190 vinfos[1].maxsolutions = _nj1;
6191 vinfos[2].jointtype = 1;
6192 vinfos[2].foffset = j2;
6193 vinfos[2].indices[0] = _ij2[0];
6194 vinfos[2].indices[1] = _ij2[1];
6195 vinfos[2].maxsolutions = _nj2;
6196 vinfos[3].jointtype = 1;
6197 vinfos[3].foffset = j3;
6198 vinfos[3].indices[0] = _ij3[0];
6199 vinfos[3].indices[1] = _ij3[1];
6200 vinfos[3].maxsolutions = _nj3;
6201 vinfos[4].jointtype = 1;
6202 vinfos[4].foffset = j4;
6203 vinfos[4].indices[0] = _ij4[0];
6204 vinfos[4].indices[1] = _ij4[1];
6205 vinfos[4].maxsolutions = _nj4;
6206 vinfos[5].jointtype = 1;
6207 vinfos[5].foffset = j5;
6208 vinfos[5].indices[0] = _ij5[0];
6209 vinfos[5].indices[1] = _ij5[1];
6210 vinfos[5].maxsolutions = _nj5;
6211 std::vector<int> vfree(0);
6223 if( bgotonextstatement )
6225 bool bgotonextstatement =
true;
6228 evalcond[0]=
IKabs(new_r11);
6229 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
6231 bgotonextstatement=
false;
6234 IkReal x471=((-1.0)*new_r01);
6239 IkReal x472=((-1.0)*(x473.
value));
6247 IkReal gconst0=x472;
6249 IkReal x474 = new_r01*new_r01;
6253 IkReal gconst2=(x471*(pow(x474,-0.5)));
6254 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
6255 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6259 IkReal x475=((-1.0)*new_r01);
6264 IkReal x476=((-1.0)*(x477.
value));
6272 IkReal gconst0=x476;
6274 IkReal x478 = new_r01*new_r01;
6278 IkReal gconst2=(x475*(pow(x478,-0.5)));
6279 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
6280 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6284 IkReal x479=((-1.0)*new_r01);
6289 IkReal x480=((-1.0)*(x481.
value));
6297 IkReal gconst0=x480;
6299 IkReal x482 = new_r01*new_r01;
6303 IkReal gconst2=(x479*(pow(x482,-0.5)));
6305 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6312 IkReal j3array[1], cj3array[1], sj3array[1];
6313 bool j3valid[1]={
false};
6326 sj3array[0]=
IKsin(j3array[0]);
6327 cj3array[0]=
IKcos(j3array[0]);
6328 if( j3array[0] >
IKPI )
6332 else if( j3array[0] < -
IKPI )
6333 { j3array[0]+=
IK2PI;
6336 for(
int ij3 = 0; ij3 < 1; ++ij3)
6342 _ij3[0] = ij3; _ij3[1] = -1;
6343 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6347 j3valid[iij3]=
false; _ij3[1] = iij3;
break;