1 #define IKFAST_HAS_LIBRARY 25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] 40 #ifndef __PRETTY_FUNCTION__ 41 #define __PRETTY_FUNCTION__ __FUNCDNAME__ 45 #ifndef __PRETTY_FUNCTION__ 46 #define __PRETTY_FUNCTION__ __func__ 49 #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()); } } 54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x 56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) 59 #define IK2PI ((IkReal)6.28318530717959) 60 #define IKPI ((IkReal)3.14159265358979) 61 #define IKPI_2 ((IkReal)1.57079632679490) 77 void dgetrf_ (
const int* m,
const int* n,
double* a,
const int* lda,
int* ipiv,
int* info);
78 void zgetrf_ (
const int* m,
const int* n, std::complex<double>* a,
const int* lda,
int* ipiv,
int* info);
79 void dgetri_(
const int* n,
const double* a,
const int* lda,
int* ipiv,
double* work,
const int* lwork,
int* info);
80 void dgesv_ (
const int* n,
const int* nrhs,
double* a,
const int* lda,
int* ipiv,
double* b,
const int* ldb,
int* info);
81 void dgetrs_(
const char *trans,
const int *n,
const int *nrhs,
double *a,
const int *lda,
int *ipiv,
double *b,
const int *ldb,
int *info);
82 void dgeev_(
const char *jobvl,
const char *jobvr,
const int *n,
double *a,
const int *lda,
double *wr,
double *wi,
double *vl,
const int *ldvl,
double *vr,
const int *ldvr,
double *work,
const int *lwork,
int *info);
87 #ifdef IKFAST_NAMESPACE 88 namespace IKFAST_NAMESPACE {
91 inline float IKabs(
float f) {
return fabsf(f); }
92 inline double IKabs(
double f) {
return fabs(f); }
94 inline float IKsqr(
float f) {
return f*f; }
95 inline double IKsqr(
double f) {
return f*f; }
97 inline float IKlog(
float f) {
return logf(f); }
98 inline double IKlog(
double f) {
return log(f); }
101 #ifndef IKFAST_SINCOS_THRESH 102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7) 106 #ifndef IKFAST_ATAN2_MAGTHRESH 107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7) 111 #ifndef IKFAST_SOLUTION_THRESH 112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) 116 #ifndef IKFAST_EVALCOND_THRESH 117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) 124 if( f <= -1 )
return float(-
IKPI_2);
125 else if( f >= 1 )
return float(
IKPI_2);
131 if( f <= -1 )
return -
IKPI_2;
132 else if( f >= 1 )
return IKPI_2;
157 if( f <= -1 )
return float(
IKPI);
158 else if( f >= 1 )
return float(0);
164 if( f <= -1 )
return IKPI;
165 else if( f >= 1 )
return 0;
168 inline float IKsin(
float f) {
return sinf(f); }
170 inline float IKcos(
float f) {
return cosf(f); }
172 inline float IKtan(
float f) {
return tanf(f); }
174 inline float IKsqrt(
float f) {
if( f <= 0.0f )
return 0.0f;
return sqrtf(f); }
175 inline double IKsqrt(
double f) {
if( f <= 0.0 )
return 0.0;
return sqrt(f); }
177 return atan2f(fy,fx);
184 else if( isnan(fx) ) {
187 return atan2f(fy,fx);
197 else if( isnan(fx) ) {
203 template <
typename T>
210 template <
typename T>
216 if( !isnan(fy) && !isnan(fx) ) {
245 template <
typename T>
264 ret.
value = (T)1.0e30;
268 ret.
value = T(1.0)/f;
273 int num = n > 0 ? n : -n;
277 else if( num == 3 ) {
299 IKFAST_API
void ComputeFk(
const IkReal* j, IkReal* eetrans, IkReal* eerot) {
300 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;
334 x33=((((-1.0)*x30))+((x2*x24)));
335 x34=((((-1.0)*x31))+((x2*x26)));
336 x35=((((-1.0)*x17*x24))+x30);
337 x36=((((-1.0)*x16*x2*x3))+x31);
341 x40=(x37+(((-1.0)*x15*x4)));
343 x42=(((x11*(((((-1.0)*x25))+x29))))+((x1*x10*x32)));
344 x43=(((x10*x40))+((x11*(((((-1.0)*x0*x17*x6))+(((-1.0)*x18*x22)))))));
345 IkReal x45=((1.0)*x16);
346 x44=(((x10*x41))+((x11*(((((-1.0)*x22*x45))+(((-1.0)*x23*x45)))))));
347 eerot[0]=(((x8*(((((-1.0)*x28))+((x35*x5))))))+((x43*x9)));
348 eerot[1]=(((x43*x8))+((x9*((x28+(((-1.0)*x15*x35)))))));
349 eerot[2]=(((x11*x40))+((x10*((((x0*x22))+((x0*x23)))))));
350 eetrans[0]=((((-0.035)*x0*x25))+(((0.44)*x24))+((x11*((((x21*x33))+(((-1.0)*x20*x4))))))+((x19*x22))+((x19*x23))+(((0.05)*x0))+((x14*x24))+((x10*(((((0.08)*x0*x22))+((x0*x12*x6)))))));
351 eerot[3]=(((x44*x9))+((x8*((((x0*x1))+x39)))));
352 eerot[4]=(((x9*(((((-1.0)*x1*x18))+(((-1.0)*x15*x36))))))+((x44*x8)));
353 eerot[5]=(((x10*((((x22*x4))+((x23*x4))))))+((x11*x41)));
354 eetrans[1]=(((x10*(((((0.08)*x22*x4))+((x12*x4*x6))))))+(((0.44)*x26))+(((0.05)*x4))+((x14*x26))+(((-0.035)*x25*x4))+((x11*((((x0*x20))+((x21*x34))))))+((x13*x23))+((x13*x22)));
355 eerot[6]=(((x5*x8*(((((-1.0)*x22))+(((-1.0)*x17*x6))))))+((x42*x9)));
356 eerot[7]=(((x42*x8))+((x32*x5*x9)));
357 eerot[8]=(((x27*x32))+((x10*(((((-1.0)*x29))+x25)))));
358 eetrans[2]=((0.33)+((x14*x6))+((x27*(((((0.08)*x22))+((x12*x6))))))+((x10*(((((-1.0)*x12*x3))+(((0.08)*x25))))))+(((0.42)*x25))+(((0.44)*x6))+(((0.035)*x22))+(((-0.42)*x2*x3)));
371 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;
372 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2],
_nj5;
377 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;
378 for(
int dummyiter = 0; dummyiter < 1; ++dummyiter) {
389 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
392 new_r01=((-1.0)*r01);
393 new_r02=((-1.0)*r02);
394 new_px=((((-0.08)*r02))+px);
396 new_r11=((-1.0)*r11);
397 new_r12=((-1.0)*r12);
398 new_py=((((-0.08)*r12))+py);
400 new_r21=((-1.0)*r21);
401 new_r22=((-1.0)*r22);
402 new_pz=((-0.33)+(((-0.08)*r22))+pz);
403 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;
404 IkReal x46=((1.0)*px);
405 IkReal x47=((1.0)*pz);
406 IkReal x48=((1.0)*py);
407 pp=((px*px)+(py*py)+(pz*pz));
408 npx=(((px*r00))+((py*r10))+((pz*r20)));
409 npy=(((px*r01))+((py*r11))+((pz*r21)));
410 npz=(((px*r02))+((py*r12))+((pz*r22)));
411 rxp0_0=((((-1.0)*r20*x48))+((pz*r10)));
412 rxp0_1=(((px*r20))+(((-1.0)*r00*x47)));
413 rxp0_2=((((-1.0)*r10*x46))+((py*r00)));
414 rxp1_0=((((-1.0)*r21*x48))+((pz*r11)));
415 rxp1_1=(((px*r21))+(((-1.0)*r01*x47)));
416 rxp1_2=((((-1.0)*r11*x46))+((py*r01)));
417 rxp2_0=((((-1.0)*r22*x48))+((pz*r12)));
418 rxp2_1=((((-1.0)*r02*x47))+((px*r22)));
419 rxp2_2=((((-1.0)*r12*x46))+((py*r02)));
423 if(
IKabs(j0eval[0]) < 0.0000010000000000 )
430 IkReal j0array[2], cj0array[2], sj0array[2];
431 bool j0valid[2]={
false};
437 IkReal x49=x50.
value;
438 j0array[0]=((-1.0)*x49);
439 sj0array[0]=
IKsin(j0array[0]);
440 cj0array[0]=
IKcos(j0array[0]);
441 j0array[1]=((3.14159265358979)+(((-1.0)*x49)));
442 sj0array[1]=
IKsin(j0array[1]);
443 cj0array[1]=
IKcos(j0array[1]);
444 if( j0array[0] >
IKPI )
448 else if( j0array[0] < -
IKPI )
452 if( j0array[1] >
IKPI )
456 else if( j0array[1] < -
IKPI )
460 for(
int ij0 = 0; ij0 < 2; ++ij0)
466 _ij0[0] = ij0; _ij0[1] = -1;
467 for(
int iij0 = ij0+1; iij0 < 2; ++iij0)
471 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
474 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
477 IkReal j2array[2], cj2array[2], sj2array[2];
478 bool j2valid[2]={
false};
480 if( (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) < -1-
IKFAST_SINCOS_THRESH || (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) > 1+
IKFAST_SINCOS_THRESH )
482 IkReal x51=
IKasin(((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px))));
483 j2array[0]=((-0.0831412318884412)+(((-1.0)*x51)));
484 sj2array[0]=
IKsin(j2array[0]);
485 cj2array[0]=
IKcos(j2array[0]);
486 j2array[1]=((3.05845142170135)+x51);
487 sj2array[1]=
IKsin(j2array[1]);
488 cj2array[1]=
IKcos(j2array[1]);
489 if( j2array[0] >
IKPI )
493 else if( j2array[0] < -
IKPI )
497 if( j2array[1] >
IKPI )
501 else if( j2array[1] < -
IKPI )
505 for(
int ij2 = 0; ij2 < 2; ++ij2)
511 _ij2[0] = ij2; _ij2[1] = -1;
512 for(
int iij2 = ij2+1; iij2 < 2; ++iij2)
516 j2valid[iij2]=
false; _ij2[1] = iij2;
break;
519 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
525 IkReal x54=((240.0)*cj2);
526 IkReal x55=((20.0)*sj2);
527 IkReal x56=((0.42)*cj2);
529 IkReal x58=(cj2*sj2);
530 IkReal x59=((0.035)*sj2);
533 j1eval[0]=((((-1.0)*x53*x54))+(((12.0)*cj2))+(((-1.0)*sj2))+(((20.0)*x61))+(((251.428571428571)*pz))+(((-1.0)*x54*x57))+((x53*x55))+(((240.0)*x60))+((x55*x57)));
534 j1eval[1]=((
IKabs(((-0.001225)+(((-0.175175)*x52))+(pz*pz)+(((0.0294)*x58)))))+(
IKabs(((0.0147)+(((-0.0294)*x52))+(((-0.05)*pz))+(((-0.175175)*x58))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x57))+((pz*x53))))));
535 j1eval[2]=
IKsign(((((-1.0)*x53*x56))+(((0.021)*cj2))+(((-1.0)*x56*x57))+(((0.42)*x60))+(((0.035)*x61))+((x57*x59))+(((-0.00175)*sj2))+((x53*x59))+(((0.44)*pz))));
536 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
543 IkReal x65=((240.0)*sj2);
544 IkReal x66=((0.035)*cj2);
545 IkReal x67=((20.0)*cj2);
547 IkReal x69=((0.42)*sj2);
548 j1eval[0]=((12.5714285714286)+cj2+(((12.0)*sj2))+(((20.0)*x63))+(((-251.428571428571)*x62))+(((-251.428571428571)*x64))+(((-240.0)*x68))+(((-1.0)*x64*x67))+(((-1.0)*x64*x65))+(((-1.0)*x62*x67))+(((-1.0)*x62*x65)));
549 j1eval[1]=
IKsign(((0.022)+(((0.035)*x63))+(((-0.44)*x64))+(((-0.44)*x62))+(((-0.42)*x68))+(((-1.0)*x64*x69))+(((-1.0)*x64*x66))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-1.0)*x62*x69))+(((-1.0)*x62*x66))));
550 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
560 IkReal x76=(x70*x71);
561 IkReal x77=(x71*x72);
562 j1eval[0]=((-1.0)+(((40.0)*x75))+(((40.0)*x74))+(((-800.0)*x74*x75))+(((400.0)*x76))+(((-400.0)*x70))+(((-400.0)*x77))+(((-400.0)*x73)));
563 j1eval[1]=
IKsign(((-0.0025)+(((-2.0)*x74*x75))+(((0.1)*x75))+(((0.1)*x74))+x76+(((-1.0)*x70))+(((-1.0)*x73))+(((-1.0)*x77))));
564 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
571 IkReal j1array[1], cj1array[1], sj1array[1];
572 bool j1valid[1]={
false};
577 IkReal x81=((0.035)*sj2);
579 IkReal x83=((0.035)*cj2);
580 IkReal x84=((0.42)*cj2);
581 IkReal x85=((0.42)*sj2);
582 IkReal x86=((0.42)*x82);
583 CheckValue<IkReal> x87 =
IKatan2WithCheck(IkReal(((0.022)+(((-1.0)*x82*x83))+(((-1.0)*x82*x85))+(((-1.0)*x80*x85))+(((-1.0)*x80*x83))+((pz*x84))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-0.44)*x80))+(((-0.44)*x82))+(((-1.0)*pz*x81)))),IkReal(((((0.021)*cj2))+(((-1.0)*x82*x84))+(((-1.0)*x80*x84))+((x81*x82))+(((-0.00175)*sj2))+(((-0.44)*pz))+((x80*x81))+(((-1.0)*pz*x83))+(((-1.0)*pz*x85)))),
IKFAST_ATAN2_MAGTHRESH);
591 j1array[0]=((-1.5707963267949)+(x87.
value)+(((1.5707963267949)*(x88.
value))));
592 sj1array[0]=
IKsin(j1array[0]);
593 cj1array[0]=
IKcos(j1array[0]);
594 if( j1array[0] >
IKPI )
598 else if( j1array[0] < -
IKPI )
602 for(
int ij1 = 0; ij1 < 1; ++ij1)
608 _ij1[0] = ij1; _ij1[1] = -1;
609 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
613 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
616 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
619 IkReal x89=
IKcos(j1);
620 IkReal x90=
IKsin(j1);
621 IkReal x91=((0.035)*sj2);
623 IkReal x93=((0.42)*sj2);
624 IkReal x94=((0.035)*cj2);
626 IkReal x96=((0.42)*x90);
627 IkReal x97=((1.0)*x95);
628 IkReal x98=((1.0)*x89);
629 IkReal x99=(cj2*x89);
630 IkReal x100=((0.88)*x90);
631 evalcond[0]=((((0.05)*x89))+((pz*x90))+(((-1.0)*x92*x98))+(((0.42)*cj2))+(((-1.0)*x91))+(((-1.0)*x89*x97)));
632 evalcond[1]=((((-1.0)*x90*x91))+((cj2*x96))+pz+(((-0.44)*x89))+(((-1.0)*x89*x94))+(((-1.0)*x89*x93)));
633 evalcond[2]=((0.44)+(((-1.0)*pz*x98))+(((0.05)*x90))+(((-1.0)*x90*x97))+(((-1.0)*x90*x92))+x93+x94);
634 evalcond[3]=((-0.018475)+((x100*x95))+((x100*x92))+(((0.1)*x92))+(((0.1)*x95))+(((-0.044)*x90))+(((-1.0)*pp))+(((0.88)*pz*x89)));
635 evalcond[4]=((0.05)+(((0.42)*x99))+((x90*x93))+((x90*x94))+(((-1.0)*x92))+(((-1.0)*x97))+(((0.44)*x90))+(((-1.0)*x89*x91)));
642 rotationfunction0(solutions);
653 IkReal j1array[1], cj1array[1], sj1array[1];
654 bool j1valid[1]={
false};
656 IkReal x1520=cj2*cj2;
657 IkReal x1521=(cj2*sj2);
658 IkReal x1522=(cj0*px);
659 IkReal x1523=((0.42)*sj2);
660 IkReal x1524=((0.035)*cj2);
661 IkReal x1525=((1.0)*pz);
662 IkReal x1526=(py*sj0);
663 CheckValue<IkReal> x1527 =
IKatan2WithCheck(IkReal(((-0.37)+(((-0.0294)*x1521))+(((-0.3696)*sj2))+(((-0.0308)*cj2))+(pz*pz)+(((0.175175)*x1520)))),IkReal(((0.0147)+(((-0.0294)*x1520))+(((-0.175175)*x1521))+(((-1.0)*x1525*x1526))+(((0.0154)*sj2))+(((-1.0)*x1522*x1525))+(((-0.1848)*cj2))+(((0.05)*pz)))),
IKFAST_ATAN2_MAGTHRESH);
667 CheckValue<IkReal> x1528=
IKPowWithIntegerCheck(
IKsign(((0.022)+(((-1.0)*x1524*x1526))+(((-0.44)*x1522))+(((-0.44)*x1526))+(((-0.42)*cj2*pz))+(((-1.0)*x1522*x1524))+(((-1.0)*x1522*x1523))+(((0.00175)*cj2))+(((-1.0)*x1523*x1526))+(((0.035)*pz*sj2))+(((0.021)*sj2)))),-1);
671 j1array[0]=((-1.5707963267949)+(x1527.
value)+(((1.5707963267949)*(x1528.
value))));
672 sj1array[0]=
IKsin(j1array[0]);
673 cj1array[0]=
IKcos(j1array[0]);
674 if( j1array[0] >
IKPI )
678 else if( j1array[0] < -
IKPI )
682 for(
int ij1 = 0; ij1 < 1; ++ij1)
688 _ij1[0] = ij1; _ij1[1] = -1;
689 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
693 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
696 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
699 IkReal x1529=
IKcos(j1);
700 IkReal x1530=
IKsin(j1);
701 IkReal x1531=((0.035)*sj2);
702 IkReal x1532=(py*sj0);
703 IkReal x1533=((0.42)*sj2);
704 IkReal x1534=((0.035)*cj2);
705 IkReal x1535=(cj0*px);
706 IkReal x1536=((0.42)*x1530);
707 IkReal x1537=((1.0)*x1535);
708 IkReal x1538=((1.0)*x1529);
709 IkReal x1539=(cj2*x1529);
710 IkReal x1540=((0.88)*x1530);
711 evalcond[0]=((((-1.0)*x1529*x1537))+(((0.05)*x1529))+(((-1.0)*x1531))+(((-1.0)*x1532*x1538))+(((0.42)*cj2))+((pz*x1530)));
712 evalcond[1]=((((-1.0)*x1529*x1533))+(((-1.0)*x1529*x1534))+(((-1.0)*x1530*x1531))+((cj2*x1536))+(((-0.44)*x1529))+pz);
713 evalcond[2]=((0.44)+(((-1.0)*x1530*x1532))+(((0.05)*x1530))+(((-1.0)*x1530*x1537))+x1534+x1533+(((-1.0)*pz*x1538)));
714 evalcond[3]=((-0.018475)+((x1532*x1540))+(((-1.0)*pp))+(((0.1)*x1535))+(((0.1)*x1532))+(((0.88)*pz*x1529))+(((-0.044)*x1530))+((x1535*x1540)));
715 evalcond[4]=((0.05)+(((-1.0)*x1529*x1531))+(((0.44)*x1530))+(((-1.0)*x1532))+(((-1.0)*x1537))+((x1530*x1534))+((x1530*x1533))+(((0.42)*x1539)));
722 rotationfunction0(solutions);
733 IkReal j1array[1], cj1array[1], sj1array[1];
734 bool j1valid[1]={
false};
736 IkReal x1541=cj2*cj2;
737 IkReal x1542=(py*sj0);
738 IkReal x1543=((0.035)*sj2);
739 IkReal x1544=((0.42)*cj2);
740 IkReal x1545=(cj0*px);
741 IkReal x1546=(cj2*sj2);
742 CheckValue<IkReal> x1547=
IKPowWithIntegerCheck(
IKsign(((((0.021)*cj2))+(((0.42)*pz*sj2))+(((-0.00175)*sj2))+(((0.035)*cj2*pz))+((x1543*x1545))+(((-1.0)*x1542*x1544))+(((-1.0)*x1544*x1545))+((x1542*x1543))+(((0.44)*pz)))),-1);
746 CheckValue<IkReal> x1548 =
IKatan2WithCheck(IkReal(((0.0147)+(((-0.0294)*x1541))+(((-0.05)*pz))+(((-0.175175)*x1546))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x1542))+((pz*x1545)))),IkReal(((-0.001225)+(((-0.175175)*x1541))+(pz*pz)+(((0.0294)*x1546)))),
IKFAST_ATAN2_MAGTHRESH);
750 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1547.
value)))+(x1548.
value));
751 sj1array[0]=
IKsin(j1array[0]);
752 cj1array[0]=
IKcos(j1array[0]);
753 if( j1array[0] >
IKPI )
757 else if( j1array[0] < -
IKPI )
761 for(
int ij1 = 0; ij1 < 1; ++ij1)
767 _ij1[0] = ij1; _ij1[1] = -1;
768 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
772 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
775 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
778 IkReal x1549=
IKcos(j1);
779 IkReal x1550=
IKsin(j1);
780 IkReal x1551=((0.035)*sj2);
781 IkReal x1552=(py*sj0);
782 IkReal x1553=((0.42)*sj2);
783 IkReal x1554=((0.035)*cj2);
784 IkReal x1555=(cj0*px);
785 IkReal x1556=((0.42)*x1550);
786 IkReal x1557=((1.0)*x1555);
787 IkReal x1558=((1.0)*x1549);
788 IkReal x1559=(cj2*x1549);
789 IkReal x1560=((0.88)*x1550);
790 evalcond[0]=((((-1.0)*x1549*x1557))+(((-1.0)*x1552*x1558))+(((0.05)*x1549))+((pz*x1550))+(((0.42)*cj2))+(((-1.0)*x1551)));
791 evalcond[1]=((((-1.0)*x1549*x1554))+(((-1.0)*x1549*x1553))+((cj2*x1556))+(((-0.44)*x1549))+pz+(((-1.0)*x1550*x1551)));
792 evalcond[2]=((0.44)+(((-1.0)*x1550*x1552))+(((-1.0)*pz*x1558))+(((0.05)*x1550))+x1553+x1554+(((-1.0)*x1550*x1557)));
793 evalcond[3]=((-0.018475)+((x1555*x1560))+(((0.1)*x1552))+(((0.1)*x1555))+((x1552*x1560))+(((0.88)*pz*x1549))+(((-1.0)*pp))+(((-0.044)*x1550)));
794 evalcond[4]=((0.05)+(((-1.0)*x1552))+(((-1.0)*x1549*x1551))+((x1550*x1554))+((x1550*x1553))+(((0.42)*x1559))+(((-1.0)*x1557))+(((0.44)*x1550)));
801 rotationfunction0(solutions);
820 for(
int rotationiter = 0; rotationiter < 1; ++rotationiter) {
821 IkReal x101=(r11*sj0);
822 IkReal x102=(cj0*r02);
823 IkReal x103=(sj1*sj2);
824 IkReal x104=(r10*sj0);
825 IkReal x105=((1.0)*cj1);
826 IkReal x106=((1.0)*sj0);
827 IkReal x107=(cj0*r00);
828 IkReal x108=(r12*sj0);
829 IkReal x109=(cj0*r01);
830 IkReal x110=((((-1.0)*sj2*x105))+((cj2*sj1)));
831 IkReal x111=(x103+((cj1*cj2)));
832 IkReal x112=(sj0*x110);
833 IkReal x113=(cj0*x110);
834 IkReal x114=((((-1.0)*cj2*x105))+(((-1.0)*x103)));
835 new_r00=(((r20*x111))+((x104*x110))+((x107*x110)));
836 new_r01=(((x109*x110))+((r21*x111))+((x101*x110)));
837 new_r02=(((r22*x111))+((x102*x110))+((x108*x110)));
838 new_r10=((((-1.0)*r00*x106))+((cj0*r10)));
839 new_r11=((((-1.0)*r01*x106))+((cj0*r11)));
840 new_r12=((((-1.0)*r02*x106))+((cj0*r12)));
841 new_r20=(((r20*x110))+((x104*x114))+((x107*x114)));
842 new_r21=(((x109*x114))+((r21*x110))+((x101*x114)));
843 new_r22=(((r22*x110))+((x102*x114))+((x108*x114)));
845 IkReal j4array[2], cj4array[2], sj4array[2];
846 bool j4valid[2]={
false};
851 j4valid[0] = j4valid[1] =
true;
852 j4array[0] =
IKacos(cj4array[0]);
853 sj4array[0] =
IKsin(j4array[0]);
854 cj4array[1] = cj4array[0];
855 j4array[1] = -j4array[0];
856 sj4array[1] = -sj4array[0];
858 else if( isnan(cj4array[0]) )
862 cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
864 for(
int ij4 = 0; ij4 < 2; ++ij4)
870 _ij4[0] = ij4; _ij4[1] = -1;
871 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
875 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
878 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
884 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
885 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
891 j3eval[2]=((
IKabs(new_r12))+(
IKabs(new_r02)));
892 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
898 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
902 bool bgotonextstatement =
true;
905 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
910 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 )
912 bgotonextstatement=
false;
918 j3=
IKatan2(((-1.0)*new_r01), new_r00);
920 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
921 vinfos[0].jointtype = 1;
922 vinfos[0].foffset = j0;
923 vinfos[0].indices[0] = _ij0[0];
924 vinfos[0].indices[1] = _ij0[1];
925 vinfos[0].maxsolutions = _nj0;
926 vinfos[1].jointtype = 1;
927 vinfos[1].foffset = j1;
928 vinfos[1].indices[0] = _ij1[0];
929 vinfos[1].indices[1] = _ij1[1];
930 vinfos[1].maxsolutions = _nj1;
931 vinfos[2].jointtype = 1;
932 vinfos[2].foffset = j2;
933 vinfos[2].indices[0] = _ij2[0];
934 vinfos[2].indices[1] = _ij2[1];
935 vinfos[2].maxsolutions = _nj2;
936 vinfos[3].jointtype = 1;
937 vinfos[3].foffset = j3;
938 vinfos[3].fmul = j3mul;
939 vinfos[3].freeind = 0;
940 vinfos[3].maxsolutions = 0;
941 vinfos[4].jointtype = 1;
942 vinfos[4].foffset = j4;
943 vinfos[4].indices[0] = _ij4[0];
944 vinfos[4].indices[1] = _ij4[1];
945 vinfos[4].maxsolutions = _nj4;
946 vinfos[5].jointtype = 1;
947 vinfos[5].foffset = j5;
948 vinfos[5].fmul = j5mul;
949 vinfos[5].freeind = 0;
950 vinfos[5].maxsolutions = 0;
951 std::vector<int> vfree(1);
958 if( bgotonextstatement )
960 bool bgotonextstatement =
true;
963 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
968 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 )
970 bgotonextstatement=
false;
976 j3=
IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
978 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
979 vinfos[0].jointtype = 1;
980 vinfos[0].foffset = j0;
981 vinfos[0].indices[0] = _ij0[0];
982 vinfos[0].indices[1] = _ij0[1];
983 vinfos[0].maxsolutions = _nj0;
984 vinfos[1].jointtype = 1;
985 vinfos[1].foffset = j1;
986 vinfos[1].indices[0] = _ij1[0];
987 vinfos[1].indices[1] = _ij1[1];
988 vinfos[1].maxsolutions = _nj1;
989 vinfos[2].jointtype = 1;
990 vinfos[2].foffset = j2;
991 vinfos[2].indices[0] = _ij2[0];
992 vinfos[2].indices[1] = _ij2[1];
993 vinfos[2].maxsolutions = _nj2;
994 vinfos[3].jointtype = 1;
995 vinfos[3].foffset = j3;
996 vinfos[3].fmul = j3mul;
997 vinfos[3].freeind = 0;
998 vinfos[3].maxsolutions = 0;
999 vinfos[4].jointtype = 1;
1000 vinfos[4].foffset = j4;
1001 vinfos[4].indices[0] = _ij4[0];
1002 vinfos[4].indices[1] = _ij4[1];
1003 vinfos[4].maxsolutions = _nj4;
1004 vinfos[5].jointtype = 1;
1005 vinfos[5].foffset = j5;
1006 vinfos[5].fmul = j5mul;
1007 vinfos[5].freeind = 0;
1008 vinfos[5].maxsolutions = 0;
1009 std::vector<int> vfree(1);
1016 if( bgotonextstatement )
1018 bool bgotonextstatement =
true;
1021 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
1022 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1024 bgotonextstatement=
false;
1031 IkReal x115=new_r22*new_r22;
1032 IkReal x116=((16.0)*new_r10);
1033 IkReal x117=((16.0)*new_r01);
1034 IkReal x118=((16.0)*new_r22);
1035 IkReal x119=((8.0)*new_r11);
1036 IkReal x120=((8.0)*new_r00);
1037 IkReal x121=(x115*x116);
1038 IkReal x122=(x115*x117);
1039 j3eval[0]=((
IKabs((((new_r11*x118))+(((16.0)*new_r00))+(((-32.0)*new_r00*x115)))))+(
IKabs((((new_r22*x120))+(((-1.0)*x115*x119)))))+(
IKabs(((((-32.0)*new_r11))+((new_r00*x118))+(((16.0)*new_r11*x115)))))+(
IKabs(((((-1.0)*x121))+x116)))+(
IKabs(((((-1.0)*x122))+x117)))+(
IKabs(((((-1.0)*x116))+x121)))+(
IKabs(((((-1.0)*x117))+x122)))+(
IKabs((((new_r22*x119))+(((-1.0)*x120))))));
1040 if(
IKabs(j3eval[0]) < 0.0000000010000000 )
1046 IkReal op[4+1], zeror[4];
1048 IkReal j3evalpoly[1];
1049 IkReal x123=new_r22*new_r22;
1050 IkReal x124=((16.0)*new_r10);
1051 IkReal x125=(new_r11*new_r22);
1052 IkReal x126=(x123*x124);
1053 IkReal x127=((((-8.0)*new_r00))+(((8.0)*x125)));
1055 op[1]=((((-1.0)*x126))+x124);
1056 op[2]=((((16.0)*x125))+(((16.0)*new_r00))+(((-32.0)*new_r00*x123)));
1057 op[3]=((((-1.0)*x124))+x126);
1059 polyroots4(op,zeror,numroots);
1060 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1061 int numsolutions = 0;
1062 for(
int ij3 = 0; ij3 < numroots; ++ij3)
1064 IkReal htj3 = zeror[ij3];
1065 tempj3array[0]=((2.0)*(
atan(htj3)));
1066 for(
int kj3 = 0; kj3 < 1; ++kj3)
1068 j3array[numsolutions] = tempj3array[kj3];
1069 if( j3array[numsolutions] >
IKPI )
1071 j3array[numsolutions]-=
IK2PI;
1073 else if( j3array[numsolutions] < -
IKPI )
1075 j3array[numsolutions]+=
IK2PI;
1077 sj3array[numsolutions] =
IKsin(j3array[numsolutions]);
1078 cj3array[numsolutions] =
IKcos(j3array[numsolutions]);
1082 bool j3valid[4]={
true,
true,
true,
true};
1084 for(
int ij3 = 0; ij3 < numsolutions; ++ij3)
1090 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1093 IkReal x128=((16.0)*new_r01);
1094 IkReal x129=new_r22*new_r22;
1095 IkReal x130=(new_r00*new_r22);
1096 IkReal x131=((8.0)*x130);
1097 IkReal x132=(new_r11*x129);
1098 IkReal x133=(x128*x129);
1099 IkReal x134=((8.0)*x132);
1100 j3evalpoly[0]=(((htj3*(((((-1.0)*x128))+x133))))+(((htj3*htj3)*(((((16.0)*x132))+(((16.0)*x130))+(((-32.0)*new_r11))))))+x131+(((htj3*htj3*htj3)*((x128+(((-1.0)*x133))))))+(((-1.0)*x134))+(((htj3*htj3*htj3*htj3)*((x131+(((-1.0)*x134)))))));
1101 if(
IKabs(j3evalpoly[0]) > 0.0000000010000000 )
1105 _ij3[0] = ij3; _ij3[1] = -1;
1106 for(
int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1110 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
1119 IkReal x135=cj3*cj3;
1120 IkReal x136=(cj3*new_r22);
1121 IkReal x137=((-1.0)+(((-1.0)*x135*(new_r22*new_r22)))+x135);
1123 j5eval[1]=((
IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x136)))))+(
IKabs((((new_r01*x136))+((new_r00*sj3))))));
1125 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
1134 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1143 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1147 bool bgotonextstatement =
true;
1150 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
1151 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1153 bgotonextstatement=
false;
1155 IkReal j5array[1], cj5array[1], sj5array[1];
1156 bool j5valid[1]={
false};
1160 j5array[0]=
IKatan2(new_r10, new_r11);
1161 sj5array[0]=
IKsin(j5array[0]);
1162 cj5array[0]=
IKcos(j5array[0]);
1163 if( j5array[0] >
IKPI )
1167 else if( j5array[0] < -
IKPI )
1168 { j5array[0]+=
IK2PI;
1171 for(
int ij5 = 0; ij5 < 1; ++ij5)
1177 _ij5[0] = ij5; _ij5[1] = -1;
1178 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1182 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1185 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1188 IkReal x138=
IKsin(j5);
1189 IkReal x139=
IKcos(j5);
1191 evalcond[1]=((-1.0)*x139);
1192 evalcond[2]=(x138+(((-1.0)*new_r10)));
1193 evalcond[3]=(x139+(((-1.0)*new_r11)));
1201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1202 vinfos[0].jointtype = 1;
1203 vinfos[0].foffset = j0;
1204 vinfos[0].indices[0] = _ij0[0];
1205 vinfos[0].indices[1] = _ij0[1];
1206 vinfos[0].maxsolutions = _nj0;
1207 vinfos[1].jointtype = 1;
1208 vinfos[1].foffset = j1;
1209 vinfos[1].indices[0] = _ij1[0];
1210 vinfos[1].indices[1] = _ij1[1];
1211 vinfos[1].maxsolutions = _nj1;
1212 vinfos[2].jointtype = 1;
1213 vinfos[2].foffset = j2;
1214 vinfos[2].indices[0] = _ij2[0];
1215 vinfos[2].indices[1] = _ij2[1];
1216 vinfos[2].maxsolutions = _nj2;
1217 vinfos[3].jointtype = 1;
1218 vinfos[3].foffset = j3;
1219 vinfos[3].indices[0] = _ij3[0];
1220 vinfos[3].indices[1] = _ij3[1];
1221 vinfos[3].maxsolutions = _nj3;
1222 vinfos[4].jointtype = 1;
1223 vinfos[4].foffset = j4;
1224 vinfos[4].indices[0] = _ij4[0];
1225 vinfos[4].indices[1] = _ij4[1];
1226 vinfos[4].maxsolutions = _nj4;
1227 vinfos[5].jointtype = 1;
1228 vinfos[5].foffset = j5;
1229 vinfos[5].indices[0] = _ij5[0];
1230 vinfos[5].indices[1] = _ij5[1];
1231 vinfos[5].maxsolutions = _nj5;
1232 std::vector<int> vfree(0);
1240 if( bgotonextstatement )
1242 bool bgotonextstatement =
true;
1245 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1246 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1248 bgotonextstatement=
false;
1250 IkReal j5array[1], cj5array[1], sj5array[1];
1251 bool j5valid[1]={
false};
1255 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1256 sj5array[0]=
IKsin(j5array[0]);
1257 cj5array[0]=
IKcos(j5array[0]);
1258 if( j5array[0] >
IKPI )
1262 else if( j5array[0] < -
IKPI )
1263 { j5array[0]+=
IK2PI;
1266 for(
int ij5 = 0; ij5 < 1; ++ij5)
1272 _ij5[0] = ij5; _ij5[1] = -1;
1273 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1277 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1280 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1283 IkReal x140=
IKsin(j5);
1284 IkReal x141=
IKcos(j5);
1286 evalcond[1]=(x140+new_r10);
1287 evalcond[2]=(x141+new_r11);
1288 evalcond[3]=((-1.0)*x141);
1296 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1297 vinfos[0].jointtype = 1;
1298 vinfos[0].foffset = j0;
1299 vinfos[0].indices[0] = _ij0[0];
1300 vinfos[0].indices[1] = _ij0[1];
1301 vinfos[0].maxsolutions = _nj0;
1302 vinfos[1].jointtype = 1;
1303 vinfos[1].foffset = j1;
1304 vinfos[1].indices[0] = _ij1[0];
1305 vinfos[1].indices[1] = _ij1[1];
1306 vinfos[1].maxsolutions = _nj1;
1307 vinfos[2].jointtype = 1;
1308 vinfos[2].foffset = j2;
1309 vinfos[2].indices[0] = _ij2[0];
1310 vinfos[2].indices[1] = _ij2[1];
1311 vinfos[2].maxsolutions = _nj2;
1312 vinfos[3].jointtype = 1;
1313 vinfos[3].foffset = j3;
1314 vinfos[3].indices[0] = _ij3[0];
1315 vinfos[3].indices[1] = _ij3[1];
1316 vinfos[3].maxsolutions = _nj3;
1317 vinfos[4].jointtype = 1;
1318 vinfos[4].foffset = j4;
1319 vinfos[4].indices[0] = _ij4[0];
1320 vinfos[4].indices[1] = _ij4[1];
1321 vinfos[4].maxsolutions = _nj4;
1322 vinfos[5].jointtype = 1;
1323 vinfos[5].foffset = j5;
1324 vinfos[5].indices[0] = _ij5[0];
1325 vinfos[5].indices[1] = _ij5[1];
1326 vinfos[5].maxsolutions = _nj5;
1327 std::vector<int> vfree(0);
1335 if( bgotonextstatement )
1337 bool bgotonextstatement =
true;
1344 if((x142.
value) < -0.00001)
1347 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj3+(((-1.0)*gconst24)))))+(
IKabs(((-1.0)+(
IKsign(sj3)))))), 6.28318530717959)));
1348 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1350 bgotonextstatement=
false;
1357 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1359 sj3=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
1368 if((x143.
value) < -0.00001)
1371 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1372 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1375 IkReal j5array[1], cj5array[1], sj5array[1];
1376 bool j5valid[1]={
false};
1378 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1386 j5array[0]=
IKatan2(((((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x144.
value)));
1387 sj5array[0]=
IKsin(j5array[0]);
1388 cj5array[0]=
IKcos(j5array[0]);
1389 if( j5array[0] >
IKPI )
1393 else if( j5array[0] < -
IKPI )
1394 { j5array[0]+=
IK2PI;
1397 for(
int ij5 = 0; ij5 < 1; ++ij5)
1403 _ij5[0] = ij5; _ij5[1] = -1;
1404 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1408 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1411 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1414 IkReal x145=
IKcos(j5);
1415 IkReal x146=
IKsin(j5);
1416 IkReal x147=((1.0)*gconst24);
1417 if((((1.0)+(((-1.0)*gconst24*x147)))) < -0.00001)
1419 IkReal x148=
IKsqrt(((1.0)+(((-1.0)*gconst24*x147))));
1421 evalcond[1]=((-1.0)*x145);
1422 evalcond[2]=((((-1.0)*x145*x147))+new_r11);
1423 evalcond[3]=((((-1.0)*x146*x147))+new_r10);
1424 evalcond[4]=(((x145*x148))+new_r01);
1425 evalcond[5]=(((x146*x148))+new_r00);
1426 evalcond[6]=(((new_r00*x148))+x146+(((-1.0)*new_r10*x147)));
1427 evalcond[7]=(((new_r01*x148))+(((-1.0)*new_r11*x147))+x145);
1435 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1436 vinfos[0].jointtype = 1;
1437 vinfos[0].foffset = j0;
1438 vinfos[0].indices[0] = _ij0[0];
1439 vinfos[0].indices[1] = _ij0[1];
1440 vinfos[0].maxsolutions = _nj0;
1441 vinfos[1].jointtype = 1;
1442 vinfos[1].foffset = j1;
1443 vinfos[1].indices[0] = _ij1[0];
1444 vinfos[1].indices[1] = _ij1[1];
1445 vinfos[1].maxsolutions = _nj1;
1446 vinfos[2].jointtype = 1;
1447 vinfos[2].foffset = j2;
1448 vinfos[2].indices[0] = _ij2[0];
1449 vinfos[2].indices[1] = _ij2[1];
1450 vinfos[2].maxsolutions = _nj2;
1451 vinfos[3].jointtype = 1;
1452 vinfos[3].foffset = j3;
1453 vinfos[3].indices[0] = _ij3[0];
1454 vinfos[3].indices[1] = _ij3[1];
1455 vinfos[3].maxsolutions = _nj3;
1456 vinfos[4].jointtype = 1;
1457 vinfos[4].foffset = j4;
1458 vinfos[4].indices[0] = _ij4[0];
1459 vinfos[4].indices[1] = _ij4[1];
1460 vinfos[4].maxsolutions = _nj4;
1461 vinfos[5].jointtype = 1;
1462 vinfos[5].foffset = j5;
1463 vinfos[5].indices[0] = _ij5[0];
1464 vinfos[5].indices[1] = _ij5[1];
1465 vinfos[5].maxsolutions = _nj5;
1466 std::vector<int> vfree(0);
1475 IkReal j5array[1], cj5array[1], sj5array[1];
1476 bool j5valid[1]={
false};
1486 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x149.
value)))+(x150.
value));
1487 sj5array[0]=
IKsin(j5array[0]);
1488 cj5array[0]=
IKcos(j5array[0]);
1489 if( j5array[0] >
IKPI )
1493 else if( j5array[0] < -
IKPI )
1494 { j5array[0]+=
IK2PI;
1497 for(
int ij5 = 0; ij5 < 1; ++ij5)
1503 _ij5[0] = ij5; _ij5[1] = -1;
1504 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1508 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1511 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1514 IkReal x151=
IKcos(j5);
1515 IkReal x152=
IKsin(j5);
1516 IkReal x153=((1.0)*gconst24);
1517 if((((1.0)+(((-1.0)*gconst24*x153)))) < -0.00001)
1519 IkReal x154=
IKsqrt(((1.0)+(((-1.0)*gconst24*x153))));
1521 evalcond[1]=((-1.0)*x151);
1522 evalcond[2]=((((-1.0)*x151*x153))+new_r11);
1523 evalcond[3]=((((-1.0)*x152*x153))+new_r10);
1524 evalcond[4]=(((x151*x154))+new_r01);
1525 evalcond[5]=(((x152*x154))+new_r00);
1526 evalcond[6]=(((new_r00*x154))+x152+(((-1.0)*new_r10*x153)));
1527 evalcond[7]=(((new_r01*x154))+x151+(((-1.0)*new_r11*x153)));
1535 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1536 vinfos[0].jointtype = 1;
1537 vinfos[0].foffset = j0;
1538 vinfos[0].indices[0] = _ij0[0];
1539 vinfos[0].indices[1] = _ij0[1];
1540 vinfos[0].maxsolutions = _nj0;
1541 vinfos[1].jointtype = 1;
1542 vinfos[1].foffset = j1;
1543 vinfos[1].indices[0] = _ij1[0];
1544 vinfos[1].indices[1] = _ij1[1];
1545 vinfos[1].maxsolutions = _nj1;
1546 vinfos[2].jointtype = 1;
1547 vinfos[2].foffset = j2;
1548 vinfos[2].indices[0] = _ij2[0];
1549 vinfos[2].indices[1] = _ij2[1];
1550 vinfos[2].maxsolutions = _nj2;
1551 vinfos[3].jointtype = 1;
1552 vinfos[3].foffset = j3;
1553 vinfos[3].indices[0] = _ij3[0];
1554 vinfos[3].indices[1] = _ij3[1];
1555 vinfos[3].maxsolutions = _nj3;
1556 vinfos[4].jointtype = 1;
1557 vinfos[4].foffset = j4;
1558 vinfos[4].indices[0] = _ij4[0];
1559 vinfos[4].indices[1] = _ij4[1];
1560 vinfos[4].maxsolutions = _nj4;
1561 vinfos[5].jointtype = 1;
1562 vinfos[5].foffset = j5;
1563 vinfos[5].indices[0] = _ij5[0];
1564 vinfos[5].indices[1] = _ij5[1];
1565 vinfos[5].maxsolutions = _nj5;
1566 std::vector<int> vfree(0);
1578 if( bgotonextstatement )
1580 bool bgotonextstatement =
true;
1587 if((x155.
value) < -0.00001)
1590 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj3+(((-1.0)*gconst24)))))+(
IKabs(((1.0)+(
IKsign(sj3)))))), 6.28318530717959)));
1591 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1593 bgotonextstatement=
false;
1600 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1602 sj3=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
1606 j3=((-1.0)*(
IKacos(gconst24)));
1611 if((x156.
value) < -0.00001)
1614 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1615 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1618 IkReal j5array[1], cj5array[1], sj5array[1];
1619 bool j5valid[1]={
false};
1621 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1629 j5array[0]=
IKatan2((((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x157.
value)));
1630 sj5array[0]=
IKsin(j5array[0]);
1631 cj5array[0]=
IKcos(j5array[0]);
1632 if( j5array[0] >
IKPI )
1636 else if( j5array[0] < -
IKPI )
1637 { j5array[0]+=
IK2PI;
1640 for(
int ij5 = 0; ij5 < 1; ++ij5)
1646 _ij5[0] = ij5; _ij5[1] = -1;
1647 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1651 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1654 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1657 IkReal x158=
IKcos(j5);
1658 IkReal x159=
IKsin(j5);
1659 IkReal x160=((1.0)*gconst24);
1660 IkReal x161=((1.0)*x159);
1661 if((((1.0)+(((-1.0)*gconst24*x160)))) < -0.00001)
1663 IkReal x162=
IKsqrt(((1.0)+(((-1.0)*gconst24*x160))));
1664 IkReal x163=((1.0)*x162);
1666 evalcond[1]=((-1.0)*x158);
1667 evalcond[2]=((((-1.0)*x158*x160))+new_r11);
1668 evalcond[3]=((((-1.0)*x159*x160))+new_r10);
1669 evalcond[4]=((((-1.0)*x158*x163))+new_r01);
1670 evalcond[5]=((((-1.0)*x161*x162))+new_r00);
1671 evalcond[6]=((((-1.0)*new_r00*x163))+(((-1.0)*new_r10*x160))+x159);
1672 evalcond[7]=((((-1.0)*new_r11*x160))+x158+(((-1.0)*new_r01*x163)));
1680 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1681 vinfos[0].jointtype = 1;
1682 vinfos[0].foffset = j0;
1683 vinfos[0].indices[0] = _ij0[0];
1684 vinfos[0].indices[1] = _ij0[1];
1685 vinfos[0].maxsolutions = _nj0;
1686 vinfos[1].jointtype = 1;
1687 vinfos[1].foffset = j1;
1688 vinfos[1].indices[0] = _ij1[0];
1689 vinfos[1].indices[1] = _ij1[1];
1690 vinfos[1].maxsolutions = _nj1;
1691 vinfos[2].jointtype = 1;
1692 vinfos[2].foffset = j2;
1693 vinfos[2].indices[0] = _ij2[0];
1694 vinfos[2].indices[1] = _ij2[1];
1695 vinfos[2].maxsolutions = _nj2;
1696 vinfos[3].jointtype = 1;
1697 vinfos[3].foffset = j3;
1698 vinfos[3].indices[0] = _ij3[0];
1699 vinfos[3].indices[1] = _ij3[1];
1700 vinfos[3].maxsolutions = _nj3;
1701 vinfos[4].jointtype = 1;
1702 vinfos[4].foffset = j4;
1703 vinfos[4].indices[0] = _ij4[0];
1704 vinfos[4].indices[1] = _ij4[1];
1705 vinfos[4].maxsolutions = _nj4;
1706 vinfos[5].jointtype = 1;
1707 vinfos[5].foffset = j5;
1708 vinfos[5].indices[0] = _ij5[0];
1709 vinfos[5].indices[1] = _ij5[1];
1710 vinfos[5].maxsolutions = _nj5;
1711 std::vector<int> vfree(0);
1720 IkReal j5array[1], cj5array[1], sj5array[1];
1721 bool j5valid[1]={
false};
1731 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x164.
value)))+(x165.
value));
1732 sj5array[0]=
IKsin(j5array[0]);
1733 cj5array[0]=
IKcos(j5array[0]);
1734 if( j5array[0] >
IKPI )
1738 else if( j5array[0] < -
IKPI )
1739 { j5array[0]+=
IK2PI;
1742 for(
int ij5 = 0; ij5 < 1; ++ij5)
1748 _ij5[0] = ij5; _ij5[1] = -1;
1749 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1753 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1759 IkReal x166=
IKcos(j5);
1760 IkReal x167=
IKsin(j5);
1761 IkReal x168=((1.0)*gconst24);
1762 IkReal x169=((1.0)*x167);
1763 if((((1.0)+(((-1.0)*gconst24*x168)))) < -0.00001)
1765 IkReal x170=
IKsqrt(((1.0)+(((-1.0)*gconst24*x168))));
1766 IkReal x171=((1.0)*x170);
1768 evalcond[1]=((-1.0)*x166);
1769 evalcond[2]=((((-1.0)*x166*x168))+new_r11);
1770 evalcond[3]=(new_r10+(((-1.0)*x167*x168)));
1771 evalcond[4]=(new_r01+(((-1.0)*x166*x171)));
1772 evalcond[5]=(new_r00+(((-1.0)*x169*x170)));
1773 evalcond[6]=((((-1.0)*new_r10*x168))+x167+(((-1.0)*new_r00*x171)));
1774 evalcond[7]=((((-1.0)*new_r11*x168))+x166+(((-1.0)*new_r01*x171)));
1782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1783 vinfos[0].jointtype = 1;
1784 vinfos[0].foffset = j0;
1785 vinfos[0].indices[0] = _ij0[0];
1786 vinfos[0].indices[1] = _ij0[1];
1787 vinfos[0].maxsolutions = _nj0;
1788 vinfos[1].jointtype = 1;
1789 vinfos[1].foffset = j1;
1790 vinfos[1].indices[0] = _ij1[0];
1791 vinfos[1].indices[1] = _ij1[1];
1792 vinfos[1].maxsolutions = _nj1;
1793 vinfos[2].jointtype = 1;
1794 vinfos[2].foffset = j2;
1795 vinfos[2].indices[0] = _ij2[0];
1796 vinfos[2].indices[1] = _ij2[1];
1797 vinfos[2].maxsolutions = _nj2;
1798 vinfos[3].jointtype = 1;
1799 vinfos[3].foffset = j3;
1800 vinfos[3].indices[0] = _ij3[0];
1801 vinfos[3].indices[1] = _ij3[1];
1802 vinfos[3].maxsolutions = _nj3;
1803 vinfos[4].jointtype = 1;
1804 vinfos[4].foffset = j4;
1805 vinfos[4].indices[0] = _ij4[0];
1806 vinfos[4].indices[1] = _ij4[1];
1807 vinfos[4].maxsolutions = _nj4;
1808 vinfos[5].jointtype = 1;
1809 vinfos[5].foffset = j5;
1810 vinfos[5].indices[0] = _ij5[0];
1811 vinfos[5].indices[1] = _ij5[1];
1812 vinfos[5].maxsolutions = _nj5;
1813 std::vector<int> vfree(0);
1825 if( bgotonextstatement )
1827 bool bgotonextstatement =
true;
1834 if((x172.
value) < -0.00001)
1837 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj3+(((-1.0)*gconst25)))))+(
IKabs(((-1.0)+(
IKsign(sj3)))))), 6.28318530717959)));
1838 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1840 bgotonextstatement=
false;
1847 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1849 sj3=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
1858 if((x173.
value) < -0.00001)
1861 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
1862 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
1865 IkReal j5array[1], cj5array[1], sj5array[1];
1866 bool j5valid[1]={
false};
1868 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1876 j5array[0]=
IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x174.
value)));
1877 sj5array[0]=
IKsin(j5array[0]);
1878 cj5array[0]=
IKcos(j5array[0]);
1879 if( j5array[0] >
IKPI )
1883 else if( j5array[0] < -
IKPI )
1884 { j5array[0]+=
IK2PI;
1887 for(
int ij5 = 0; ij5 < 1; ++ij5)
1893 _ij5[0] = ij5; _ij5[1] = -1;
1894 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1898 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
1901 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1904 IkReal x175=
IKcos(j5);
1905 IkReal x176=
IKsin(j5);
1906 IkReal x177=((1.0)*gconst25);
1907 if((((1.0)+(((-1.0)*gconst25*x177)))) < -0.00001)
1909 IkReal x178=
IKsqrt(((1.0)+(((-1.0)*gconst25*x177))));
1911 evalcond[1]=((-1.0)*x175);
1912 evalcond[2]=((((-1.0)*x175*x177))+new_r11);
1913 evalcond[3]=((((-1.0)*x176*x177))+new_r10);
1914 evalcond[4]=(((x175*x178))+new_r01);
1915 evalcond[5]=(((x176*x178))+new_r00);
1916 evalcond[6]=(((new_r00*x178))+(((-1.0)*new_r10*x177))+x176);
1917 evalcond[7]=(((new_r01*x178))+(((-1.0)*new_r11*x177))+x175);
1925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1926 vinfos[0].jointtype = 1;
1927 vinfos[0].foffset = j0;
1928 vinfos[0].indices[0] = _ij0[0];
1929 vinfos[0].indices[1] = _ij0[1];
1930 vinfos[0].maxsolutions = _nj0;
1931 vinfos[1].jointtype = 1;
1932 vinfos[1].foffset = j1;
1933 vinfos[1].indices[0] = _ij1[0];
1934 vinfos[1].indices[1] = _ij1[1];
1935 vinfos[1].maxsolutions = _nj1;
1936 vinfos[2].jointtype = 1;
1937 vinfos[2].foffset = j2;
1938 vinfos[2].indices[0] = _ij2[0];
1939 vinfos[2].indices[1] = _ij2[1];
1940 vinfos[2].maxsolutions = _nj2;
1941 vinfos[3].jointtype = 1;
1942 vinfos[3].foffset = j3;
1943 vinfos[3].indices[0] = _ij3[0];
1944 vinfos[3].indices[1] = _ij3[1];
1945 vinfos[3].maxsolutions = _nj3;
1946 vinfos[4].jointtype = 1;
1947 vinfos[4].foffset = j4;
1948 vinfos[4].indices[0] = _ij4[0];
1949 vinfos[4].indices[1] = _ij4[1];
1950 vinfos[4].maxsolutions = _nj4;
1951 vinfos[5].jointtype = 1;
1952 vinfos[5].foffset = j5;
1953 vinfos[5].indices[0] = _ij5[0];
1954 vinfos[5].indices[1] = _ij5[1];
1955 vinfos[5].maxsolutions = _nj5;
1956 std::vector<int> vfree(0);
1965 IkReal j5array[1], cj5array[1], sj5array[1];
1966 bool j5valid[1]={
false};
1976 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x179.
value)))+(x180.
value));
1977 sj5array[0]=
IKsin(j5array[0]);
1978 cj5array[0]=
IKcos(j5array[0]);
1979 if( j5array[0] >
IKPI )
1983 else if( j5array[0] < -
IKPI )
1984 { j5array[0]+=
IK2PI;
1987 for(
int ij5 = 0; ij5 < 1; ++ij5)
1993 _ij5[0] = ij5; _ij5[1] = -1;
1994 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
1998 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2001 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2004 IkReal x181=
IKcos(j5);
2005 IkReal x182=
IKsin(j5);
2006 IkReal x183=((1.0)*gconst25);
2007 if((((1.0)+(((-1.0)*gconst25*x183)))) < -0.00001)
2009 IkReal x184=
IKsqrt(((1.0)+(((-1.0)*gconst25*x183))));
2011 evalcond[1]=((-1.0)*x181);
2012 evalcond[2]=(new_r11+(((-1.0)*x181*x183)));
2013 evalcond[3]=(new_r10+(((-1.0)*x182*x183)));
2014 evalcond[4]=(new_r01+((x181*x184)));
2015 evalcond[5]=(new_r00+((x182*x184)));
2016 evalcond[6]=(((new_r00*x184))+x182+(((-1.0)*new_r10*x183)));
2017 evalcond[7]=(((new_r01*x184))+x181+(((-1.0)*new_r11*x183)));
2025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2026 vinfos[0].jointtype = 1;
2027 vinfos[0].foffset = j0;
2028 vinfos[0].indices[0] = _ij0[0];
2029 vinfos[0].indices[1] = _ij0[1];
2030 vinfos[0].maxsolutions = _nj0;
2031 vinfos[1].jointtype = 1;
2032 vinfos[1].foffset = j1;
2033 vinfos[1].indices[0] = _ij1[0];
2034 vinfos[1].indices[1] = _ij1[1];
2035 vinfos[1].maxsolutions = _nj1;
2036 vinfos[2].jointtype = 1;
2037 vinfos[2].foffset = j2;
2038 vinfos[2].indices[0] = _ij2[0];
2039 vinfos[2].indices[1] = _ij2[1];
2040 vinfos[2].maxsolutions = _nj2;
2041 vinfos[3].jointtype = 1;
2042 vinfos[3].foffset = j3;
2043 vinfos[3].indices[0] = _ij3[0];
2044 vinfos[3].indices[1] = _ij3[1];
2045 vinfos[3].maxsolutions = _nj3;
2046 vinfos[4].jointtype = 1;
2047 vinfos[4].foffset = j4;
2048 vinfos[4].indices[0] = _ij4[0];
2049 vinfos[4].indices[1] = _ij4[1];
2050 vinfos[4].maxsolutions = _nj4;
2051 vinfos[5].jointtype = 1;
2052 vinfos[5].foffset = j5;
2053 vinfos[5].indices[0] = _ij5[0];
2054 vinfos[5].indices[1] = _ij5[1];
2055 vinfos[5].maxsolutions = _nj5;
2056 std::vector<int> vfree(0);
2068 if( bgotonextstatement )
2070 bool bgotonextstatement =
true;
2077 if((x185.
value) < -0.00001)
2080 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj3+(((-1.0)*gconst25)))))+(
IKabs(((1.0)+(
IKsign(sj3)))))), 6.28318530717959)));
2081 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2083 bgotonextstatement=
false;
2090 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2092 sj3=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
2096 j3=((-1.0)*(
IKacos(gconst25)));
2101 if((x186.
value) < -0.00001)
2104 j5eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
2105 if(
IKabs(j5eval[0]) < 0.0000010000000000 )
2108 IkReal j5array[1], cj5array[1], sj5array[1];
2109 bool j5valid[1]={
false};
2111 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2119 j5array[0]=
IKatan2((((gconst25*new_r10))+((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x187.
value)));
2120 sj5array[0]=
IKsin(j5array[0]);
2121 cj5array[0]=
IKcos(j5array[0]);
2122 if( j5array[0] >
IKPI )
2126 else if( j5array[0] < -
IKPI )
2127 { j5array[0]+=
IK2PI;
2130 for(
int ij5 = 0; ij5 < 1; ++ij5)
2136 _ij5[0] = ij5; _ij5[1] = -1;
2137 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2141 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2144 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2147 IkReal x188=
IKcos(j5);
2148 IkReal x189=
IKsin(j5);
2149 IkReal x190=((1.0)*gconst25);
2150 if((((1.0)+(((-1.0)*gconst25*x190)))) < -0.00001)
2152 IkReal x191=
IKsqrt(((1.0)+(((-1.0)*gconst25*x190))));
2153 IkReal x192=((1.0)*x191);
2155 evalcond[1]=((-1.0)*x188);
2156 evalcond[2]=((((-1.0)*x188*x190))+new_r11);
2157 evalcond[3]=((((-1.0)*x189*x190))+new_r10);
2158 evalcond[4]=((((-1.0)*x188*x192))+new_r01);
2159 evalcond[5]=((((-1.0)*x189*x192))+new_r00);
2160 evalcond[6]=(x189+(((-1.0)*new_r10*x190))+(((-1.0)*new_r00*x192)));
2161 evalcond[7]=(x188+(((-1.0)*new_r11*x190))+(((-1.0)*new_r01*x192)));
2169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2170 vinfos[0].jointtype = 1;
2171 vinfos[0].foffset = j0;
2172 vinfos[0].indices[0] = _ij0[0];
2173 vinfos[0].indices[1] = _ij0[1];
2174 vinfos[0].maxsolutions = _nj0;
2175 vinfos[1].jointtype = 1;
2176 vinfos[1].foffset = j1;
2177 vinfos[1].indices[0] = _ij1[0];
2178 vinfos[1].indices[1] = _ij1[1];
2179 vinfos[1].maxsolutions = _nj1;
2180 vinfos[2].jointtype = 1;
2181 vinfos[2].foffset = j2;
2182 vinfos[2].indices[0] = _ij2[0];
2183 vinfos[2].indices[1] = _ij2[1];
2184 vinfos[2].maxsolutions = _nj2;
2185 vinfos[3].jointtype = 1;
2186 vinfos[3].foffset = j3;
2187 vinfos[3].indices[0] = _ij3[0];
2188 vinfos[3].indices[1] = _ij3[1];
2189 vinfos[3].maxsolutions = _nj3;
2190 vinfos[4].jointtype = 1;
2191 vinfos[4].foffset = j4;
2192 vinfos[4].indices[0] = _ij4[0];
2193 vinfos[4].indices[1] = _ij4[1];
2194 vinfos[4].maxsolutions = _nj4;
2195 vinfos[5].jointtype = 1;
2196 vinfos[5].foffset = j5;
2197 vinfos[5].indices[0] = _ij5[0];
2198 vinfos[5].indices[1] = _ij5[1];
2199 vinfos[5].maxsolutions = _nj5;
2200 std::vector<int> vfree(0);
2209 IkReal j5array[1], cj5array[1], sj5array[1];
2210 bool j5valid[1]={
false};
2220 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x193.
value)))+(x194.
value));
2221 sj5array[0]=
IKsin(j5array[0]);
2222 cj5array[0]=
IKcos(j5array[0]);
2223 if( j5array[0] >
IKPI )
2227 else if( j5array[0] < -
IKPI )
2228 { j5array[0]+=
IK2PI;
2231 for(
int ij5 = 0; ij5 < 1; ++ij5)
2237 _ij5[0] = ij5; _ij5[1] = -1;
2238 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2242 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2245 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2248 IkReal x195=
IKcos(j5);
2249 IkReal x196=
IKsin(j5);
2250 IkReal x197=((1.0)*gconst25);
2251 if((((1.0)+(((-1.0)*gconst25*x197)))) < -0.00001)
2253 IkReal x198=
IKsqrt(((1.0)+(((-1.0)*gconst25*x197))));
2254 IkReal x199=((1.0)*x198);
2256 evalcond[1]=((-1.0)*x195);
2257 evalcond[2]=((((-1.0)*x195*x197))+new_r11);
2258 evalcond[3]=((((-1.0)*x196*x197))+new_r10);
2259 evalcond[4]=((((-1.0)*x195*x199))+new_r01);
2260 evalcond[5]=((((-1.0)*x196*x199))+new_r00);
2261 evalcond[6]=(x196+(((-1.0)*new_r10*x197))+(((-1.0)*new_r00*x199)));
2262 evalcond[7]=(x195+(((-1.0)*new_r11*x197))+(((-1.0)*new_r01*x199)));
2270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2271 vinfos[0].jointtype = 1;
2272 vinfos[0].foffset = j0;
2273 vinfos[0].indices[0] = _ij0[0];
2274 vinfos[0].indices[1] = _ij0[1];
2275 vinfos[0].maxsolutions = _nj0;
2276 vinfos[1].jointtype = 1;
2277 vinfos[1].foffset = j1;
2278 vinfos[1].indices[0] = _ij1[0];
2279 vinfos[1].indices[1] = _ij1[1];
2280 vinfos[1].maxsolutions = _nj1;
2281 vinfos[2].jointtype = 1;
2282 vinfos[2].foffset = j2;
2283 vinfos[2].indices[0] = _ij2[0];
2284 vinfos[2].indices[1] = _ij2[1];
2285 vinfos[2].maxsolutions = _nj2;
2286 vinfos[3].jointtype = 1;
2287 vinfos[3].foffset = j3;
2288 vinfos[3].indices[0] = _ij3[0];
2289 vinfos[3].indices[1] = _ij3[1];
2290 vinfos[3].maxsolutions = _nj3;
2291 vinfos[4].jointtype = 1;
2292 vinfos[4].foffset = j4;
2293 vinfos[4].indices[0] = _ij4[0];
2294 vinfos[4].indices[1] = _ij4[1];
2295 vinfos[4].maxsolutions = _nj4;
2296 vinfos[5].jointtype = 1;
2297 vinfos[5].foffset = j5;
2298 vinfos[5].indices[0] = _ij5[0];
2299 vinfos[5].indices[1] = _ij5[1];
2300 vinfos[5].maxsolutions = _nj5;
2301 std::vector<int> vfree(0);
2313 if( bgotonextstatement )
2315 bool bgotonextstatement =
true;
2320 bgotonextstatement=
false;
2325 if( bgotonextstatement )
2339 IkReal j5array[1], cj5array[1], sj5array[1];
2340 bool j5valid[1]={
false};
2342 IkReal x200=(new_r00*sj3);
2349 j5array[0]=
IKatan2((((cj3*new_r10))+(((-1.0)*x200))), ((x201.
value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2350 sj5array[0]=
IKsin(j5array[0]);
2351 cj5array[0]=
IKcos(j5array[0]);
2352 if( j5array[0] >
IKPI )
2356 else if( j5array[0] < -
IKPI )
2357 { j5array[0]+=
IK2PI;
2360 for(
int ij5 = 0; ij5 < 1; ++ij5)
2366 _ij5[0] = ij5; _ij5[1] = -1;
2367 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2371 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2374 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2376 IkReal evalcond[10];
2377 IkReal x202=
IKsin(j5);
2378 IkReal x203=
IKcos(j5);
2379 IkReal x204=((1.0)*cj3);
2380 IkReal x205=(cj3*new_r00);
2381 IkReal x206=(cj3*new_r22);
2382 IkReal x207=(new_r22*sj3);
2383 IkReal x208=(new_r22*x203);
2384 IkReal x209=(sj3*x202);
2385 evalcond[0]=(((new_r00*sj3))+x202+(((-1.0)*new_r10*x204)));
2386 evalcond[1]=(((new_r01*sj3))+x203+(((-1.0)*new_r11*x204)));
2387 evalcond[2]=(((new_r11*sj3))+((new_r22*x202))+((cj3*new_r01)));
2388 evalcond[3]=(((new_r11*x207))+((new_r01*x206))+x202);
2389 evalcond[4]=(((sj3*x203))+((x202*x206))+new_r01);
2390 evalcond[5]=(((new_r10*sj3))+x205+(((-1.0)*x208)));
2391 evalcond[6]=((((-1.0)*x204*x208))+x209+new_r00);
2392 evalcond[7]=((((-1.0)*x203*x204))+((x202*x207))+new_r11);
2393 evalcond[8]=(((new_r10*x207))+((new_r22*x205))+(((-1.0)*x203)));
2394 evalcond[9]=((((-1.0)*x202*x204))+(((-1.0)*x203*x207))+new_r10);
2402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2403 vinfos[0].jointtype = 1;
2404 vinfos[0].foffset = j0;
2405 vinfos[0].indices[0] = _ij0[0];
2406 vinfos[0].indices[1] = _ij0[1];
2407 vinfos[0].maxsolutions = _nj0;
2408 vinfos[1].jointtype = 1;
2409 vinfos[1].foffset = j1;
2410 vinfos[1].indices[0] = _ij1[0];
2411 vinfos[1].indices[1] = _ij1[1];
2412 vinfos[1].maxsolutions = _nj1;
2413 vinfos[2].jointtype = 1;
2414 vinfos[2].foffset = j2;
2415 vinfos[2].indices[0] = _ij2[0];
2416 vinfos[2].indices[1] = _ij2[1];
2417 vinfos[2].maxsolutions = _nj2;
2418 vinfos[3].jointtype = 1;
2419 vinfos[3].foffset = j3;
2420 vinfos[3].indices[0] = _ij3[0];
2421 vinfos[3].indices[1] = _ij3[1];
2422 vinfos[3].maxsolutions = _nj3;
2423 vinfos[4].jointtype = 1;
2424 vinfos[4].foffset = j4;
2425 vinfos[4].indices[0] = _ij4[0];
2426 vinfos[4].indices[1] = _ij4[1];
2427 vinfos[4].maxsolutions = _nj4;
2428 vinfos[5].jointtype = 1;
2429 vinfos[5].foffset = j5;
2430 vinfos[5].indices[0] = _ij5[0];
2431 vinfos[5].indices[1] = _ij5[1];
2432 vinfos[5].maxsolutions = _nj5;
2433 std::vector<int> vfree(0);
2446 IkReal j5array[1], cj5array[1], sj5array[1];
2447 bool j5valid[1]={
false};
2449 IkReal x210=((1.0)*new_r01);
2456 j5array[0]=
IKatan2(((x211.
value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3))))), ((((-1.0)*sj3*x210))+((cj3*new_r11))));
2457 sj5array[0]=
IKsin(j5array[0]);
2458 cj5array[0]=
IKcos(j5array[0]);
2459 if( j5array[0] >
IKPI )
2463 else if( j5array[0] < -
IKPI )
2464 { j5array[0]+=
IK2PI;
2467 for(
int ij5 = 0; ij5 < 1; ++ij5)
2473 _ij5[0] = ij5; _ij5[1] = -1;
2474 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2478 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2481 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2483 IkReal evalcond[10];
2484 IkReal x212=
IKsin(j5);
2485 IkReal x213=
IKcos(j5);
2486 IkReal x214=((1.0)*cj3);
2487 IkReal x215=(cj3*new_r00);
2488 IkReal x216=(cj3*new_r22);
2489 IkReal x217=(new_r22*sj3);
2490 IkReal x218=(new_r22*x213);
2491 IkReal x219=(sj3*x212);
2492 evalcond[0]=(((new_r00*sj3))+(((-1.0)*new_r10*x214))+x212);
2493 evalcond[1]=((((-1.0)*new_r11*x214))+((new_r01*sj3))+x213);
2494 evalcond[2]=(((new_r22*x212))+((new_r11*sj3))+((cj3*new_r01)));
2495 evalcond[3]=(x212+((new_r01*x216))+((new_r11*x217)));
2496 evalcond[4]=(((sj3*x213))+((x212*x216))+new_r01);
2497 evalcond[5]=(((new_r10*sj3))+x215+(((-1.0)*x218)));
2498 evalcond[6]=(x219+(((-1.0)*x214*x218))+new_r00);
2499 evalcond[7]=(((x212*x217))+(((-1.0)*x213*x214))+new_r11);
2500 evalcond[8]=(((new_r22*x215))+(((-1.0)*x213))+((new_r10*x217)));
2501 evalcond[9]=((((-1.0)*x213*x217))+new_r10+(((-1.0)*x212*x214)));
2509 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2510 vinfos[0].jointtype = 1;
2511 vinfos[0].foffset = j0;
2512 vinfos[0].indices[0] = _ij0[0];
2513 vinfos[0].indices[1] = _ij0[1];
2514 vinfos[0].maxsolutions = _nj0;
2515 vinfos[1].jointtype = 1;
2516 vinfos[1].foffset = j1;
2517 vinfos[1].indices[0] = _ij1[0];
2518 vinfos[1].indices[1] = _ij1[1];
2519 vinfos[1].maxsolutions = _nj1;
2520 vinfos[2].jointtype = 1;
2521 vinfos[2].foffset = j2;
2522 vinfos[2].indices[0] = _ij2[0];
2523 vinfos[2].indices[1] = _ij2[1];
2524 vinfos[2].maxsolutions = _nj2;
2525 vinfos[3].jointtype = 1;
2526 vinfos[3].foffset = j3;
2527 vinfos[3].indices[0] = _ij3[0];
2528 vinfos[3].indices[1] = _ij3[1];
2529 vinfos[3].maxsolutions = _nj3;
2530 vinfos[4].jointtype = 1;
2531 vinfos[4].foffset = j4;
2532 vinfos[4].indices[0] = _ij4[0];
2533 vinfos[4].indices[1] = _ij4[1];
2534 vinfos[4].maxsolutions = _nj4;
2535 vinfos[5].jointtype = 1;
2536 vinfos[5].foffset = j5;
2537 vinfos[5].indices[0] = _ij5[0];
2538 vinfos[5].indices[1] = _ij5[1];
2539 vinfos[5].maxsolutions = _nj5;
2540 std::vector<int> vfree(0);
2553 IkReal j5array[1], cj5array[1], sj5array[1];
2554 bool j5valid[1]={
false};
2556 IkReal x220=cj3*cj3;
2557 IkReal x221=(cj3*new_r22);
2566 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x222.
value)))+(x223.
value));
2567 sj5array[0]=
IKsin(j5array[0]);
2568 cj5array[0]=
IKcos(j5array[0]);
2569 if( j5array[0] >
IKPI )
2573 else if( j5array[0] < -
IKPI )
2574 { j5array[0]+=
IK2PI;
2577 for(
int ij5 = 0; ij5 < 1; ++ij5)
2583 _ij5[0] = ij5; _ij5[1] = -1;
2584 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2588 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2591 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2593 IkReal evalcond[10];
2594 IkReal x224=
IKsin(j5);
2595 IkReal x225=
IKcos(j5);
2596 IkReal x226=((1.0)*cj3);
2597 IkReal x227=(cj3*new_r00);
2598 IkReal x228=(cj3*new_r22);
2599 IkReal x229=(new_r22*sj3);
2600 IkReal x230=(new_r22*x225);
2601 IkReal x231=(sj3*x224);
2602 evalcond[0]=((((-1.0)*new_r10*x226))+((new_r00*sj3))+x224);
2603 evalcond[1]=((((-1.0)*new_r11*x226))+((new_r01*sj3))+x225);
2604 evalcond[2]=(((new_r11*sj3))+((new_r22*x224))+((cj3*new_r01)));
2605 evalcond[3]=(((new_r01*x228))+x224+((new_r11*x229)));
2606 evalcond[4]=(((x224*x228))+((sj3*x225))+new_r01);
2607 evalcond[5]=(((new_r10*sj3))+x227+(((-1.0)*x230)));
2608 evalcond[6]=(x231+(((-1.0)*x226*x230))+new_r00);
2609 evalcond[7]=(((x224*x229))+(((-1.0)*x225*x226))+new_r11);
2610 evalcond[8]=(((new_r22*x227))+((new_r10*x229))+(((-1.0)*x225)));
2611 evalcond[9]=((((-1.0)*x224*x226))+(((-1.0)*x225*x229))+new_r10);
2619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2620 vinfos[0].jointtype = 1;
2621 vinfos[0].foffset = j0;
2622 vinfos[0].indices[0] = _ij0[0];
2623 vinfos[0].indices[1] = _ij0[1];
2624 vinfos[0].maxsolutions = _nj0;
2625 vinfos[1].jointtype = 1;
2626 vinfos[1].foffset = j1;
2627 vinfos[1].indices[0] = _ij1[0];
2628 vinfos[1].indices[1] = _ij1[1];
2629 vinfos[1].maxsolutions = _nj1;
2630 vinfos[2].jointtype = 1;
2631 vinfos[2].foffset = j2;
2632 vinfos[2].indices[0] = _ij2[0];
2633 vinfos[2].indices[1] = _ij2[1];
2634 vinfos[2].maxsolutions = _nj2;
2635 vinfos[3].jointtype = 1;
2636 vinfos[3].foffset = j3;
2637 vinfos[3].indices[0] = _ij3[0];
2638 vinfos[3].indices[1] = _ij3[1];
2639 vinfos[3].maxsolutions = _nj3;
2640 vinfos[4].jointtype = 1;
2641 vinfos[4].foffset = j4;
2642 vinfos[4].indices[0] = _ij4[0];
2643 vinfos[4].indices[1] = _ij4[1];
2644 vinfos[4].maxsolutions = _nj4;
2645 vinfos[5].jointtype = 1;
2646 vinfos[5].foffset = j5;
2647 vinfos[5].indices[0] = _ij5[0];
2648 vinfos[5].indices[1] = _ij5[1];
2649 vinfos[5].maxsolutions = _nj5;
2650 std::vector<int> vfree(0);
2667 if( bgotonextstatement )
2669 bool bgotonextstatement =
true;
2674 bgotonextstatement=
false;
2679 if( bgotonextstatement )
2690 IkReal j3array[1], cj3array[1], sj3array[1];
2691 bool j3valid[1]={
false};
2697 IkReal x232=x233.
value;
2704 j3array[0]=
IKatan2((x232*(x234.
value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x232));
2705 sj3array[0]=
IKsin(j3array[0]);
2706 cj3array[0]=
IKcos(j3array[0]);
2707 if( j3array[0] >
IKPI )
2711 else if( j3array[0] < -
IKPI )
2712 { j3array[0]+=
IK2PI;
2715 for(
int ij3 = 0; ij3 < 1; ++ij3)
2721 _ij3[0] = ij3; _ij3[1] = -1;
2722 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
2726 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
2729 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2732 IkReal x235=
IKcos(j3);
2733 IkReal x236=
IKsin(j3);
2734 IkReal x237=((1.0)*cj4);
2735 IkReal x238=(new_r02*x235);
2736 IkReal x239=(sj4*x236);
2737 IkReal x240=(sj4*x235);
2738 IkReal x241=(new_r12*x236);
2739 evalcond[0]=(x240+new_r02);
2740 evalcond[1]=(x239+new_r12);
2741 evalcond[2]=((((-1.0)*new_r02*x236))+((new_r12*x235)));
2742 evalcond[3]=(sj4+x238+x241);
2743 evalcond[4]=(((cj4*x241))+((new_r22*sj4))+((cj4*x238)));
2744 evalcond[5]=(((new_r10*x239))+(((-1.0)*new_r20*x237))+((new_r00*x240)));
2745 evalcond[6]=((((-1.0)*new_r21*x237))+((new_r11*x239))+((new_r01*x240)));
2746 evalcond[7]=((1.0)+(((-1.0)*new_r22*x237))+((new_r12*x239))+((sj4*x238)));
2757 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
2758 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
2764 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
2770 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
2774 bool bgotonextstatement =
true;
2777 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
2778 evalcond[1]=new_r02;
2779 evalcond[2]=new_r12;
2780 evalcond[3]=new_r21;
2781 evalcond[4]=new_r20;
2782 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 )
2784 bgotonextstatement=
false;
2786 IkReal j5array[1], cj5array[1], sj5array[1];
2787 bool j5valid[1]={
false};
2789 IkReal x242=((1.0)*new_r01);
2792 j5array[0]=
IKatan2(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x242))+((cj3*new_r00))));
2793 sj5array[0]=
IKsin(j5array[0]);
2794 cj5array[0]=
IKcos(j5array[0]);
2795 if( j5array[0] >
IKPI )
2799 else if( j5array[0] < -
IKPI )
2800 { j5array[0]+=
IK2PI;
2803 for(
int ij5 = 0; ij5 < 1; ++ij5)
2809 _ij5[0] = ij5; _ij5[1] = -1;
2810 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2814 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2817 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2820 IkReal x243=
IKsin(j5);
2821 IkReal x244=
IKcos(j5);
2822 IkReal x245=((1.0)*cj3);
2823 IkReal x246=(sj3*x243);
2824 IkReal x247=((1.0)*x244);
2825 IkReal x248=(x244*x245);
2826 evalcond[0]=(((new_r11*sj3))+x243+((cj3*new_r01)));
2827 evalcond[1]=(((new_r00*sj3))+x243+(((-1.0)*new_r10*x245)));
2828 evalcond[2]=(((new_r01*sj3))+x244+(((-1.0)*new_r11*x245)));
2829 evalcond[3]=(((sj3*x244))+new_r01+((cj3*x243)));
2830 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x247)));
2831 evalcond[5]=(x246+(((-1.0)*x248))+new_r00);
2832 evalcond[6]=(x246+(((-1.0)*x248))+new_r11);
2833 evalcond[7]=((((-1.0)*sj3*x247))+(((-1.0)*x243*x245))+new_r10);
2841 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2842 vinfos[0].jointtype = 1;
2843 vinfos[0].foffset = j0;
2844 vinfos[0].indices[0] = _ij0[0];
2845 vinfos[0].indices[1] = _ij0[1];
2846 vinfos[0].maxsolutions = _nj0;
2847 vinfos[1].jointtype = 1;
2848 vinfos[1].foffset = j1;
2849 vinfos[1].indices[0] = _ij1[0];
2850 vinfos[1].indices[1] = _ij1[1];
2851 vinfos[1].maxsolutions = _nj1;
2852 vinfos[2].jointtype = 1;
2853 vinfos[2].foffset = j2;
2854 vinfos[2].indices[0] = _ij2[0];
2855 vinfos[2].indices[1] = _ij2[1];
2856 vinfos[2].maxsolutions = _nj2;
2857 vinfos[3].jointtype = 1;
2858 vinfos[3].foffset = j3;
2859 vinfos[3].indices[0] = _ij3[0];
2860 vinfos[3].indices[1] = _ij3[1];
2861 vinfos[3].maxsolutions = _nj3;
2862 vinfos[4].jointtype = 1;
2863 vinfos[4].foffset = j4;
2864 vinfos[4].indices[0] = _ij4[0];
2865 vinfos[4].indices[1] = _ij4[1];
2866 vinfos[4].maxsolutions = _nj4;
2867 vinfos[5].jointtype = 1;
2868 vinfos[5].foffset = j5;
2869 vinfos[5].indices[0] = _ij5[0];
2870 vinfos[5].indices[1] = _ij5[1];
2871 vinfos[5].maxsolutions = _nj5;
2872 std::vector<int> vfree(0);
2880 if( bgotonextstatement )
2882 bool bgotonextstatement =
true;
2885 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2886 evalcond[1]=new_r02;
2887 evalcond[2]=new_r12;
2888 evalcond[3]=new_r21;
2889 evalcond[4]=new_r20;
2890 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 )
2892 bgotonextstatement=
false;
2894 IkReal j5array[1], cj5array[1], sj5array[1];
2895 bool j5valid[1]={
false};
2897 IkReal x249=((1.0)*sj3);
2900 j5array[0]=
IKatan2(((((-1.0)*new_r00*x249))+((cj3*new_r01))), ((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00))));
2901 sj5array[0]=
IKsin(j5array[0]);
2902 cj5array[0]=
IKcos(j5array[0]);
2903 if( j5array[0] >
IKPI )
2907 else if( j5array[0] < -
IKPI )
2908 { j5array[0]+=
IK2PI;
2911 for(
int ij5 = 0; ij5 < 1; ++ij5)
2917 _ij5[0] = ij5; _ij5[1] = -1;
2918 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
2922 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
2925 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2928 IkReal x250=
IKcos(j5);
2929 IkReal x251=
IKsin(j5);
2930 IkReal x252=((1.0)*cj3);
2931 IkReal x253=(sj3*x250);
2932 IkReal x254=((1.0)*x251);
2933 IkReal x255=(x251*x252);
2934 evalcond[0]=(((new_r10*sj3))+x250+((cj3*new_r00)));
2935 evalcond[1]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x251);
2936 evalcond[2]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x250);
2937 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x254)));
2938 evalcond[4]=(((cj3*x250))+((sj3*x251))+new_r00);
2939 evalcond[5]=(x253+new_r01+(((-1.0)*x255)));
2940 evalcond[6]=(x253+new_r10+(((-1.0)*x255)));
2941 evalcond[7]=((((-1.0)*x250*x252))+(((-1.0)*sj3*x254))+new_r11);
2949 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2950 vinfos[0].jointtype = 1;
2951 vinfos[0].foffset = j0;
2952 vinfos[0].indices[0] = _ij0[0];
2953 vinfos[0].indices[1] = _ij0[1];
2954 vinfos[0].maxsolutions = _nj0;
2955 vinfos[1].jointtype = 1;
2956 vinfos[1].foffset = j1;
2957 vinfos[1].indices[0] = _ij1[0];
2958 vinfos[1].indices[1] = _ij1[1];
2959 vinfos[1].maxsolutions = _nj1;
2960 vinfos[2].jointtype = 1;
2961 vinfos[2].foffset = j2;
2962 vinfos[2].indices[0] = _ij2[0];
2963 vinfos[2].indices[1] = _ij2[1];
2964 vinfos[2].maxsolutions = _nj2;
2965 vinfos[3].jointtype = 1;
2966 vinfos[3].foffset = j3;
2967 vinfos[3].indices[0] = _ij3[0];
2968 vinfos[3].indices[1] = _ij3[1];
2969 vinfos[3].maxsolutions = _nj3;
2970 vinfos[4].jointtype = 1;
2971 vinfos[4].foffset = j4;
2972 vinfos[4].indices[0] = _ij4[0];
2973 vinfos[4].indices[1] = _ij4[1];
2974 vinfos[4].maxsolutions = _nj4;
2975 vinfos[5].jointtype = 1;
2976 vinfos[5].foffset = j5;
2977 vinfos[5].indices[0] = _ij5[0];
2978 vinfos[5].indices[1] = _ij5[1];
2979 vinfos[5].maxsolutions = _nj5;
2980 std::vector<int> vfree(0);
2988 if( bgotonextstatement )
2990 bool bgotonextstatement =
true;
2993 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
2994 evalcond[1]=new_r02;
2995 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
2997 bgotonextstatement=
false;
2999 IkReal j5array[1], cj5array[1], sj5array[1];
3000 bool j5valid[1]={
false};
3004 j5array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3005 sj5array[0]=
IKsin(j5array[0]);
3006 cj5array[0]=
IKcos(j5array[0]);
3007 if( j5array[0] >
IKPI )
3011 else if( j5array[0] < -
IKPI )
3012 { j5array[0]+=
IK2PI;
3015 for(
int ij5 = 0; ij5 < 1; ++ij5)
3021 _ij5[0] = ij5; _ij5[1] = -1;
3022 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3026 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3029 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3032 IkReal x256=
IKsin(j5);
3033 IkReal x257=
IKcos(j5);
3034 IkReal x258=((1.0)*x257);
3035 evalcond[0]=(x256+new_r00);
3036 evalcond[1]=(x257+new_r01);
3037 evalcond[2]=(((sj4*x256))+new_r21);
3038 evalcond[3]=(new_r11+((cj4*x256)));
3039 evalcond[4]=((((-1.0)*sj4*x258))+new_r20);
3040 evalcond[5]=((((-1.0)*cj4*x258))+new_r10);
3041 evalcond[6]=(((cj4*new_r11))+x256+((new_r21*sj4)));
3042 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x258)));
3050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3051 vinfos[0].jointtype = 1;
3052 vinfos[0].foffset = j0;
3053 vinfos[0].indices[0] = _ij0[0];
3054 vinfos[0].indices[1] = _ij0[1];
3055 vinfos[0].maxsolutions = _nj0;
3056 vinfos[1].jointtype = 1;
3057 vinfos[1].foffset = j1;
3058 vinfos[1].indices[0] = _ij1[0];
3059 vinfos[1].indices[1] = _ij1[1];
3060 vinfos[1].maxsolutions = _nj1;
3061 vinfos[2].jointtype = 1;
3062 vinfos[2].foffset = j2;
3063 vinfos[2].indices[0] = _ij2[0];
3064 vinfos[2].indices[1] = _ij2[1];
3065 vinfos[2].maxsolutions = _nj2;
3066 vinfos[3].jointtype = 1;
3067 vinfos[3].foffset = j3;
3068 vinfos[3].indices[0] = _ij3[0];
3069 vinfos[3].indices[1] = _ij3[1];
3070 vinfos[3].maxsolutions = _nj3;
3071 vinfos[4].jointtype = 1;
3072 vinfos[4].foffset = j4;
3073 vinfos[4].indices[0] = _ij4[0];
3074 vinfos[4].indices[1] = _ij4[1];
3075 vinfos[4].maxsolutions = _nj4;
3076 vinfos[5].jointtype = 1;
3077 vinfos[5].foffset = j5;
3078 vinfos[5].indices[0] = _ij5[0];
3079 vinfos[5].indices[1] = _ij5[1];
3080 vinfos[5].maxsolutions = _nj5;
3081 std::vector<int> vfree(0);
3089 if( bgotonextstatement )
3091 bool bgotonextstatement =
true;
3094 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3095 evalcond[1]=new_r02;
3096 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3098 bgotonextstatement=
false;
3100 IkReal j5array[1], cj5array[1], sj5array[1];
3101 bool j5valid[1]={
false};
3105 j5array[0]=
IKatan2(new_r00, new_r01);
3106 sj5array[0]=
IKsin(j5array[0]);
3107 cj5array[0]=
IKcos(j5array[0]);
3108 if( j5array[0] >
IKPI )
3112 else if( j5array[0] < -
IKPI )
3113 { j5array[0]+=
IK2PI;
3116 for(
int ij5 = 0; ij5 < 1; ++ij5)
3122 _ij5[0] = ij5; _ij5[1] = -1;
3123 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3127 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3130 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3133 IkReal x259=
IKsin(j5);
3134 IkReal x260=
IKcos(j5);
3135 IkReal x261=((1.0)*cj4);
3136 IkReal x262=((1.0)*x260);
3137 evalcond[0]=(((sj4*x259))+new_r21);
3138 evalcond[1]=(x259+(((-1.0)*new_r00)));
3139 evalcond[2]=(x260+(((-1.0)*new_r01)));
3140 evalcond[3]=((((-1.0)*sj4*x262))+new_r20);
3141 evalcond[4]=((((-1.0)*new_r11))+((cj4*x259)));
3142 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x260*x261)));
3143 evalcond[6]=(x259+(((-1.0)*new_r11*x261))+((new_r21*sj4)));
3144 evalcond[7]=(((new_r20*sj4))+(((-1.0)*new_r10*x261))+(((-1.0)*x262)));
3152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3153 vinfos[0].jointtype = 1;
3154 vinfos[0].foffset = j0;
3155 vinfos[0].indices[0] = _ij0[0];
3156 vinfos[0].indices[1] = _ij0[1];
3157 vinfos[0].maxsolutions = _nj0;
3158 vinfos[1].jointtype = 1;
3159 vinfos[1].foffset = j1;
3160 vinfos[1].indices[0] = _ij1[0];
3161 vinfos[1].indices[1] = _ij1[1];
3162 vinfos[1].maxsolutions = _nj1;
3163 vinfos[2].jointtype = 1;
3164 vinfos[2].foffset = j2;
3165 vinfos[2].indices[0] = _ij2[0];
3166 vinfos[2].indices[1] = _ij2[1];
3167 vinfos[2].maxsolutions = _nj2;
3168 vinfos[3].jointtype = 1;
3169 vinfos[3].foffset = j3;
3170 vinfos[3].indices[0] = _ij3[0];
3171 vinfos[3].indices[1] = _ij3[1];
3172 vinfos[3].maxsolutions = _nj3;
3173 vinfos[4].jointtype = 1;
3174 vinfos[4].foffset = j4;
3175 vinfos[4].indices[0] = _ij4[0];
3176 vinfos[4].indices[1] = _ij4[1];
3177 vinfos[4].maxsolutions = _nj4;
3178 vinfos[5].jointtype = 1;
3179 vinfos[5].foffset = j5;
3180 vinfos[5].indices[0] = _ij5[0];
3181 vinfos[5].indices[1] = _ij5[1];
3182 vinfos[5].maxsolutions = _nj5;
3183 std::vector<int> vfree(0);
3191 if( bgotonextstatement )
3193 bool bgotonextstatement =
true;
3196 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
3197 evalcond[1]=new_r12;
3198 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3200 bgotonextstatement=
false;
3202 IkReal j5array[1], cj5array[1], sj5array[1];
3203 bool j5valid[1]={
false};
3207 j5array[0]=
IKatan2(new_r10, new_r11);
3208 sj5array[0]=
IKsin(j5array[0]);
3209 cj5array[0]=
IKcos(j5array[0]);
3210 if( j5array[0] >
IKPI )
3214 else if( j5array[0] < -
IKPI )
3215 { j5array[0]+=
IK2PI;
3218 for(
int ij5 = 0; ij5 < 1; ++ij5)
3224 _ij5[0] = ij5; _ij5[1] = -1;
3225 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3229 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3232 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3235 IkReal x263=
IKcos(j5);
3236 IkReal x264=
IKsin(j5);
3237 IkReal x265=((1.0)*x263);
3238 evalcond[0]=(new_r20+((new_r02*x263)));
3239 evalcond[1]=(x264+(((-1.0)*new_r10)));
3240 evalcond[2]=(x263+(((-1.0)*new_r11)));
3241 evalcond[3]=(((cj4*x264))+new_r01);
3242 evalcond[4]=((((-1.0)*new_r02*x264))+new_r21);
3243 evalcond[5]=((((-1.0)*cj4*x265))+new_r00);
3244 evalcond[6]=(((cj4*new_r01))+x264+((new_r21*sj4)));
3245 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x265)));
3253 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3254 vinfos[0].jointtype = 1;
3255 vinfos[0].foffset = j0;
3256 vinfos[0].indices[0] = _ij0[0];
3257 vinfos[0].indices[1] = _ij0[1];
3258 vinfos[0].maxsolutions = _nj0;
3259 vinfos[1].jointtype = 1;
3260 vinfos[1].foffset = j1;
3261 vinfos[1].indices[0] = _ij1[0];
3262 vinfos[1].indices[1] = _ij1[1];
3263 vinfos[1].maxsolutions = _nj1;
3264 vinfos[2].jointtype = 1;
3265 vinfos[2].foffset = j2;
3266 vinfos[2].indices[0] = _ij2[0];
3267 vinfos[2].indices[1] = _ij2[1];
3268 vinfos[2].maxsolutions = _nj2;
3269 vinfos[3].jointtype = 1;
3270 vinfos[3].foffset = j3;
3271 vinfos[3].indices[0] = _ij3[0];
3272 vinfos[3].indices[1] = _ij3[1];
3273 vinfos[3].maxsolutions = _nj3;
3274 vinfos[4].jointtype = 1;
3275 vinfos[4].foffset = j4;
3276 vinfos[4].indices[0] = _ij4[0];
3277 vinfos[4].indices[1] = _ij4[1];
3278 vinfos[4].maxsolutions = _nj4;
3279 vinfos[5].jointtype = 1;
3280 vinfos[5].foffset = j5;
3281 vinfos[5].indices[0] = _ij5[0];
3282 vinfos[5].indices[1] = _ij5[1];
3283 vinfos[5].maxsolutions = _nj5;
3284 std::vector<int> vfree(0);
3292 if( bgotonextstatement )
3294 bool bgotonextstatement =
true;
3297 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3298 evalcond[1]=new_r12;
3299 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
3301 bgotonextstatement=
false;
3303 IkReal j5array[1], cj5array[1], sj5array[1];
3304 bool j5valid[1]={
false};
3308 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3309 sj5array[0]=
IKsin(j5array[0]);
3310 cj5array[0]=
IKcos(j5array[0]);
3311 if( j5array[0] >
IKPI )
3315 else if( j5array[0] < -
IKPI )
3316 { j5array[0]+=
IK2PI;
3319 for(
int ij5 = 0; ij5 < 1; ++ij5)
3325 _ij5[0] = ij5; _ij5[1] = -1;
3326 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3330 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3333 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3336 IkReal x266=
IKsin(j5);
3337 IkReal x267=
IKcos(j5);
3338 IkReal x268=((1.0)*new_r00);
3339 IkReal x269=((1.0)*new_r01);
3340 IkReal x270=((1.0)*x267);
3341 evalcond[0]=(x266+new_r10);
3342 evalcond[1]=(x267+new_r11);
3343 evalcond[2]=(new_r21+((new_r02*x266)));
3344 evalcond[3]=((((-1.0)*new_r02*x270))+new_r20);
3345 evalcond[4]=(((cj4*x266))+(((-1.0)*x269)));
3346 evalcond[5]=((((-1.0)*cj4*x270))+(((-1.0)*x268)));
3347 evalcond[6]=((((-1.0)*cj4*x269))+x266+((new_r21*sj4)));
3348 evalcond[7]=((((-1.0)*cj4*x268))+((new_r20*sj4))+(((-1.0)*x270)));
3356 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3357 vinfos[0].jointtype = 1;
3358 vinfos[0].foffset = j0;
3359 vinfos[0].indices[0] = _ij0[0];
3360 vinfos[0].indices[1] = _ij0[1];
3361 vinfos[0].maxsolutions = _nj0;
3362 vinfos[1].jointtype = 1;
3363 vinfos[1].foffset = j1;
3364 vinfos[1].indices[0] = _ij1[0];
3365 vinfos[1].indices[1] = _ij1[1];
3366 vinfos[1].maxsolutions = _nj1;
3367 vinfos[2].jointtype = 1;
3368 vinfos[2].foffset = j2;
3369 vinfos[2].indices[0] = _ij2[0];
3370 vinfos[2].indices[1] = _ij2[1];
3371 vinfos[2].maxsolutions = _nj2;
3372 vinfos[3].jointtype = 1;
3373 vinfos[3].foffset = j3;
3374 vinfos[3].indices[0] = _ij3[0];
3375 vinfos[3].indices[1] = _ij3[1];
3376 vinfos[3].maxsolutions = _nj3;
3377 vinfos[4].jointtype = 1;
3378 vinfos[4].foffset = j4;
3379 vinfos[4].indices[0] = _ij4[0];
3380 vinfos[4].indices[1] = _ij4[1];
3381 vinfos[4].maxsolutions = _nj4;
3382 vinfos[5].jointtype = 1;
3383 vinfos[5].foffset = j5;
3384 vinfos[5].indices[0] = _ij5[0];
3385 vinfos[5].indices[1] = _ij5[1];
3386 vinfos[5].maxsolutions = _nj5;
3387 std::vector<int> vfree(0);
3395 if( bgotonextstatement )
3397 bool bgotonextstatement =
true;
3400 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
3401 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3403 bgotonextstatement=
false;
3411 if(
IKabs(j5eval[0]) < 0.0000000100000000 )
3417 IkReal op[2+1], zeror[2];
3422 polyroots2(op,zeror,numroots);
3423 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3424 int numsolutions = 0;
3425 for(
int ij5 = 0; ij5 < numroots; ++ij5)
3427 IkReal htj5 = zeror[ij5];
3428 tempj5array[0]=((2.0)*(
atan(htj5)));
3429 for(
int kj5 = 0; kj5 < 1; ++kj5)
3431 j5array[numsolutions] = tempj5array[kj5];
3432 if( j5array[numsolutions] >
IKPI )
3434 j5array[numsolutions]-=
IK2PI;
3436 else if( j5array[numsolutions] < -
IKPI )
3438 j5array[numsolutions]+=
IK2PI;
3440 sj5array[numsolutions] =
IKsin(j5array[numsolutions]);
3441 cj5array[numsolutions] =
IKcos(j5array[numsolutions]);
3445 bool j5valid[2]={
true,
true};
3447 for(
int ij5 = 0; ij5 < numsolutions; ++ij5)
3453 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3456 _ij5[0] = ij5; _ij5[1] = -1;
3457 for(
int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3461 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3465 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3466 vinfos[0].jointtype = 1;
3467 vinfos[0].foffset = j0;
3468 vinfos[0].indices[0] = _ij0[0];
3469 vinfos[0].indices[1] = _ij0[1];
3470 vinfos[0].maxsolutions = _nj0;
3471 vinfos[1].jointtype = 1;
3472 vinfos[1].foffset = j1;
3473 vinfos[1].indices[0] = _ij1[0];
3474 vinfos[1].indices[1] = _ij1[1];
3475 vinfos[1].maxsolutions = _nj1;
3476 vinfos[2].jointtype = 1;
3477 vinfos[2].foffset = j2;
3478 vinfos[2].indices[0] = _ij2[0];
3479 vinfos[2].indices[1] = _ij2[1];
3480 vinfos[2].maxsolutions = _nj2;
3481 vinfos[3].jointtype = 1;
3482 vinfos[3].foffset = j3;
3483 vinfos[3].indices[0] = _ij3[0];
3484 vinfos[3].indices[1] = _ij3[1];
3485 vinfos[3].maxsolutions = _nj3;
3486 vinfos[4].jointtype = 1;
3487 vinfos[4].foffset = j4;
3488 vinfos[4].indices[0] = _ij4[0];
3489 vinfos[4].indices[1] = _ij4[1];
3490 vinfos[4].maxsolutions = _nj4;
3491 vinfos[5].jointtype = 1;
3492 vinfos[5].foffset = j5;
3493 vinfos[5].indices[0] = _ij5[0];
3494 vinfos[5].indices[1] = _ij5[1];
3495 vinfos[5].maxsolutions = _nj5;
3496 std::vector<int> vfree(0);
3507 if( bgotonextstatement )
3509 bool bgotonextstatement =
true;
3514 bgotonextstatement=
false;
3519 if( bgotonextstatement )
3534 IkReal j5array[1], cj5array[1], sj5array[1];
3535 bool j5valid[1]={
false};
3541 IkReal x271=x272.
value;
3548 j5array[0]=
IKatan2(((-1.0)*new_r21*x271), (x271*(x273.
value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3549 sj5array[0]=
IKsin(j5array[0]);
3550 cj5array[0]=
IKcos(j5array[0]);
3551 if( j5array[0] >
IKPI )
3555 else if( j5array[0] < -
IKPI )
3556 { j5array[0]+=
IK2PI;
3559 for(
int ij5 = 0; ij5 < 1; ++ij5)
3565 _ij5[0] = ij5; _ij5[1] = -1;
3566 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3570 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3573 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3575 IkReal evalcond[12];
3576 IkReal x274=
IKsin(j5);
3577 IkReal x275=
IKcos(j5);
3578 IkReal x276=(cj3*new_r00);
3579 IkReal x277=(cj3*cj4);
3580 IkReal x278=(cj4*sj3);
3581 IkReal x279=((1.0)*cj3);
3582 IkReal x280=(sj3*x274);
3583 IkReal x281=((1.0)*x275);
3584 evalcond[0]=(new_r21+((sj4*x274)));
3585 evalcond[1]=((((-1.0)*sj4*x281))+new_r20);
3586 evalcond[2]=(((new_r00*sj3))+x274+(((-1.0)*new_r10*x279)));
3587 evalcond[3]=(((new_r01*sj3))+x275+(((-1.0)*new_r11*x279)));
3588 evalcond[4]=(((cj4*x274))+((new_r11*sj3))+((cj3*new_r01)));
3589 evalcond[5]=(((sj3*x275))+((x274*x277))+new_r01);
3590 evalcond[6]=(((new_r10*sj3))+x276+(((-1.0)*cj4*x281)));
3591 evalcond[7]=((((-1.0)*x277*x281))+x280+new_r00);
3592 evalcond[8]=(((x274*x278))+(((-1.0)*x275*x279))+new_r11);
3593 evalcond[9]=((((-1.0)*x274*x279))+new_r10+(((-1.0)*x278*x281)));
3594 evalcond[10]=(x274+((new_r01*x277))+((new_r21*sj4))+((new_r11*x278)));
3595 evalcond[11]=(((new_r20*sj4))+((cj4*x276))+(((-1.0)*x281))+((new_r10*x278)));
3603 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3604 vinfos[0].jointtype = 1;
3605 vinfos[0].foffset = j0;
3606 vinfos[0].indices[0] = _ij0[0];
3607 vinfos[0].indices[1] = _ij0[1];
3608 vinfos[0].maxsolutions = _nj0;
3609 vinfos[1].jointtype = 1;
3610 vinfos[1].foffset = j1;
3611 vinfos[1].indices[0] = _ij1[0];
3612 vinfos[1].indices[1] = _ij1[1];
3613 vinfos[1].maxsolutions = _nj1;
3614 vinfos[2].jointtype = 1;
3615 vinfos[2].foffset = j2;
3616 vinfos[2].indices[0] = _ij2[0];
3617 vinfos[2].indices[1] = _ij2[1];
3618 vinfos[2].maxsolutions = _nj2;
3619 vinfos[3].jointtype = 1;
3620 vinfos[3].foffset = j3;
3621 vinfos[3].indices[0] = _ij3[0];
3622 vinfos[3].indices[1] = _ij3[1];
3623 vinfos[3].maxsolutions = _nj3;
3624 vinfos[4].jointtype = 1;
3625 vinfos[4].foffset = j4;
3626 vinfos[4].indices[0] = _ij4[0];
3627 vinfos[4].indices[1] = _ij4[1];
3628 vinfos[4].maxsolutions = _nj4;
3629 vinfos[5].jointtype = 1;
3630 vinfos[5].foffset = j5;
3631 vinfos[5].indices[0] = _ij5[0];
3632 vinfos[5].indices[1] = _ij5[1];
3633 vinfos[5].maxsolutions = _nj5;
3634 std::vector<int> vfree(0);
3647 IkReal j5array[1], cj5array[1], sj5array[1];
3648 bool j5valid[1]={
false};
3654 IkReal x282=x283.
value;
3661 j5array[0]=
IKatan2(((-1.0)*new_r21*x282), (x282*(x284.
value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3662 sj5array[0]=
IKsin(j5array[0]);
3663 cj5array[0]=
IKcos(j5array[0]);
3664 if( j5array[0] >
IKPI )
3668 else if( j5array[0] < -
IKPI )
3669 { j5array[0]+=
IK2PI;
3672 for(
int ij5 = 0; ij5 < 1; ++ij5)
3678 _ij5[0] = ij5; _ij5[1] = -1;
3679 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3683 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3686 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3688 IkReal evalcond[12];
3689 IkReal x285=
IKsin(j5);
3690 IkReal x286=
IKcos(j5);
3691 IkReal x287=(cj3*new_r00);
3692 IkReal x288=(cj3*cj4);
3693 IkReal x289=(cj4*sj3);
3694 IkReal x290=((1.0)*cj3);
3695 IkReal x291=(sj3*x285);
3696 IkReal x292=((1.0)*x286);
3697 evalcond[0]=(new_r21+((sj4*x285)));
3698 evalcond[1]=(new_r20+(((-1.0)*sj4*x292)));
3699 evalcond[2]=((((-1.0)*new_r10*x290))+((new_r00*sj3))+x285);
3700 evalcond[3]=((((-1.0)*new_r11*x290))+((new_r01*sj3))+x286);
3701 evalcond[4]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x285)));
3702 evalcond[5]=(((x285*x288))+((sj3*x286))+new_r01);
3703 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x292))+x287);
3704 evalcond[7]=(x291+(((-1.0)*x288*x292))+new_r00);
3705 evalcond[8]=(((x285*x289))+(((-1.0)*x286*x290))+new_r11);
3706 evalcond[9]=((((-1.0)*x285*x290))+(((-1.0)*x289*x292))+new_r10);
3707 evalcond[10]=(x285+((new_r21*sj4))+((new_r11*x289))+((new_r01*x288)));
3708 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x292))+((cj4*x287))+((new_r10*x289)));
3716 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3717 vinfos[0].jointtype = 1;
3718 vinfos[0].foffset = j0;
3719 vinfos[0].indices[0] = _ij0[0];
3720 vinfos[0].indices[1] = _ij0[1];
3721 vinfos[0].maxsolutions = _nj0;
3722 vinfos[1].jointtype = 1;
3723 vinfos[1].foffset = j1;
3724 vinfos[1].indices[0] = _ij1[0];
3725 vinfos[1].indices[1] = _ij1[1];
3726 vinfos[1].maxsolutions = _nj1;
3727 vinfos[2].jointtype = 1;
3728 vinfos[2].foffset = j2;
3729 vinfos[2].indices[0] = _ij2[0];
3730 vinfos[2].indices[1] = _ij2[1];
3731 vinfos[2].maxsolutions = _nj2;
3732 vinfos[3].jointtype = 1;
3733 vinfos[3].foffset = j3;
3734 vinfos[3].indices[0] = _ij3[0];
3735 vinfos[3].indices[1] = _ij3[1];
3736 vinfos[3].maxsolutions = _nj3;
3737 vinfos[4].jointtype = 1;
3738 vinfos[4].foffset = j4;
3739 vinfos[4].indices[0] = _ij4[0];
3740 vinfos[4].indices[1] = _ij4[1];
3741 vinfos[4].maxsolutions = _nj4;
3742 vinfos[5].jointtype = 1;
3743 vinfos[5].foffset = j5;
3744 vinfos[5].indices[0] = _ij5[0];
3745 vinfos[5].indices[1] = _ij5[1];
3746 vinfos[5].maxsolutions = _nj5;
3747 std::vector<int> vfree(0);
3760 IkReal j5array[1], cj5array[1], sj5array[1];
3761 bool j5valid[1]={
false};
3771 j5array[0]=((-1.5707963267949)+(x293.
value)+(((1.5707963267949)*(x294.
value))));
3772 sj5array[0]=
IKsin(j5array[0]);
3773 cj5array[0]=
IKcos(j5array[0]);
3774 if( j5array[0] >
IKPI )
3778 else if( j5array[0] < -
IKPI )
3779 { j5array[0]+=
IK2PI;
3782 for(
int ij5 = 0; ij5 < 1; ++ij5)
3788 _ij5[0] = ij5; _ij5[1] = -1;
3789 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3793 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3796 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3798 IkReal evalcond[12];
3799 IkReal x295=
IKsin(j5);
3800 IkReal x296=
IKcos(j5);
3801 IkReal x297=(cj3*new_r00);
3802 IkReal x298=(cj3*cj4);
3803 IkReal x299=(cj4*sj3);
3804 IkReal x300=((1.0)*cj3);
3805 IkReal x301=(sj3*x295);
3806 IkReal x302=((1.0)*x296);
3807 evalcond[0]=(((sj4*x295))+new_r21);
3808 evalcond[1]=((((-1.0)*sj4*x302))+new_r20);
3809 evalcond[2]=(((new_r00*sj3))+x295+(((-1.0)*new_r10*x300)));
3810 evalcond[3]=(((new_r01*sj3))+x296+(((-1.0)*new_r11*x300)));
3811 evalcond[4]=(((new_r11*sj3))+((cj4*x295))+((cj3*new_r01)));
3812 evalcond[5]=(((x295*x298))+new_r01+((sj3*x296)));
3813 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x302))+x297);
3814 evalcond[7]=((((-1.0)*x298*x302))+x301+new_r00);
3815 evalcond[8]=((((-1.0)*x296*x300))+((x295*x299))+new_r11);
3816 evalcond[9]=((((-1.0)*x295*x300))+(((-1.0)*x299*x302))+new_r10);
3817 evalcond[10]=(((new_r11*x299))+x295+((new_r01*x298))+((new_r21*sj4)));
3818 evalcond[11]=(((new_r20*sj4))+((new_r10*x299))+((cj4*x297))+(((-1.0)*x302)));
3826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3827 vinfos[0].jointtype = 1;
3828 vinfos[0].foffset = j0;
3829 vinfos[0].indices[0] = _ij0[0];
3830 vinfos[0].indices[1] = _ij0[1];
3831 vinfos[0].maxsolutions = _nj0;
3832 vinfos[1].jointtype = 1;
3833 vinfos[1].foffset = j1;
3834 vinfos[1].indices[0] = _ij1[0];
3835 vinfos[1].indices[1] = _ij1[1];
3836 vinfos[1].maxsolutions = _nj1;
3837 vinfos[2].jointtype = 1;
3838 vinfos[2].foffset = j2;
3839 vinfos[2].indices[0] = _ij2[0];
3840 vinfos[2].indices[1] = _ij2[1];
3841 vinfos[2].maxsolutions = _nj2;
3842 vinfos[3].jointtype = 1;
3843 vinfos[3].foffset = j3;
3844 vinfos[3].indices[0] = _ij3[0];
3845 vinfos[3].indices[1] = _ij3[1];
3846 vinfos[3].maxsolutions = _nj3;
3847 vinfos[4].jointtype = 1;
3848 vinfos[4].foffset = j4;
3849 vinfos[4].indices[0] = _ij4[0];
3850 vinfos[4].indices[1] = _ij4[1];
3851 vinfos[4].maxsolutions = _nj4;
3852 vinfos[5].jointtype = 1;
3853 vinfos[5].foffset = j5;
3854 vinfos[5].indices[0] = _ij5[0];
3855 vinfos[5].indices[1] = _ij5[1];
3856 vinfos[5].maxsolutions = _nj5;
3857 std::vector<int> vfree(0);
3876 IkReal j3array[1], cj3array[1], sj3array[1];
3877 bool j3valid[1]={
false};
3887 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x303.
value)))+(x304.
value));
3888 sj3array[0]=
IKsin(j3array[0]);
3889 cj3array[0]=
IKcos(j3array[0]);
3890 if( j3array[0] >
IKPI )
3894 else if( j3array[0] < -
IKPI )
3895 { j3array[0]+=
IK2PI;
3898 for(
int ij3 = 0; ij3 < 1; ++ij3)
3904 _ij3[0] = ij3; _ij3[1] = -1;
3905 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
3909 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
3912 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3915 IkReal x305=
IKcos(j3);
3916 IkReal x306=
IKsin(j3);
3917 IkReal x307=((1.0)*cj4);
3918 IkReal x308=(new_r02*x305);
3919 IkReal x309=(sj4*x306);
3920 IkReal x310=(sj4*x305);
3921 IkReal x311=(new_r12*x306);
3922 evalcond[0]=(x310+new_r02);
3923 evalcond[1]=(x309+new_r12);
3924 evalcond[2]=((((-1.0)*new_r02*x306))+((new_r12*x305)));
3925 evalcond[3]=(sj4+x308+x311);
3926 evalcond[4]=(((cj4*x311))+((new_r22*sj4))+((cj4*x308)));
3927 evalcond[5]=(((new_r00*x310))+((new_r10*x309))+(((-1.0)*new_r20*x307)));
3928 evalcond[6]=(((new_r11*x309))+(((-1.0)*new_r21*x307))+((new_r01*x310)));
3929 evalcond[7]=((1.0)+(((-1.0)*new_r22*x307))+((new_r12*x309))+((sj4*x308)));
3940 j5eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
3941 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 ||
IKabs(j5eval[2]) < 0.0000010000000000 )
3947 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
3953 if(
IKabs(j5eval[0]) < 0.0000010000000000 ||
IKabs(j5eval[1]) < 0.0000010000000000 )
3957 bool bgotonextstatement =
true;
3960 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
3961 evalcond[1]=new_r02;
3962 evalcond[2]=new_r12;
3963 evalcond[3]=new_r21;
3964 evalcond[4]=new_r20;
3965 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 )
3967 bgotonextstatement=
false;
3969 IkReal j5array[1], cj5array[1], sj5array[1];
3970 bool j5valid[1]={
false};
3972 IkReal x312=((1.0)*new_r01);
3975 j5array[0]=
IKatan2(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312))), ((((-1.0)*sj3*x312))+((cj3*new_r00))));
3976 sj5array[0]=
IKsin(j5array[0]);
3977 cj5array[0]=
IKcos(j5array[0]);
3978 if( j5array[0] >
IKPI )
3982 else if( j5array[0] < -
IKPI )
3983 { j5array[0]+=
IK2PI;
3986 for(
int ij5 = 0; ij5 < 1; ++ij5)
3992 _ij5[0] = ij5; _ij5[1] = -1;
3993 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
3997 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4000 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4003 IkReal x313=
IKsin(j5);
4004 IkReal x314=
IKcos(j5);
4005 IkReal x315=((1.0)*cj3);
4006 IkReal x316=(sj3*x313);
4007 IkReal x317=((1.0)*x314);
4008 IkReal x318=(x314*x315);
4009 evalcond[0]=(((new_r11*sj3))+x313+((cj3*new_r01)));
4010 evalcond[1]=(((new_r00*sj3))+x313+(((-1.0)*new_r10*x315)));
4011 evalcond[2]=(((new_r01*sj3))+(((-1.0)*new_r11*x315))+x314);
4012 evalcond[3]=(((cj3*x313))+new_r01+((sj3*x314)));
4013 evalcond[4]=(((new_r10*sj3))+(((-1.0)*x317))+((cj3*new_r00)));
4014 evalcond[5]=(x316+(((-1.0)*x318))+new_r00);
4015 evalcond[6]=(x316+(((-1.0)*x318))+new_r11);
4016 evalcond[7]=((((-1.0)*x313*x315))+(((-1.0)*sj3*x317))+new_r10);
4024 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4025 vinfos[0].jointtype = 1;
4026 vinfos[0].foffset = j0;
4027 vinfos[0].indices[0] = _ij0[0];
4028 vinfos[0].indices[1] = _ij0[1];
4029 vinfos[0].maxsolutions = _nj0;
4030 vinfos[1].jointtype = 1;
4031 vinfos[1].foffset = j1;
4032 vinfos[1].indices[0] = _ij1[0];
4033 vinfos[1].indices[1] = _ij1[1];
4034 vinfos[1].maxsolutions = _nj1;
4035 vinfos[2].jointtype = 1;
4036 vinfos[2].foffset = j2;
4037 vinfos[2].indices[0] = _ij2[0];
4038 vinfos[2].indices[1] = _ij2[1];
4039 vinfos[2].maxsolutions = _nj2;
4040 vinfos[3].jointtype = 1;
4041 vinfos[3].foffset = j3;
4042 vinfos[3].indices[0] = _ij3[0];
4043 vinfos[3].indices[1] = _ij3[1];
4044 vinfos[3].maxsolutions = _nj3;
4045 vinfos[4].jointtype = 1;
4046 vinfos[4].foffset = j4;
4047 vinfos[4].indices[0] = _ij4[0];
4048 vinfos[4].indices[1] = _ij4[1];
4049 vinfos[4].maxsolutions = _nj4;
4050 vinfos[5].jointtype = 1;
4051 vinfos[5].foffset = j5;
4052 vinfos[5].indices[0] = _ij5[0];
4053 vinfos[5].indices[1] = _ij5[1];
4054 vinfos[5].maxsolutions = _nj5;
4055 std::vector<int> vfree(0);
4063 if( bgotonextstatement )
4065 bool bgotonextstatement =
true;
4068 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4069 evalcond[1]=new_r02;
4070 evalcond[2]=new_r12;
4071 evalcond[3]=new_r21;
4072 evalcond[4]=new_r20;
4073 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 )
4075 bgotonextstatement=
false;
4077 IkReal j5array[1], cj5array[1], sj5array[1];
4078 bool j5valid[1]={
false};
4080 IkReal x319=((1.0)*sj3);
4083 j5array[0]=
IKatan2(((((-1.0)*new_r00*x319))+((cj3*new_r01))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319))));
4084 sj5array[0]=
IKsin(j5array[0]);
4085 cj5array[0]=
IKcos(j5array[0]);
4086 if( j5array[0] >
IKPI )
4090 else if( j5array[0] < -
IKPI )
4091 { j5array[0]+=
IK2PI;
4094 for(
int ij5 = 0; ij5 < 1; ++ij5)
4100 _ij5[0] = ij5; _ij5[1] = -1;
4101 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4105 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4108 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4111 IkReal x320=
IKcos(j5);
4112 IkReal x321=
IKsin(j5);
4113 IkReal x322=((1.0)*cj3);
4114 IkReal x323=(sj3*x320);
4115 IkReal x324=((1.0)*x321);
4116 IkReal x325=(x321*x322);
4117 evalcond[0]=(((new_r10*sj3))+x320+((cj3*new_r00)));
4118 evalcond[1]=(((new_r00*sj3))+x321+(((-1.0)*new_r10*x322)));
4119 evalcond[2]=(((new_r01*sj3))+x320+(((-1.0)*new_r11*x322)));
4120 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x324))+((cj3*new_r01)));
4121 evalcond[4]=(((sj3*x321))+new_r00+((cj3*x320)));
4122 evalcond[5]=((((-1.0)*x325))+x323+new_r01);
4123 evalcond[6]=((((-1.0)*x325))+x323+new_r10);
4124 evalcond[7]=((((-1.0)*x320*x322))+new_r11+(((-1.0)*sj3*x324)));
4132 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4133 vinfos[0].jointtype = 1;
4134 vinfos[0].foffset = j0;
4135 vinfos[0].indices[0] = _ij0[0];
4136 vinfos[0].indices[1] = _ij0[1];
4137 vinfos[0].maxsolutions = _nj0;
4138 vinfos[1].jointtype = 1;
4139 vinfos[1].foffset = j1;
4140 vinfos[1].indices[0] = _ij1[0];
4141 vinfos[1].indices[1] = _ij1[1];
4142 vinfos[1].maxsolutions = _nj1;
4143 vinfos[2].jointtype = 1;
4144 vinfos[2].foffset = j2;
4145 vinfos[2].indices[0] = _ij2[0];
4146 vinfos[2].indices[1] = _ij2[1];
4147 vinfos[2].maxsolutions = _nj2;
4148 vinfos[3].jointtype = 1;
4149 vinfos[3].foffset = j3;
4150 vinfos[3].indices[0] = _ij3[0];
4151 vinfos[3].indices[1] = _ij3[1];
4152 vinfos[3].maxsolutions = _nj3;
4153 vinfos[4].jointtype = 1;
4154 vinfos[4].foffset = j4;
4155 vinfos[4].indices[0] = _ij4[0];
4156 vinfos[4].indices[1] = _ij4[1];
4157 vinfos[4].maxsolutions = _nj4;
4158 vinfos[5].jointtype = 1;
4159 vinfos[5].foffset = j5;
4160 vinfos[5].indices[0] = _ij5[0];
4161 vinfos[5].indices[1] = _ij5[1];
4162 vinfos[5].maxsolutions = _nj5;
4163 std::vector<int> vfree(0);
4171 if( bgotonextstatement )
4173 bool bgotonextstatement =
true;
4176 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4177 evalcond[1]=new_r02;
4178 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4180 bgotonextstatement=
false;
4182 IkReal j5array[1], cj5array[1], sj5array[1];
4183 bool j5valid[1]={
false};
4187 j5array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4188 sj5array[0]=
IKsin(j5array[0]);
4189 cj5array[0]=
IKcos(j5array[0]);
4190 if( j5array[0] >
IKPI )
4194 else if( j5array[0] < -
IKPI )
4195 { j5array[0]+=
IK2PI;
4198 for(
int ij5 = 0; ij5 < 1; ++ij5)
4204 _ij5[0] = ij5; _ij5[1] = -1;
4205 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4209 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4212 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4215 IkReal x326=
IKsin(j5);
4216 IkReal x327=
IKcos(j5);
4217 IkReal x328=((1.0)*x327);
4218 evalcond[0]=(x326+new_r00);
4219 evalcond[1]=(x327+new_r01);
4220 evalcond[2]=(((sj4*x326))+new_r21);
4221 evalcond[3]=(((cj4*x326))+new_r11);
4222 evalcond[4]=(new_r20+(((-1.0)*sj4*x328)));
4223 evalcond[5]=(new_r10+(((-1.0)*cj4*x328)));
4224 evalcond[6]=(((cj4*new_r11))+x326+((new_r21*sj4)));
4225 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x328)));
4233 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4234 vinfos[0].jointtype = 1;
4235 vinfos[0].foffset = j0;
4236 vinfos[0].indices[0] = _ij0[0];
4237 vinfos[0].indices[1] = _ij0[1];
4238 vinfos[0].maxsolutions = _nj0;
4239 vinfos[1].jointtype = 1;
4240 vinfos[1].foffset = j1;
4241 vinfos[1].indices[0] = _ij1[0];
4242 vinfos[1].indices[1] = _ij1[1];
4243 vinfos[1].maxsolutions = _nj1;
4244 vinfos[2].jointtype = 1;
4245 vinfos[2].foffset = j2;
4246 vinfos[2].indices[0] = _ij2[0];
4247 vinfos[2].indices[1] = _ij2[1];
4248 vinfos[2].maxsolutions = _nj2;
4249 vinfos[3].jointtype = 1;
4250 vinfos[3].foffset = j3;
4251 vinfos[3].indices[0] = _ij3[0];
4252 vinfos[3].indices[1] = _ij3[1];
4253 vinfos[3].maxsolutions = _nj3;
4254 vinfos[4].jointtype = 1;
4255 vinfos[4].foffset = j4;
4256 vinfos[4].indices[0] = _ij4[0];
4257 vinfos[4].indices[1] = _ij4[1];
4258 vinfos[4].maxsolutions = _nj4;
4259 vinfos[5].jointtype = 1;
4260 vinfos[5].foffset = j5;
4261 vinfos[5].indices[0] = _ij5[0];
4262 vinfos[5].indices[1] = _ij5[1];
4263 vinfos[5].maxsolutions = _nj5;
4264 std::vector<int> vfree(0);
4272 if( bgotonextstatement )
4274 bool bgotonextstatement =
true;
4277 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4278 evalcond[1]=new_r02;
4279 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4281 bgotonextstatement=
false;
4283 IkReal j5array[1], cj5array[1], sj5array[1];
4284 bool j5valid[1]={
false};
4288 j5array[0]=
IKatan2(new_r00, new_r01);
4289 sj5array[0]=
IKsin(j5array[0]);
4290 cj5array[0]=
IKcos(j5array[0]);
4291 if( j5array[0] >
IKPI )
4295 else if( j5array[0] < -
IKPI )
4296 { j5array[0]+=
IK2PI;
4299 for(
int ij5 = 0; ij5 < 1; ++ij5)
4305 _ij5[0] = ij5; _ij5[1] = -1;
4306 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4310 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4313 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4316 IkReal x329=
IKsin(j5);
4317 IkReal x330=
IKcos(j5);
4318 IkReal x331=((1.0)*cj4);
4319 IkReal x332=((1.0)*x330);
4320 evalcond[0]=(((sj4*x329))+new_r21);
4321 evalcond[1]=(x329+(((-1.0)*new_r00)));
4322 evalcond[2]=(x330+(((-1.0)*new_r01)));
4323 evalcond[3]=((((-1.0)*sj4*x332))+new_r20);
4324 evalcond[4]=(((cj4*x329))+(((-1.0)*new_r11)));
4325 evalcond[5]=((((-1.0)*x330*x331))+(((-1.0)*new_r10)));
4326 evalcond[6]=((((-1.0)*new_r11*x331))+x329+((new_r21*sj4)));
4327 evalcond[7]=((((-1.0)*new_r10*x331))+((new_r20*sj4))+(((-1.0)*x332)));
4335 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4336 vinfos[0].jointtype = 1;
4337 vinfos[0].foffset = j0;
4338 vinfos[0].indices[0] = _ij0[0];
4339 vinfos[0].indices[1] = _ij0[1];
4340 vinfos[0].maxsolutions = _nj0;
4341 vinfos[1].jointtype = 1;
4342 vinfos[1].foffset = j1;
4343 vinfos[1].indices[0] = _ij1[0];
4344 vinfos[1].indices[1] = _ij1[1];
4345 vinfos[1].maxsolutions = _nj1;
4346 vinfos[2].jointtype = 1;
4347 vinfos[2].foffset = j2;
4348 vinfos[2].indices[0] = _ij2[0];
4349 vinfos[2].indices[1] = _ij2[1];
4350 vinfos[2].maxsolutions = _nj2;
4351 vinfos[3].jointtype = 1;
4352 vinfos[3].foffset = j3;
4353 vinfos[3].indices[0] = _ij3[0];
4354 vinfos[3].indices[1] = _ij3[1];
4355 vinfos[3].maxsolutions = _nj3;
4356 vinfos[4].jointtype = 1;
4357 vinfos[4].foffset = j4;
4358 vinfos[4].indices[0] = _ij4[0];
4359 vinfos[4].indices[1] = _ij4[1];
4360 vinfos[4].maxsolutions = _nj4;
4361 vinfos[5].jointtype = 1;
4362 vinfos[5].foffset = j5;
4363 vinfos[5].indices[0] = _ij5[0];
4364 vinfos[5].indices[1] = _ij5[1];
4365 vinfos[5].maxsolutions = _nj5;
4366 std::vector<int> vfree(0);
4374 if( bgotonextstatement )
4376 bool bgotonextstatement =
true;
4379 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j3))), 6.28318530717959)));
4380 evalcond[1]=new_r12;
4381 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4383 bgotonextstatement=
false;
4385 IkReal j5array[1], cj5array[1], sj5array[1];
4386 bool j5valid[1]={
false};
4390 j5array[0]=
IKatan2(new_r10, new_r11);
4391 sj5array[0]=
IKsin(j5array[0]);
4392 cj5array[0]=
IKcos(j5array[0]);
4393 if( j5array[0] >
IKPI )
4397 else if( j5array[0] < -
IKPI )
4398 { j5array[0]+=
IK2PI;
4401 for(
int ij5 = 0; ij5 < 1; ++ij5)
4407 _ij5[0] = ij5; _ij5[1] = -1;
4408 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4412 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4415 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4418 IkReal x333=
IKcos(j5);
4419 IkReal x334=
IKsin(j5);
4420 IkReal x335=((1.0)*x333);
4421 evalcond[0]=(((new_r02*x333))+new_r20);
4422 evalcond[1]=(x334+(((-1.0)*new_r10)));
4423 evalcond[2]=(x333+(((-1.0)*new_r11)));
4424 evalcond[3]=(((cj4*x334))+new_r01);
4425 evalcond[4]=((((-1.0)*new_r02*x334))+new_r21);
4426 evalcond[5]=((((-1.0)*cj4*x335))+new_r00);
4427 evalcond[6]=(((cj4*new_r01))+x334+((new_r21*sj4)));
4428 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x335)));
4436 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4437 vinfos[0].jointtype = 1;
4438 vinfos[0].foffset = j0;
4439 vinfos[0].indices[0] = _ij0[0];
4440 vinfos[0].indices[1] = _ij0[1];
4441 vinfos[0].maxsolutions = _nj0;
4442 vinfos[1].jointtype = 1;
4443 vinfos[1].foffset = j1;
4444 vinfos[1].indices[0] = _ij1[0];
4445 vinfos[1].indices[1] = _ij1[1];
4446 vinfos[1].maxsolutions = _nj1;
4447 vinfos[2].jointtype = 1;
4448 vinfos[2].foffset = j2;
4449 vinfos[2].indices[0] = _ij2[0];
4450 vinfos[2].indices[1] = _ij2[1];
4451 vinfos[2].maxsolutions = _nj2;
4452 vinfos[3].jointtype = 1;
4453 vinfos[3].foffset = j3;
4454 vinfos[3].indices[0] = _ij3[0];
4455 vinfos[3].indices[1] = _ij3[1];
4456 vinfos[3].maxsolutions = _nj3;
4457 vinfos[4].jointtype = 1;
4458 vinfos[4].foffset = j4;
4459 vinfos[4].indices[0] = _ij4[0];
4460 vinfos[4].indices[1] = _ij4[1];
4461 vinfos[4].maxsolutions = _nj4;
4462 vinfos[5].jointtype = 1;
4463 vinfos[5].foffset = j5;
4464 vinfos[5].indices[0] = _ij5[0];
4465 vinfos[5].indices[1] = _ij5[1];
4466 vinfos[5].maxsolutions = _nj5;
4467 std::vector<int> vfree(0);
4475 if( bgotonextstatement )
4477 bool bgotonextstatement =
true;
4480 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4481 evalcond[1]=new_r12;
4482 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
4484 bgotonextstatement=
false;
4486 IkReal j5array[1], cj5array[1], sj5array[1];
4487 bool j5valid[1]={
false};
4491 j5array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4492 sj5array[0]=
IKsin(j5array[0]);
4493 cj5array[0]=
IKcos(j5array[0]);
4494 if( j5array[0] >
IKPI )
4498 else if( j5array[0] < -
IKPI )
4499 { j5array[0]+=
IK2PI;
4502 for(
int ij5 = 0; ij5 < 1; ++ij5)
4508 _ij5[0] = ij5; _ij5[1] = -1;
4509 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4513 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4516 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4519 IkReal x336=
IKsin(j5);
4520 IkReal x337=
IKcos(j5);
4521 IkReal x338=((1.0)*new_r00);
4522 IkReal x339=((1.0)*new_r01);
4523 IkReal x340=((1.0)*x337);
4524 evalcond[0]=(x336+new_r10);
4525 evalcond[1]=(x337+new_r11);
4526 evalcond[2]=(((new_r02*x336))+new_r21);
4527 evalcond[3]=((((-1.0)*new_r02*x340))+new_r20);
4528 evalcond[4]=((((-1.0)*x339))+((cj4*x336)));
4529 evalcond[5]=((((-1.0)*cj4*x340))+(((-1.0)*x338)));
4530 evalcond[6]=((((-1.0)*cj4*x339))+x336+((new_r21*sj4)));
4531 evalcond[7]=(((new_r20*sj4))+(((-1.0)*cj4*x338))+(((-1.0)*x340)));
4539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4540 vinfos[0].jointtype = 1;
4541 vinfos[0].foffset = j0;
4542 vinfos[0].indices[0] = _ij0[0];
4543 vinfos[0].indices[1] = _ij0[1];
4544 vinfos[0].maxsolutions = _nj0;
4545 vinfos[1].jointtype = 1;
4546 vinfos[1].foffset = j1;
4547 vinfos[1].indices[0] = _ij1[0];
4548 vinfos[1].indices[1] = _ij1[1];
4549 vinfos[1].maxsolutions = _nj1;
4550 vinfos[2].jointtype = 1;
4551 vinfos[2].foffset = j2;
4552 vinfos[2].indices[0] = _ij2[0];
4553 vinfos[2].indices[1] = _ij2[1];
4554 vinfos[2].maxsolutions = _nj2;
4555 vinfos[3].jointtype = 1;
4556 vinfos[3].foffset = j3;
4557 vinfos[3].indices[0] = _ij3[0];
4558 vinfos[3].indices[1] = _ij3[1];
4559 vinfos[3].maxsolutions = _nj3;
4560 vinfos[4].jointtype = 1;
4561 vinfos[4].foffset = j4;
4562 vinfos[4].indices[0] = _ij4[0];
4563 vinfos[4].indices[1] = _ij4[1];
4564 vinfos[4].maxsolutions = _nj4;
4565 vinfos[5].jointtype = 1;
4566 vinfos[5].foffset = j5;
4567 vinfos[5].indices[0] = _ij5[0];
4568 vinfos[5].indices[1] = _ij5[1];
4569 vinfos[5].maxsolutions = _nj5;
4570 std::vector<int> vfree(0);
4578 if( bgotonextstatement )
4580 bool bgotonextstatement =
true;
4583 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
4584 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4586 bgotonextstatement=
false;
4594 if(
IKabs(j5eval[0]) < 0.0000000100000000 )
4600 IkReal op[2+1], zeror[2];
4605 polyroots2(op,zeror,numroots);
4606 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4607 int numsolutions = 0;
4608 for(
int ij5 = 0; ij5 < numroots; ++ij5)
4610 IkReal htj5 = zeror[ij5];
4611 tempj5array[0]=((2.0)*(
atan(htj5)));
4612 for(
int kj5 = 0; kj5 < 1; ++kj5)
4614 j5array[numsolutions] = tempj5array[kj5];
4615 if( j5array[numsolutions] >
IKPI )
4617 j5array[numsolutions]-=
IK2PI;
4619 else if( j5array[numsolutions] < -
IKPI )
4621 j5array[numsolutions]+=
IK2PI;
4623 sj5array[numsolutions] =
IKsin(j5array[numsolutions]);
4624 cj5array[numsolutions] =
IKcos(j5array[numsolutions]);
4628 bool j5valid[2]={
true,
true};
4630 for(
int ij5 = 0; ij5 < numsolutions; ++ij5)
4636 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4639 _ij5[0] = ij5; _ij5[1] = -1;
4640 for(
int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4644 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4648 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4649 vinfos[0].jointtype = 1;
4650 vinfos[0].foffset = j0;
4651 vinfos[0].indices[0] = _ij0[0];
4652 vinfos[0].indices[1] = _ij0[1];
4653 vinfos[0].maxsolutions = _nj0;
4654 vinfos[1].jointtype = 1;
4655 vinfos[1].foffset = j1;
4656 vinfos[1].indices[0] = _ij1[0];
4657 vinfos[1].indices[1] = _ij1[1];
4658 vinfos[1].maxsolutions = _nj1;
4659 vinfos[2].jointtype = 1;
4660 vinfos[2].foffset = j2;
4661 vinfos[2].indices[0] = _ij2[0];
4662 vinfos[2].indices[1] = _ij2[1];
4663 vinfos[2].maxsolutions = _nj2;
4664 vinfos[3].jointtype = 1;
4665 vinfos[3].foffset = j3;
4666 vinfos[3].indices[0] = _ij3[0];
4667 vinfos[3].indices[1] = _ij3[1];
4668 vinfos[3].maxsolutions = _nj3;
4669 vinfos[4].jointtype = 1;
4670 vinfos[4].foffset = j4;
4671 vinfos[4].indices[0] = _ij4[0];
4672 vinfos[4].indices[1] = _ij4[1];
4673 vinfos[4].maxsolutions = _nj4;
4674 vinfos[5].jointtype = 1;
4675 vinfos[5].foffset = j5;
4676 vinfos[5].indices[0] = _ij5[0];
4677 vinfos[5].indices[1] = _ij5[1];
4678 vinfos[5].maxsolutions = _nj5;
4679 std::vector<int> vfree(0);
4690 if( bgotonextstatement )
4692 bool bgotonextstatement =
true;
4697 bgotonextstatement=
false;
4702 if( bgotonextstatement )
4717 IkReal j5array[1], cj5array[1], sj5array[1];
4718 bool j5valid[1]={
false};
4724 IkReal x341=x342.
value;
4731 j5array[0]=
IKatan2(((-1.0)*new_r21*x341), (x341*(x343.
value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4732 sj5array[0]=
IKsin(j5array[0]);
4733 cj5array[0]=
IKcos(j5array[0]);
4734 if( j5array[0] >
IKPI )
4738 else if( j5array[0] < -
IKPI )
4739 { j5array[0]+=
IK2PI;
4742 for(
int ij5 = 0; ij5 < 1; ++ij5)
4748 _ij5[0] = ij5; _ij5[1] = -1;
4749 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4753 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4758 IkReal evalcond[12];
4759 IkReal x344=
IKsin(j5);
4760 IkReal x345=
IKcos(j5);
4761 IkReal x346=(cj3*new_r00);
4762 IkReal x347=(cj3*cj4);
4763 IkReal x348=(cj4*sj3);
4764 IkReal x349=((1.0)*cj3);
4765 IkReal x350=(sj3*x344);
4766 IkReal x351=((1.0)*x345);
4767 evalcond[0]=(((sj4*x344))+new_r21);
4768 evalcond[1]=(new_r20+(((-1.0)*sj4*x351)));
4769 evalcond[2]=((((-1.0)*new_r10*x349))+((new_r00*sj3))+x344);
4770 evalcond[3]=((((-1.0)*new_r11*x349))+((new_r01*sj3))+x345);
4771 evalcond[4]=(((new_r11*sj3))+((cj4*x344))+((cj3*new_r01)));
4772 evalcond[5]=(((sj3*x345))+((x344*x347))+new_r01);
4773 evalcond[6]=(((new_r10*sj3))+x346+(((-1.0)*cj4*x351)));
4774 evalcond[7]=(x350+(((-1.0)*x347*x351))+new_r00);
4775 evalcond[8]=(((x344*x348))+(((-1.0)*x345*x349))+new_r11);
4776 evalcond[9]=((((-1.0)*x348*x351))+(((-1.0)*x344*x349))+new_r10);
4777 evalcond[10]=(((new_r11*x348))+((new_r01*x347))+x344+((new_r21*sj4)));
4778 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x351))+((new_r10*x348))+((cj4*x346)));
4786 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4787 vinfos[0].jointtype = 1;
4788 vinfos[0].foffset = j0;
4789 vinfos[0].indices[0] = _ij0[0];
4790 vinfos[0].indices[1] = _ij0[1];
4791 vinfos[0].maxsolutions = _nj0;
4792 vinfos[1].jointtype = 1;
4793 vinfos[1].foffset = j1;
4794 vinfos[1].indices[0] = _ij1[0];
4795 vinfos[1].indices[1] = _ij1[1];
4796 vinfos[1].maxsolutions = _nj1;
4797 vinfos[2].jointtype = 1;
4798 vinfos[2].foffset = j2;
4799 vinfos[2].indices[0] = _ij2[0];
4800 vinfos[2].indices[1] = _ij2[1];
4801 vinfos[2].maxsolutions = _nj2;
4802 vinfos[3].jointtype = 1;
4803 vinfos[3].foffset = j3;
4804 vinfos[3].indices[0] = _ij3[0];
4805 vinfos[3].indices[1] = _ij3[1];
4806 vinfos[3].maxsolutions = _nj3;
4807 vinfos[4].jointtype = 1;
4808 vinfos[4].foffset = j4;
4809 vinfos[4].indices[0] = _ij4[0];
4810 vinfos[4].indices[1] = _ij4[1];
4811 vinfos[4].maxsolutions = _nj4;
4812 vinfos[5].jointtype = 1;
4813 vinfos[5].foffset = j5;
4814 vinfos[5].indices[0] = _ij5[0];
4815 vinfos[5].indices[1] = _ij5[1];
4816 vinfos[5].maxsolutions = _nj5;
4817 std::vector<int> vfree(0);
4830 IkReal j5array[1], cj5array[1], sj5array[1];
4831 bool j5valid[1]={
false};
4837 IkReal x352=x353.
value;
4844 j5array[0]=
IKatan2(((-1.0)*new_r21*x352), (x352*(x354.
value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4845 sj5array[0]=
IKsin(j5array[0]);
4846 cj5array[0]=
IKcos(j5array[0]);
4847 if( j5array[0] >
IKPI )
4851 else if( j5array[0] < -
IKPI )
4852 { j5array[0]+=
IK2PI;
4855 for(
int ij5 = 0; ij5 < 1; ++ij5)
4861 _ij5[0] = ij5; _ij5[1] = -1;
4862 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4866 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4869 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4871 IkReal evalcond[12];
4872 IkReal x355=
IKsin(j5);
4873 IkReal x356=
IKcos(j5);
4874 IkReal x357=(cj3*new_r00);
4875 IkReal x358=(cj3*cj4);
4876 IkReal x359=(cj4*sj3);
4877 IkReal x360=((1.0)*cj3);
4878 IkReal x361=(sj3*x355);
4879 IkReal x362=((1.0)*x356);
4880 evalcond[0]=(((sj4*x355))+new_r21);
4881 evalcond[1]=((((-1.0)*sj4*x362))+new_r20);
4882 evalcond[2]=(((new_r00*sj3))+x355+(((-1.0)*new_r10*x360)));
4883 evalcond[3]=(((new_r01*sj3))+x356+(((-1.0)*new_r11*x360)));
4884 evalcond[4]=(((new_r11*sj3))+((cj4*x355))+((cj3*new_r01)));
4885 evalcond[5]=(((x355*x358))+((sj3*x356))+new_r01);
4886 evalcond[6]=((((-1.0)*cj4*x362))+((new_r10*sj3))+x357);
4887 evalcond[7]=((((-1.0)*x358*x362))+x361+new_r00);
4888 evalcond[8]=(((x355*x359))+(((-1.0)*x356*x360))+new_r11);
4889 evalcond[9]=((((-1.0)*x355*x360))+(((-1.0)*x359*x362))+new_r10);
4890 evalcond[10]=(((new_r01*x358))+x355+((new_r21*sj4))+((new_r11*x359)));
4891 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x362))+((cj4*x357))+((new_r10*x359)));
4899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4900 vinfos[0].jointtype = 1;
4901 vinfos[0].foffset = j0;
4902 vinfos[0].indices[0] = _ij0[0];
4903 vinfos[0].indices[1] = _ij0[1];
4904 vinfos[0].maxsolutions = _nj0;
4905 vinfos[1].jointtype = 1;
4906 vinfos[1].foffset = j1;
4907 vinfos[1].indices[0] = _ij1[0];
4908 vinfos[1].indices[1] = _ij1[1];
4909 vinfos[1].maxsolutions = _nj1;
4910 vinfos[2].jointtype = 1;
4911 vinfos[2].foffset = j2;
4912 vinfos[2].indices[0] = _ij2[0];
4913 vinfos[2].indices[1] = _ij2[1];
4914 vinfos[2].maxsolutions = _nj2;
4915 vinfos[3].jointtype = 1;
4916 vinfos[3].foffset = j3;
4917 vinfos[3].indices[0] = _ij3[0];
4918 vinfos[3].indices[1] = _ij3[1];
4919 vinfos[3].maxsolutions = _nj3;
4920 vinfos[4].jointtype = 1;
4921 vinfos[4].foffset = j4;
4922 vinfos[4].indices[0] = _ij4[0];
4923 vinfos[4].indices[1] = _ij4[1];
4924 vinfos[4].maxsolutions = _nj4;
4925 vinfos[5].jointtype = 1;
4926 vinfos[5].foffset = j5;
4927 vinfos[5].indices[0] = _ij5[0];
4928 vinfos[5].indices[1] = _ij5[1];
4929 vinfos[5].maxsolutions = _nj5;
4930 std::vector<int> vfree(0);
4943 IkReal j5array[1], cj5array[1], sj5array[1];
4944 bool j5valid[1]={
false};
4954 j5array[0]=((-1.5707963267949)+(x363.
value)+(((1.5707963267949)*(x364.
value))));
4955 sj5array[0]=
IKsin(j5array[0]);
4956 cj5array[0]=
IKcos(j5array[0]);
4957 if( j5array[0] >
IKPI )
4961 else if( j5array[0] < -
IKPI )
4962 { j5array[0]+=
IK2PI;
4965 for(
int ij5 = 0; ij5 < 1; ++ij5)
4971 _ij5[0] = ij5; _ij5[1] = -1;
4972 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
4976 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4979 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4981 IkReal evalcond[12];
4982 IkReal x365=
IKsin(j5);
4983 IkReal x366=
IKcos(j5);
4984 IkReal x367=(cj3*new_r00);
4985 IkReal x368=(cj3*cj4);
4986 IkReal x369=(cj4*sj3);
4987 IkReal x370=((1.0)*cj3);
4988 IkReal x371=(sj3*x365);
4989 IkReal x372=((1.0)*x366);
4990 evalcond[0]=(((sj4*x365))+new_r21);
4991 evalcond[1]=((((-1.0)*sj4*x372))+new_r20);
4992 evalcond[2]=(((new_r00*sj3))+x365+(((-1.0)*new_r10*x370)));
4993 evalcond[3]=(((new_r01*sj3))+x366+(((-1.0)*new_r11*x370)));
4994 evalcond[4]=(((new_r11*sj3))+((cj4*x365))+((cj3*new_r01)));
4995 evalcond[5]=(((x365*x368))+((sj3*x366))+new_r01);
4996 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x372))+x367);
4997 evalcond[7]=((((-1.0)*x368*x372))+x371+new_r00);
4998 evalcond[8]=(((x365*x369))+(((-1.0)*x366*x370))+new_r11);
4999 evalcond[9]=((((-1.0)*x369*x372))+new_r10+(((-1.0)*x365*x370)));
5000 evalcond[10]=(x365+((new_r01*x368))+((new_r21*sj4))+((new_r11*x369)));
5001 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x372))+((cj4*x367))+((new_r10*x369)));
5009 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5010 vinfos[0].jointtype = 1;
5011 vinfos[0].foffset = j0;
5012 vinfos[0].indices[0] = _ij0[0];
5013 vinfos[0].indices[1] = _ij0[1];
5014 vinfos[0].maxsolutions = _nj0;
5015 vinfos[1].jointtype = 1;
5016 vinfos[1].foffset = j1;
5017 vinfos[1].indices[0] = _ij1[0];
5018 vinfos[1].indices[1] = _ij1[1];
5019 vinfos[1].maxsolutions = _nj1;
5020 vinfos[2].jointtype = 1;
5021 vinfos[2].foffset = j2;
5022 vinfos[2].indices[0] = _ij2[0];
5023 vinfos[2].indices[1] = _ij2[1];
5024 vinfos[2].maxsolutions = _nj2;
5025 vinfos[3].jointtype = 1;
5026 vinfos[3].foffset = j3;
5027 vinfos[3].indices[0] = _ij3[0];
5028 vinfos[3].indices[1] = _ij3[1];
5029 vinfos[3].maxsolutions = _nj3;
5030 vinfos[4].jointtype = 1;
5031 vinfos[4].foffset = j4;
5032 vinfos[4].indices[0] = _ij4[0];
5033 vinfos[4].indices[1] = _ij4[1];
5034 vinfos[4].maxsolutions = _nj4;
5035 vinfos[5].jointtype = 1;
5036 vinfos[5].foffset = j5;
5037 vinfos[5].indices[0] = _ij5[0];
5038 vinfos[5].indices[1] = _ij5[1];
5039 vinfos[5].maxsolutions = _nj5;
5040 std::vector<int> vfree(0);
5059 IkReal j5array[1], cj5array[1], sj5array[1];
5060 bool j5valid[1]={
false};
5070 j5array[0]=((-1.5707963267949)+(x373.
value)+(((1.5707963267949)*(x374.
value))));
5071 sj5array[0]=
IKsin(j5array[0]);
5072 cj5array[0]=
IKcos(j5array[0]);
5073 if( j5array[0] >
IKPI )
5077 else if( j5array[0] < -
IKPI )
5078 { j5array[0]+=
IK2PI;
5081 for(
int ij5 = 0; ij5 < 1; ++ij5)
5087 _ij5[0] = ij5; _ij5[1] = -1;
5088 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
5092 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
5095 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5098 evalcond[0]=(((sj4*(
IKsin(j5))))+new_r21);
5099 evalcond[1]=((((-1.0)*sj4*(
IKcos(j5))))+new_r20);
5110 j3eval[2]=((
IKabs(new_r12))+(
IKabs(new_r02)));
5111 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5117 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
5121 bool bgotonextstatement =
true;
5124 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
5125 evalcond[1]=new_r02;
5126 evalcond[2]=new_r12;
5127 evalcond[3]=new_r21;
5128 evalcond[4]=new_r20;
5129 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 )
5131 bgotonextstatement=
false;
5137 IkReal x375=((1.0)*new_r11);
5138 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5140 j3eval[1]=((
IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(
IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5142 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5149 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5151 j3eval[1]=((
IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(
IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5153 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5160 IkReal x378=((1.0)*new_r11);
5161 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5164 j3eval[2]=((
IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(
IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5165 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5169 bool bgotonextstatement =
true;
5172 IkReal x380=((-1.0)*new_r01);
5173 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5177 IkReal x381=
pow(x382,-0.5);
5182 IkReal gconst6=((-1.0)*(x383.
value));
5183 IkReal gconst7=(new_r11*x381);
5184 IkReal gconst8=(x380*x381);
5189 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((x384.
value)+j5)))), 6.28318530717959)));
5190 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5192 bgotonextstatement=
false;
5195 IkReal x385=((-1.0)*new_r01);
5200 IkReal x386=((-1.0)*(x388.
value));
5208 IkReal gconst6=x386;
5209 IkReal gconst7=(new_r11*x387);
5210 IkReal gconst8=(x385*x387);
5211 IkReal x389=new_r11*new_r11;
5212 IkReal x390=(new_r10*new_r11);
5213 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5215 IkReal x393=(new_r11*x392);
5217 j3eval[1]=((
IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(
IKabs((((new_r01*x393))+((x390*x392))))));
5219 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5223 IkReal x394=((-1.0)*new_r01);
5228 IkReal x395=((-1.0)*(x397.
value));
5236 IkReal gconst6=x395;
5237 IkReal gconst7=(new_r11*x396);
5238 IkReal gconst8=(x394*x396);
5239 IkReal x398=new_r11*new_r11;
5244 IkReal x399=x401.
value;
5245 IkReal x400=(x398*x399);
5246 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))))));
5247 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5251 IkReal x402=((-1.0)*new_r01);
5256 IkReal x403=((-1.0)*(x405.
value));
5264 IkReal gconst6=x403;
5265 IkReal gconst7=(new_r11*x404);
5266 IkReal gconst8=(x402*x404);
5267 IkReal x406=new_r01*new_r01;
5268 IkReal x407=new_r11*new_r11;
5273 IkReal x408=x414.
value;
5274 IkReal x409=(x407*x408);
5279 IkReal x410=x415.
value;
5280 IkReal x411=((1.0)*x410);
5281 IkReal x412=(new_r11*x411);
5282 IkReal x413=(new_r01*x411);
5283 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))))));
5284 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5288 bool bgotonextstatement =
true;
5291 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
5292 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5294 bgotonextstatement=
false;
5296 IkReal j3array[2], cj3array[2], sj3array[2];
5297 bool j3valid[2]={
false};
5303 sj3array[0]=(new_r10*(x416.
value));
5306 j3valid[0] = j3valid[1] =
true;
5307 j3array[0] =
IKasin(sj3array[0]);
5308 cj3array[0] =
IKcos(j3array[0]);
5309 sj3array[1] = sj3array[0];
5310 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
5311 cj3array[1] = -cj3array[0];
5313 else if( isnan(sj3array[0]) )
5317 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5319 for(
int ij3 = 0; ij3 < 2; ++ij3)
5325 _ij3[0] = ij3; _ij3[1] = -1;
5326 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
5330 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5333 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5336 IkReal x417=
IKcos(j3);
5337 IkReal x418=
IKsin(j3);
5338 IkReal x419=((-1.0)*x417);
5339 evalcond[0]=(new_r01*x417);
5340 evalcond[1]=(new_r10*x419);
5341 evalcond[2]=(gconst8*x419);
5342 evalcond[3]=(gconst8+((new_r01*x418)));
5343 evalcond[4]=(((gconst8*x418))+new_r01);
5344 evalcond[5]=((((-1.0)*gconst8))+((new_r10*x418)));
5352 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5353 vinfos[0].jointtype = 1;
5354 vinfos[0].foffset = j0;
5355 vinfos[0].indices[0] = _ij0[0];
5356 vinfos[0].indices[1] = _ij0[1];
5357 vinfos[0].maxsolutions = _nj0;
5358 vinfos[1].jointtype = 1;
5359 vinfos[1].foffset = j1;
5360 vinfos[1].indices[0] = _ij1[0];
5361 vinfos[1].indices[1] = _ij1[1];
5362 vinfos[1].maxsolutions = _nj1;
5363 vinfos[2].jointtype = 1;
5364 vinfos[2].foffset = j2;
5365 vinfos[2].indices[0] = _ij2[0];
5366 vinfos[2].indices[1] = _ij2[1];
5367 vinfos[2].maxsolutions = _nj2;
5368 vinfos[3].jointtype = 1;
5369 vinfos[3].foffset = j3;
5370 vinfos[3].indices[0] = _ij3[0];
5371 vinfos[3].indices[1] = _ij3[1];
5372 vinfos[3].maxsolutions = _nj3;
5373 vinfos[4].jointtype = 1;
5374 vinfos[4].foffset = j4;
5375 vinfos[4].indices[0] = _ij4[0];
5376 vinfos[4].indices[1] = _ij4[1];
5377 vinfos[4].maxsolutions = _nj4;
5378 vinfos[5].jointtype = 1;
5379 vinfos[5].foffset = j5;
5380 vinfos[5].indices[0] = _ij5[0];
5381 vinfos[5].indices[1] = _ij5[1];
5382 vinfos[5].maxsolutions = _nj5;
5383 std::vector<int> vfree(0);
5391 if( bgotonextstatement )
5393 bool bgotonextstatement =
true;
5396 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
5397 evalcond[1]=gconst7;
5398 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
5400 bgotonextstatement=
false;
5403 IkReal x420=((-1.0)*new_r01);
5408 IkReal x421=((-1.0)*(x422.
value));
5419 IkReal gconst6=x421;
5420 IkReal gconst7=new_r11;
5421 IkReal gconst8=x420;
5423 j3eval[1]=((
IKabs((new_r01*new_r11)))+(
IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5425 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5429 IkReal x423=((-1.0)*new_r01);
5434 IkReal x424=((-1.0)*(x425.
value));
5445 IkReal gconst6=x424;
5446 IkReal gconst7=new_r11;
5447 IkReal gconst8=x423;
5450 j3eval[2]=((
IKabs(new_r01*new_r01))+(
IKabs((new_r01*new_r11))));
5451 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5455 IkReal x426=((-1.0)*new_r01);
5460 IkReal x427=((-1.0)*(x428.
value));
5471 IkReal gconst6=x427;
5472 IkReal gconst7=new_r11;
5473 IkReal gconst8=x426;
5475 j3eval[1]=((((0.5)*(
IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(
IKabs((new_r01*new_r11))));
5477 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
5484 IkReal j3array[1], cj3array[1], sj3array[1];
5485 bool j3valid[1]={
false};
5487 IkReal x429=((1.0)*new_r01);
5496 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x430.
value)))+(x431.
value));
5497 sj3array[0]=
IKsin(j3array[0]);
5498 cj3array[0]=
IKcos(j3array[0]);
5499 if( j3array[0] >
IKPI )
5503 else if( j3array[0] < -
IKPI )
5504 { j3array[0]+=
IK2PI;
5507 for(
int ij3 = 0; ij3 < 1; ++ij3)
5513 _ij3[0] = ij3; _ij3[1] = -1;
5514 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5518 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5521 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5524 IkReal x432=
IKsin(j3);
5525 IkReal x433=
IKcos(j3);
5526 IkReal x434=(gconst7*x432);
5527 IkReal x435=((1.0)*x433);
5528 IkReal x436=(gconst8*x432);
5529 IkReal x437=(gconst8*x435);
5530 evalcond[0]=(((new_r01*x433))+gconst7+((new_r11*x432)));
5531 evalcond[1]=(((gconst7*x433))+x436+new_r01);
5532 evalcond[2]=((((-1.0)*x437))+x434);
5533 evalcond[3]=(((new_r01*x432))+gconst8+(((-1.0)*new_r11*x435)));
5534 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5535 evalcond[5]=((((-1.0)*x436))+(((-1.0)*gconst7*x435)));
5543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5544 vinfos[0].jointtype = 1;
5545 vinfos[0].foffset = j0;
5546 vinfos[0].indices[0] = _ij0[0];
5547 vinfos[0].indices[1] = _ij0[1];
5548 vinfos[0].maxsolutions = _nj0;
5549 vinfos[1].jointtype = 1;
5550 vinfos[1].foffset = j1;
5551 vinfos[1].indices[0] = _ij1[0];
5552 vinfos[1].indices[1] = _ij1[1];
5553 vinfos[1].maxsolutions = _nj1;
5554 vinfos[2].jointtype = 1;
5555 vinfos[2].foffset = j2;
5556 vinfos[2].indices[0] = _ij2[0];
5557 vinfos[2].indices[1] = _ij2[1];
5558 vinfos[2].maxsolutions = _nj2;
5559 vinfos[3].jointtype = 1;
5560 vinfos[3].foffset = j3;
5561 vinfos[3].indices[0] = _ij3[0];
5562 vinfos[3].indices[1] = _ij3[1];
5563 vinfos[3].maxsolutions = _nj3;
5564 vinfos[4].jointtype = 1;
5565 vinfos[4].foffset = j4;
5566 vinfos[4].indices[0] = _ij4[0];
5567 vinfos[4].indices[1] = _ij4[1];
5568 vinfos[4].maxsolutions = _nj4;
5569 vinfos[5].jointtype = 1;
5570 vinfos[5].foffset = j5;
5571 vinfos[5].indices[0] = _ij5[0];
5572 vinfos[5].indices[1] = _ij5[1];
5573 vinfos[5].maxsolutions = _nj5;
5574 std::vector<int> vfree(0);
5587 IkReal j3array[1], cj3array[1], sj3array[1];
5588 bool j3valid[1]={
false};
5598 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.
value)))+(x439.
value));
5599 sj3array[0]=
IKsin(j3array[0]);
5600 cj3array[0]=
IKcos(j3array[0]);
5601 if( j3array[0] >
IKPI )
5605 else if( j3array[0] < -
IKPI )
5606 { j3array[0]+=
IK2PI;
5609 for(
int ij3 = 0; ij3 < 1; ++ij3)
5615 _ij3[0] = ij3; _ij3[1] = -1;
5616 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5620 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5623 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5626 IkReal x440=
IKsin(j3);
5627 IkReal x441=
IKcos(j3);
5628 IkReal x442=(gconst7*x440);
5629 IkReal x443=((1.0)*x441);
5630 IkReal x444=(gconst8*x440);
5631 IkReal x445=(gconst8*x443);
5632 evalcond[0]=(((new_r01*x441))+gconst7+((new_r11*x440)));
5633 evalcond[1]=(((gconst7*x441))+x444+new_r01);
5634 evalcond[2]=((((-1.0)*x445))+x442);
5635 evalcond[3]=(((new_r01*x440))+gconst8+(((-1.0)*new_r11*x443)));
5636 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5637 evalcond[5]=((((-1.0)*x444))+(((-1.0)*gconst7*x443)));
5645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5646 vinfos[0].jointtype = 1;
5647 vinfos[0].foffset = j0;
5648 vinfos[0].indices[0] = _ij0[0];
5649 vinfos[0].indices[1] = _ij0[1];
5650 vinfos[0].maxsolutions = _nj0;
5651 vinfos[1].jointtype = 1;
5652 vinfos[1].foffset = j1;
5653 vinfos[1].indices[0] = _ij1[0];
5654 vinfos[1].indices[1] = _ij1[1];
5655 vinfos[1].maxsolutions = _nj1;
5656 vinfos[2].jointtype = 1;
5657 vinfos[2].foffset = j2;
5658 vinfos[2].indices[0] = _ij2[0];
5659 vinfos[2].indices[1] = _ij2[1];
5660 vinfos[2].maxsolutions = _nj2;
5661 vinfos[3].jointtype = 1;
5662 vinfos[3].foffset = j3;
5663 vinfos[3].indices[0] = _ij3[0];
5664 vinfos[3].indices[1] = _ij3[1];
5665 vinfos[3].maxsolutions = _nj3;
5666 vinfos[4].jointtype = 1;
5667 vinfos[4].foffset = j4;
5668 vinfos[4].indices[0] = _ij4[0];
5669 vinfos[4].indices[1] = _ij4[1];
5670 vinfos[4].maxsolutions = _nj4;
5671 vinfos[5].jointtype = 1;
5672 vinfos[5].foffset = j5;
5673 vinfos[5].indices[0] = _ij5[0];
5674 vinfos[5].indices[1] = _ij5[1];
5675 vinfos[5].maxsolutions = _nj5;
5676 std::vector<int> vfree(0);
5689 IkReal j3array[1], cj3array[1], sj3array[1];
5690 bool j3valid[1]={
false};
5700 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x446.
value)))+(x447.
value));
5701 sj3array[0]=
IKsin(j3array[0]);
5702 cj3array[0]=
IKcos(j3array[0]);
5703 if( j3array[0] >
IKPI )
5707 else if( j3array[0] < -
IKPI )
5708 { j3array[0]+=
IK2PI;
5711 for(
int ij3 = 0; ij3 < 1; ++ij3)
5717 _ij3[0] = ij3; _ij3[1] = -1;
5718 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
5722 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5725 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5728 IkReal x448=
IKsin(j3);
5729 IkReal x449=
IKcos(j3);
5730 IkReal x450=(gconst7*x448);
5731 IkReal x451=((1.0)*x449);
5732 IkReal x452=(gconst8*x448);
5733 IkReal x453=(gconst8*x451);
5734 evalcond[0]=(((new_r01*x449))+gconst7+((new_r11*x448)));
5735 evalcond[1]=(((gconst7*x449))+x452+new_r01);
5736 evalcond[2]=((((-1.0)*x453))+x450);
5737 evalcond[3]=(((new_r01*x448))+gconst8+(((-1.0)*new_r11*x451)));
5738 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5739 evalcond[5]=((((-1.0)*x452))+(((-1.0)*gconst7*x451)));
5747 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5748 vinfos[0].jointtype = 1;
5749 vinfos[0].foffset = j0;
5750 vinfos[0].indices[0] = _ij0[0];
5751 vinfos[0].indices[1] = _ij0[1];
5752 vinfos[0].maxsolutions = _nj0;
5753 vinfos[1].jointtype = 1;
5754 vinfos[1].foffset = j1;
5755 vinfos[1].indices[0] = _ij1[0];
5756 vinfos[1].indices[1] = _ij1[1];
5757 vinfos[1].maxsolutions = _nj1;
5758 vinfos[2].jointtype = 1;
5759 vinfos[2].foffset = j2;
5760 vinfos[2].indices[0] = _ij2[0];
5761 vinfos[2].indices[1] = _ij2[1];
5762 vinfos[2].maxsolutions = _nj2;
5763 vinfos[3].jointtype = 1;
5764 vinfos[3].foffset = j3;
5765 vinfos[3].indices[0] = _ij3[0];
5766 vinfos[3].indices[1] = _ij3[1];
5767 vinfos[3].maxsolutions = _nj3;
5768 vinfos[4].jointtype = 1;
5769 vinfos[4].foffset = j4;
5770 vinfos[4].indices[0] = _ij4[0];
5771 vinfos[4].indices[1] = _ij4[1];
5772 vinfos[4].maxsolutions = _nj4;
5773 vinfos[5].jointtype = 1;
5774 vinfos[5].foffset = j5;
5775 vinfos[5].indices[0] = _ij5[0];
5776 vinfos[5].indices[1] = _ij5[1];
5777 vinfos[5].maxsolutions = _nj5;
5778 std::vector<int> vfree(0);
5790 if( bgotonextstatement )
5792 bool bgotonextstatement =
true;
5795 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
5796 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5798 bgotonextstatement=
false;
5805 IkReal x454=((-1.0)*(x455.
value));
5814 IkReal gconst6=x454;
5815 IkReal x456 = new_r11*new_r11;
5819 IkReal gconst7=(new_r11*(
pow(x456,-0.5)));
5822 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5830 IkReal x457=((-1.0)*(x458.
value));
5839 IkReal gconst6=x457;
5840 IkReal x459 = new_r11*new_r11;
5844 IkReal gconst7=(new_r11*(
pow(x459,-0.5)));
5847 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
5850 IkReal j3array[2], cj3array[2], sj3array[2];
5851 bool j3valid[2]={
false};
5857 sj3array[0]=((-1.0)*new_r00*(x460.
value));
5860 j3valid[0] = j3valid[1] =
true;
5861 j3array[0] =
IKasin(sj3array[0]);
5862 cj3array[0] =
IKcos(j3array[0]);
5863 sj3array[1] = sj3array[0];
5864 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
5865 cj3array[1] = -cj3array[0];
5867 else if( isnan(sj3array[0]) )
5871 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5873 for(
int ij3 = 0; ij3 < 2; ++ij3)
5879 _ij3[0] = ij3; _ij3[1] = -1;
5880 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
5884 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5887 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5890 IkReal x461=
IKcos(j3);
5891 IkReal x462=
IKsin(j3);
5892 evalcond[0]=(gconst7*x461);
5893 evalcond[1]=(new_r00*x461);
5894 evalcond[2]=((-1.0)*new_r11*x461);
5895 evalcond[3]=(((new_r00*x462))+gconst7);
5896 evalcond[4]=(((new_r11*x462))+gconst7);
5897 evalcond[5]=(((gconst7*x462))+new_r11);
5905 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5906 vinfos[0].jointtype = 1;
5907 vinfos[0].foffset = j0;
5908 vinfos[0].indices[0] = _ij0[0];
5909 vinfos[0].indices[1] = _ij0[1];
5910 vinfos[0].maxsolutions = _nj0;
5911 vinfos[1].jointtype = 1;
5912 vinfos[1].foffset = j1;
5913 vinfos[1].indices[0] = _ij1[0];
5914 vinfos[1].indices[1] = _ij1[1];
5915 vinfos[1].maxsolutions = _nj1;
5916 vinfos[2].jointtype = 1;
5917 vinfos[2].foffset = j2;
5918 vinfos[2].indices[0] = _ij2[0];
5919 vinfos[2].indices[1] = _ij2[1];
5920 vinfos[2].maxsolutions = _nj2;
5921 vinfos[3].jointtype = 1;
5922 vinfos[3].foffset = j3;
5923 vinfos[3].indices[0] = _ij3[0];
5924 vinfos[3].indices[1] = _ij3[1];
5925 vinfos[3].maxsolutions = _nj3;
5926 vinfos[4].jointtype = 1;
5927 vinfos[4].foffset = j4;
5928 vinfos[4].indices[0] = _ij4[0];
5929 vinfos[4].indices[1] = _ij4[1];
5930 vinfos[4].maxsolutions = _nj4;
5931 vinfos[5].jointtype = 1;
5932 vinfos[5].foffset = j5;
5933 vinfos[5].indices[0] = _ij5[0];
5934 vinfos[5].indices[1] = _ij5[1];
5935 vinfos[5].maxsolutions = _nj5;
5936 std::vector<int> vfree(0);
5945 IkReal j3array[2], cj3array[2], sj3array[2];
5946 bool j3valid[2]={
false};
5952 sj3array[0]=((-1.0)*gconst7*(x463.
value));
5955 j3valid[0] = j3valid[1] =
true;
5956 j3array[0] =
IKasin(sj3array[0]);
5957 cj3array[0] =
IKcos(j3array[0]);
5958 sj3array[1] = sj3array[0];
5959 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
5960 cj3array[1] = -cj3array[0];
5962 else if( isnan(sj3array[0]) )
5966 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5968 for(
int ij3 = 0; ij3 < 2; ++ij3)
5974 _ij3[0] = ij3; _ij3[1] = -1;
5975 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
5979 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
5982 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5985 IkReal x464=
IKcos(j3);
5986 IkReal x465=
IKsin(j3);
5987 IkReal x466=(gconst7*x465);
5988 evalcond[0]=(gconst7*x464);
5989 evalcond[1]=(new_r00*x464);
5990 evalcond[2]=((-1.0)*new_r11*x464);
5991 evalcond[3]=(((new_r00*x465))+gconst7);
5992 evalcond[4]=(x466+new_r00);
5993 evalcond[5]=(x466+new_r11);
6001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6002 vinfos[0].jointtype = 1;
6003 vinfos[0].foffset = j0;
6004 vinfos[0].indices[0] = _ij0[0];
6005 vinfos[0].indices[1] = _ij0[1];
6006 vinfos[0].maxsolutions = _nj0;
6007 vinfos[1].jointtype = 1;
6008 vinfos[1].foffset = j1;
6009 vinfos[1].indices[0] = _ij1[0];
6010 vinfos[1].indices[1] = _ij1[1];
6011 vinfos[1].maxsolutions = _nj1;
6012 vinfos[2].jointtype = 1;
6013 vinfos[2].foffset = j2;
6014 vinfos[2].indices[0] = _ij2[0];
6015 vinfos[2].indices[1] = _ij2[1];
6016 vinfos[2].maxsolutions = _nj2;
6017 vinfos[3].jointtype = 1;
6018 vinfos[3].foffset = j3;
6019 vinfos[3].indices[0] = _ij3[0];
6020 vinfos[3].indices[1] = _ij3[1];
6021 vinfos[3].maxsolutions = _nj3;
6022 vinfos[4].jointtype = 1;
6023 vinfos[4].foffset = j4;
6024 vinfos[4].indices[0] = _ij4[0];
6025 vinfos[4].indices[1] = _ij4[1];
6026 vinfos[4].maxsolutions = _nj4;
6027 vinfos[5].jointtype = 1;
6028 vinfos[5].foffset = j5;
6029 vinfos[5].indices[0] = _ij5[0];
6030 vinfos[5].indices[1] = _ij5[1];
6031 vinfos[5].maxsolutions = _nj5;
6032 std::vector<int> vfree(0);
6045 IkReal j3array[2], cj3array[2], sj3array[2];
6046 bool j3valid[2]={
false};
6052 sj3array[0]=((-1.0)*gconst7*(x467.
value));
6055 j3valid[0] = j3valid[1] =
true;
6056 j3array[0] =
IKasin(sj3array[0]);
6057 cj3array[0] =
IKcos(j3array[0]);
6058 sj3array[1] = sj3array[0];
6059 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
6060 cj3array[1] = -cj3array[0];
6062 else if( isnan(sj3array[0]) )
6066 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6068 for(
int ij3 = 0; ij3 < 2; ++ij3)
6074 _ij3[0] = ij3; _ij3[1] = -1;
6075 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
6079 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6082 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6085 IkReal x468=
IKcos(j3);
6086 IkReal x469=
IKsin(j3);
6087 IkReal x470=(gconst7*x469);
6088 evalcond[0]=(gconst7*x468);
6089 evalcond[1]=(new_r00*x468);
6090 evalcond[2]=((-1.0)*new_r11*x468);
6091 evalcond[3]=(((new_r11*x469))+gconst7);
6092 evalcond[4]=(x470+new_r00);
6093 evalcond[5]=(x470+new_r11);
6101 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6102 vinfos[0].jointtype = 1;
6103 vinfos[0].foffset = j0;
6104 vinfos[0].indices[0] = _ij0[0];
6105 vinfos[0].indices[1] = _ij0[1];
6106 vinfos[0].maxsolutions = _nj0;
6107 vinfos[1].jointtype = 1;
6108 vinfos[1].foffset = j1;
6109 vinfos[1].indices[0] = _ij1[0];
6110 vinfos[1].indices[1] = _ij1[1];
6111 vinfos[1].maxsolutions = _nj1;
6112 vinfos[2].jointtype = 1;
6113 vinfos[2].foffset = j2;
6114 vinfos[2].indices[0] = _ij2[0];
6115 vinfos[2].indices[1] = _ij2[1];
6116 vinfos[2].maxsolutions = _nj2;
6117 vinfos[3].jointtype = 1;
6118 vinfos[3].foffset = j3;
6119 vinfos[3].indices[0] = _ij3[0];
6120 vinfos[3].indices[1] = _ij3[1];
6121 vinfos[3].maxsolutions = _nj3;
6122 vinfos[4].jointtype = 1;
6123 vinfos[4].foffset = j4;
6124 vinfos[4].indices[0] = _ij4[0];
6125 vinfos[4].indices[1] = _ij4[1];
6126 vinfos[4].maxsolutions = _nj4;
6127 vinfos[5].jointtype = 1;
6128 vinfos[5].foffset = j5;
6129 vinfos[5].indices[0] = _ij5[0];
6130 vinfos[5].indices[1] = _ij5[1];
6131 vinfos[5].maxsolutions = _nj5;
6132 std::vector<int> vfree(0);
6144 if( bgotonextstatement )
6146 bool bgotonextstatement =
true;
6149 evalcond[0]=
IKabs(new_r11);
6150 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
6152 bgotonextstatement=
false;
6155 IkReal x471=((-1.0)*new_r01);
6160 IkReal x472=((-1.0)*(x473.
value));
6168 IkReal gconst6=x472;
6170 IkReal x474 = new_r01*new_r01;
6174 IkReal gconst8=(x471*(
pow(x474,-0.5)));
6175 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
6176 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6180 IkReal x475=((-1.0)*new_r01);
6185 IkReal x476=((-1.0)*(x477.
value));
6193 IkReal gconst6=x476;
6195 IkReal x478 = new_r01*new_r01;
6199 IkReal gconst8=(x475*(
pow(x478,-0.5)));
6200 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
6201 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6205 IkReal x479=((-1.0)*new_r01);
6210 IkReal x480=((-1.0)*(x481.
value));
6218 IkReal gconst6=x480;
6220 IkReal x482 = new_r01*new_r01;
6224 IkReal gconst8=(x479*(
pow(x482,-0.5)));
6226 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6233 IkReal j3array[1], cj3array[1], sj3array[1];
6234 bool j3valid[1]={
false};
6247 sj3array[0]=
IKsin(j3array[0]);
6248 cj3array[0]=
IKcos(j3array[0]);
6249 if( j3array[0] >
IKPI )
6253 else if( j3array[0] < -
IKPI )
6254 { j3array[0]+=
IK2PI;
6257 for(
int ij3 = 0; ij3 < 1; ++ij3)
6263 _ij3[0] = ij3; _ij3[1] = -1;
6264 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6268 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6271 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6274 IkReal x485=
IKcos(j3);
6275 IkReal x486=
IKsin(j3);
6276 IkReal x487=(gconst8*x486);
6277 IkReal x488=(gconst8*x485);
6278 evalcond[0]=(new_r01*x485);
6279 evalcond[1]=((-1.0)*x488);
6280 evalcond[2]=(gconst8+((new_r01*x486)));
6281 evalcond[3]=(x487+new_r01);
6282 evalcond[4]=((((-1.0)*x488))+new_r00);
6283 evalcond[5]=((((-1.0)*x487))+new_r10);
6284 evalcond[6]=((((-1.0)*new_r10*x485))+((new_r00*x486)));
6285 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x486))+((new_r00*x485)));
6293 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6294 vinfos[0].jointtype = 1;
6295 vinfos[0].foffset = j0;
6296 vinfos[0].indices[0] = _ij0[0];
6297 vinfos[0].indices[1] = _ij0[1];
6298 vinfos[0].maxsolutions = _nj0;
6299 vinfos[1].jointtype = 1;
6300 vinfos[1].foffset = j1;
6301 vinfos[1].indices[0] = _ij1[0];
6302 vinfos[1].indices[1] = _ij1[1];
6303 vinfos[1].maxsolutions = _nj1;
6304 vinfos[2].jointtype = 1;
6305 vinfos[2].foffset = j2;
6306 vinfos[2].indices[0] = _ij2[0];
6307 vinfos[2].indices[1] = _ij2[1];
6308 vinfos[2].maxsolutions = _nj2;
6309 vinfos[3].jointtype = 1;
6310 vinfos[3].foffset = j3;
6311 vinfos[3].indices[0] = _ij3[0];
6312 vinfos[3].indices[1] = _ij3[1];
6313 vinfos[3].maxsolutions = _nj3;
6314 vinfos[4].jointtype = 1;
6315 vinfos[4].foffset = j4;
6316 vinfos[4].indices[0] = _ij4[0];
6317 vinfos[4].indices[1] = _ij4[1];
6318 vinfos[4].maxsolutions = _nj4;
6319 vinfos[5].jointtype = 1;
6320 vinfos[5].foffset = j5;
6321 vinfos[5].indices[0] = _ij5[0];
6322 vinfos[5].indices[1] = _ij5[1];
6323 vinfos[5].maxsolutions = _nj5;
6324 std::vector<int> vfree(0);
6337 IkReal j3array[1], cj3array[1], sj3array[1];
6338 bool j3valid[1]={
false};
6348 j3array[0]=((-1.5707963267949)+(x489.
value)+(((1.5707963267949)*(x490.
value))));
6349 sj3array[0]=
IKsin(j3array[0]);
6350 cj3array[0]=
IKcos(j3array[0]);
6351 if( j3array[0] >
IKPI )
6355 else if( j3array[0] < -
IKPI )
6356 { j3array[0]+=
IK2PI;
6359 for(
int ij3 = 0; ij3 < 1; ++ij3)
6365 _ij3[0] = ij3; _ij3[1] = -1;
6366 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6370 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6373 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6376 IkReal x491=
IKcos(j3);
6377 IkReal x492=
IKsin(j3);
6378 IkReal x493=(gconst8*x492);
6379 IkReal x494=(gconst8*x491);
6380 evalcond[0]=(new_r01*x491);
6381 evalcond[1]=((-1.0)*x494);
6382 evalcond[2]=(gconst8+((new_r01*x492)));
6383 evalcond[3]=(x493+new_r01);
6384 evalcond[4]=((((-1.0)*x494))+new_r00);
6385 evalcond[5]=((((-1.0)*x493))+new_r10);
6386 evalcond[6]=((((-1.0)*new_r10*x491))+((new_r00*x492)));
6387 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x492))+((new_r00*x491)));
6395 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6396 vinfos[0].jointtype = 1;
6397 vinfos[0].foffset = j0;
6398 vinfos[0].indices[0] = _ij0[0];
6399 vinfos[0].indices[1] = _ij0[1];
6400 vinfos[0].maxsolutions = _nj0;
6401 vinfos[1].jointtype = 1;
6402 vinfos[1].foffset = j1;
6403 vinfos[1].indices[0] = _ij1[0];
6404 vinfos[1].indices[1] = _ij1[1];
6405 vinfos[1].maxsolutions = _nj1;
6406 vinfos[2].jointtype = 1;
6407 vinfos[2].foffset = j2;
6408 vinfos[2].indices[0] = _ij2[0];
6409 vinfos[2].indices[1] = _ij2[1];
6410 vinfos[2].maxsolutions = _nj2;
6411 vinfos[3].jointtype = 1;
6412 vinfos[3].foffset = j3;
6413 vinfos[3].indices[0] = _ij3[0];
6414 vinfos[3].indices[1] = _ij3[1];
6415 vinfos[3].maxsolutions = _nj3;
6416 vinfos[4].jointtype = 1;
6417 vinfos[4].foffset = j4;
6418 vinfos[4].indices[0] = _ij4[0];
6419 vinfos[4].indices[1] = _ij4[1];
6420 vinfos[4].maxsolutions = _nj4;
6421 vinfos[5].jointtype = 1;
6422 vinfos[5].foffset = j5;
6423 vinfos[5].indices[0] = _ij5[0];
6424 vinfos[5].indices[1] = _ij5[1];
6425 vinfos[5].maxsolutions = _nj5;
6426 std::vector<int> vfree(0);
6439 IkReal j3array[1], cj3array[1], sj3array[1];
6440 bool j3valid[1]={
false};
6450 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x495.
value)))+(x496.
value));
6451 sj3array[0]=
IKsin(j3array[0]);
6452 cj3array[0]=
IKcos(j3array[0]);
6453 if( j3array[0] >
IKPI )
6457 else if( j3array[0] < -
IKPI )
6458 { j3array[0]+=
IK2PI;
6461 for(
int ij3 = 0; ij3 < 1; ++ij3)
6467 _ij3[0] = ij3; _ij3[1] = -1;
6468 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6472 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6475 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6478 IkReal x497=
IKcos(j3);
6479 IkReal x498=
IKsin(j3);
6480 IkReal x499=(gconst8*x498);
6481 IkReal x500=(gconst8*x497);
6482 evalcond[0]=(new_r01*x497);
6483 evalcond[1]=((-1.0)*x500);
6484 evalcond[2]=(gconst8+((new_r01*x498)));
6485 evalcond[3]=(x499+new_r01);
6486 evalcond[4]=(new_r00+(((-1.0)*x500)));
6487 evalcond[5]=((((-1.0)*x499))+new_r10);
6488 evalcond[6]=((((-1.0)*new_r10*x497))+((new_r00*x498)));
6489 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x498))+((new_r00*x497)));
6497 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6498 vinfos[0].jointtype = 1;
6499 vinfos[0].foffset = j0;
6500 vinfos[0].indices[0] = _ij0[0];
6501 vinfos[0].indices[1] = _ij0[1];
6502 vinfos[0].maxsolutions = _nj0;
6503 vinfos[1].jointtype = 1;
6504 vinfos[1].foffset = j1;
6505 vinfos[1].indices[0] = _ij1[0];
6506 vinfos[1].indices[1] = _ij1[1];
6507 vinfos[1].maxsolutions = _nj1;
6508 vinfos[2].jointtype = 1;
6509 vinfos[2].foffset = j2;
6510 vinfos[2].indices[0] = _ij2[0];
6511 vinfos[2].indices[1] = _ij2[1];
6512 vinfos[2].maxsolutions = _nj2;
6513 vinfos[3].jointtype = 1;
6514 vinfos[3].foffset = j3;
6515 vinfos[3].indices[0] = _ij3[0];
6516 vinfos[3].indices[1] = _ij3[1];
6517 vinfos[3].maxsolutions = _nj3;
6518 vinfos[4].jointtype = 1;
6519 vinfos[4].foffset = j4;
6520 vinfos[4].indices[0] = _ij4[0];
6521 vinfos[4].indices[1] = _ij4[1];
6522 vinfos[4].maxsolutions = _nj4;
6523 vinfos[5].jointtype = 1;
6524 vinfos[5].foffset = j5;
6525 vinfos[5].indices[0] = _ij5[0];
6526 vinfos[5].indices[1] = _ij5[1];
6527 vinfos[5].maxsolutions = _nj5;
6528 std::vector<int> vfree(0);
6540 if( bgotonextstatement )
6542 bool bgotonextstatement =
true;
6547 bgotonextstatement=
false;
6552 if( bgotonextstatement )
6564 IkReal j3array[1], cj3array[1], sj3array[1];
6565 bool j3valid[1]={
false};
6567 IkReal x501=((1.0)*new_r01);
6576 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x502.
value)))+(x503.
value));
6577 sj3array[0]=
IKsin(j3array[0]);
6578 cj3array[0]=
IKcos(j3array[0]);
6579 if( j3array[0] >
IKPI )
6583 else if( j3array[0] < -
IKPI )
6584 { j3array[0]+=
IK2PI;
6587 for(
int ij3 = 0; ij3 < 1; ++ij3)
6593 _ij3[0] = ij3; _ij3[1] = -1;
6594 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6598 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6601 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6604 IkReal x504=
IKsin(j3);
6605 IkReal x505=
IKcos(j3);
6606 IkReal x506=(gconst7*x504);
6607 IkReal x507=((1.0)*x505);
6608 IkReal x508=(gconst8*x504);
6609 IkReal x509=(gconst8*x507);
6610 evalcond[0]=(gconst7+((new_r11*x504))+((new_r01*x505)));
6611 evalcond[1]=(((gconst7*x505))+x508+new_r01);
6612 evalcond[2]=(gconst7+((new_r00*x504))+(((-1.0)*new_r10*x507)));
6613 evalcond[3]=(gconst8+(((-1.0)*new_r11*x507))+((new_r01*x504)));
6614 evalcond[4]=((((-1.0)*x509))+x506+new_r00);
6615 evalcond[5]=((((-1.0)*x509))+x506+new_r11);
6616 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x504))+((new_r00*x505)));
6617 evalcond[7]=((((-1.0)*gconst7*x507))+new_r10+(((-1.0)*x508)));
6625 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6626 vinfos[0].jointtype = 1;
6627 vinfos[0].foffset = j0;
6628 vinfos[0].indices[0] = _ij0[0];
6629 vinfos[0].indices[1] = _ij0[1];
6630 vinfos[0].maxsolutions = _nj0;
6631 vinfos[1].jointtype = 1;
6632 vinfos[1].foffset = j1;
6633 vinfos[1].indices[0] = _ij1[0];
6634 vinfos[1].indices[1] = _ij1[1];
6635 vinfos[1].maxsolutions = _nj1;
6636 vinfos[2].jointtype = 1;
6637 vinfos[2].foffset = j2;
6638 vinfos[2].indices[0] = _ij2[0];
6639 vinfos[2].indices[1] = _ij2[1];
6640 vinfos[2].maxsolutions = _nj2;
6641 vinfos[3].jointtype = 1;
6642 vinfos[3].foffset = j3;
6643 vinfos[3].indices[0] = _ij3[0];
6644 vinfos[3].indices[1] = _ij3[1];
6645 vinfos[3].maxsolutions = _nj3;
6646 vinfos[4].jointtype = 1;
6647 vinfos[4].foffset = j4;
6648 vinfos[4].indices[0] = _ij4[0];
6649 vinfos[4].indices[1] = _ij4[1];
6650 vinfos[4].maxsolutions = _nj4;
6651 vinfos[5].jointtype = 1;
6652 vinfos[5].foffset = j5;
6653 vinfos[5].indices[0] = _ij5[0];
6654 vinfos[5].indices[1] = _ij5[1];
6655 vinfos[5].maxsolutions = _nj5;
6656 std::vector<int> vfree(0);
6669 IkReal j3array[1], cj3array[1], sj3array[1];
6670 bool j3valid[1]={
false};
6672 IkReal x510=((1.0)*gconst7);
6681 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x511.
value)))+(x512.
value));
6682 sj3array[0]=
IKsin(j3array[0]);
6683 cj3array[0]=
IKcos(j3array[0]);
6684 if( j3array[0] >
IKPI )
6688 else if( j3array[0] < -
IKPI )
6689 { j3array[0]+=
IK2PI;
6692 for(
int ij3 = 0; ij3 < 1; ++ij3)
6698 _ij3[0] = ij3; _ij3[1] = -1;
6699 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6703 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6706 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6709 IkReal x513=
IKsin(j3);
6710 IkReal x514=
IKcos(j3);
6711 IkReal x515=(gconst7*x513);
6712 IkReal x516=((1.0)*x514);
6713 IkReal x517=(gconst8*x513);
6714 IkReal x518=(gconst8*x516);
6715 evalcond[0]=(((new_r01*x514))+((new_r11*x513))+gconst7);
6716 evalcond[1]=(((gconst7*x514))+x517+new_r01);
6717 evalcond[2]=(((new_r00*x513))+(((-1.0)*new_r10*x516))+gconst7);
6718 evalcond[3]=(((new_r01*x513))+(((-1.0)*new_r11*x516))+gconst8);
6719 evalcond[4]=(x515+new_r00+(((-1.0)*x518)));
6720 evalcond[5]=(x515+new_r11+(((-1.0)*x518)));
6721 evalcond[6]=(((new_r00*x514))+((new_r10*x513))+(((-1.0)*gconst8)));
6722 evalcond[7]=((((-1.0)*x517))+new_r10+(((-1.0)*gconst7*x516)));
6730 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6731 vinfos[0].jointtype = 1;
6732 vinfos[0].foffset = j0;
6733 vinfos[0].indices[0] = _ij0[0];
6734 vinfos[0].indices[1] = _ij0[1];
6735 vinfos[0].maxsolutions = _nj0;
6736 vinfos[1].jointtype = 1;
6737 vinfos[1].foffset = j1;
6738 vinfos[1].indices[0] = _ij1[0];
6739 vinfos[1].indices[1] = _ij1[1];
6740 vinfos[1].maxsolutions = _nj1;
6741 vinfos[2].jointtype = 1;
6742 vinfos[2].foffset = j2;
6743 vinfos[2].indices[0] = _ij2[0];
6744 vinfos[2].indices[1] = _ij2[1];
6745 vinfos[2].maxsolutions = _nj2;
6746 vinfos[3].jointtype = 1;
6747 vinfos[3].foffset = j3;
6748 vinfos[3].indices[0] = _ij3[0];
6749 vinfos[3].indices[1] = _ij3[1];
6750 vinfos[3].maxsolutions = _nj3;
6751 vinfos[4].jointtype = 1;
6752 vinfos[4].foffset = j4;
6753 vinfos[4].indices[0] = _ij4[0];
6754 vinfos[4].indices[1] = _ij4[1];
6755 vinfos[4].maxsolutions = _nj4;
6756 vinfos[5].jointtype = 1;
6757 vinfos[5].foffset = j5;
6758 vinfos[5].indices[0] = _ij5[0];
6759 vinfos[5].indices[1] = _ij5[1];
6760 vinfos[5].maxsolutions = _nj5;
6761 std::vector<int> vfree(0);
6774 IkReal j3array[1], cj3array[1], sj3array[1];
6775 bool j3valid[1]={
false};
6777 IkReal x519=((1.0)*new_r11);
6786 j3array[0]=((-1.5707963267949)+(x520.
value)+(((1.5707963267949)*(x521.
value))));
6787 sj3array[0]=
IKsin(j3array[0]);
6788 cj3array[0]=
IKcos(j3array[0]);
6789 if( j3array[0] >
IKPI )
6793 else if( j3array[0] < -
IKPI )
6794 { j3array[0]+=
IK2PI;
6797 for(
int ij3 = 0; ij3 < 1; ++ij3)
6803 _ij3[0] = ij3; _ij3[1] = -1;
6804 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
6808 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
6811 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6814 IkReal x522=
IKsin(j3);
6815 IkReal x523=
IKcos(j3);
6816 IkReal x524=(gconst7*x522);
6817 IkReal x525=((1.0)*x523);
6818 IkReal x526=(gconst8*x522);
6819 IkReal x527=(gconst8*x525);
6820 evalcond[0]=(((new_r01*x523))+gconst7+((new_r11*x522)));
6821 evalcond[1]=(((gconst7*x523))+x526+new_r01);
6822 evalcond[2]=(gconst7+(((-1.0)*new_r10*x525))+((new_r00*x522)));
6823 evalcond[3]=(((new_r01*x522))+gconst8+(((-1.0)*new_r11*x525)));
6824 evalcond[4]=((((-1.0)*x527))+x524+new_r00);
6825 evalcond[5]=((((-1.0)*x527))+x524+new_r11);
6826 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x522))+((new_r00*x523)));
6827 evalcond[7]=((((-1.0)*x526))+(((-1.0)*gconst7*x525))+new_r10);
6835 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6836 vinfos[0].jointtype = 1;
6837 vinfos[0].foffset = j0;
6838 vinfos[0].indices[0] = _ij0[0];
6839 vinfos[0].indices[1] = _ij0[1];
6840 vinfos[0].maxsolutions = _nj0;
6841 vinfos[1].jointtype = 1;
6842 vinfos[1].foffset = j1;
6843 vinfos[1].indices[0] = _ij1[0];
6844 vinfos[1].indices[1] = _ij1[1];
6845 vinfos[1].maxsolutions = _nj1;
6846 vinfos[2].jointtype = 1;
6847 vinfos[2].foffset = j2;
6848 vinfos[2].indices[0] = _ij2[0];
6849 vinfos[2].indices[1] = _ij2[1];
6850 vinfos[2].maxsolutions = _nj2;
6851 vinfos[3].jointtype = 1;
6852 vinfos[3].foffset = j3;
6853 vinfos[3].indices[0] = _ij3[0];
6854 vinfos[3].indices[1] = _ij3[1];
6855 vinfos[3].maxsolutions = _nj3;
6856 vinfos[4].jointtype = 1;
6857 vinfos[4].foffset = j4;
6858 vinfos[4].indices[0] = _ij4[0];
6859 vinfos[4].indices[1] = _ij4[1];
6860 vinfos[4].maxsolutions = _nj4;
6861 vinfos[5].jointtype = 1;
6862 vinfos[5].foffset = j5;
6863 vinfos[5].indices[0] = _ij5[0];
6864 vinfos[5].indices[1] = _ij5[1];
6865 vinfos[5].maxsolutions = _nj5;
6866 std::vector<int> vfree(0);
6878 if( bgotonextstatement )
6880 bool bgotonextstatement =
true;
6883 IkReal x528=((-1.0)*new_r11);
6884 IkReal x530 = ((new_r01*new_r01)+(new_r11*new_r11));
6888 IkReal x529=
pow(x530,-0.5);
6893 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x531.
value))));
6894 IkReal gconst10=(x528*x529);
6895 IkReal gconst11=((1.0)*new_r01*x529);
6900 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+(x532.
value)+j5)))), 6.28318530717959)));
6901 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
6903 bgotonextstatement=
false;
6906 IkReal x533=((-1.0)*new_r11);
6911 IkReal x534=((1.0)*(x536.
value));
6918 j5=((3.14159265)+(((-1.0)*x534)));
6919 IkReal gconst9=((3.14159265358979)+(((-1.0)*x534)));
6920 IkReal gconst10=(x533*x535);
6921 IkReal gconst11=((1.0)*new_r01*x535);
6922 IkReal x537=new_r11*new_r11;
6923 IkReal x538=((1.0)*new_r01);
6924 IkReal x539=((1.0)*new_r10);
6925 IkReal x540=((((-1.0)*new_r00*x538))+(((-1.0)*new_r11*x539)));
6927 IkReal x542=(new_r11*x541);
6929 j3eval[1]=((
IKabs(((((-1.0)*x539*x542))+(((-1.0)*x538*x542)))))+(
IKabs((((x537*x541))+(((-1.0)*new_r00*x542))))));
6931 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
6935 IkReal x543=((-1.0)*new_r11);
6940 IkReal x544=((1.0)*(x546.
value));
6947 j5=((3.14159265)+(((-1.0)*x544)));
6948 IkReal gconst9=((3.14159265358979)+(((-1.0)*x544)));
6949 IkReal gconst10=(x543*x545);
6950 IkReal gconst11=((1.0)*new_r01*x545);
6951 IkReal x547=new_r11*new_r11;
6952 IkReal x548=new_r01*new_r01*new_r01;
6957 IkReal x549=x552.
value;
6958 IkReal x550=(x547*x549);
6959 IkReal x551=(x548*x549);
6960 j3eval[0]=((
IKabs((((new_r10*x551))+x550+((new_r01*new_r10*x550)))))+(
IKabs((((new_r00*new_r01*x550))+((new_r01*new_r11*x549))+((new_r00*x551))))));
6961 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
6965 IkReal x553=((-1.0)*new_r11);
6970 IkReal x554=((1.0)*(x556.
value));
6977 j5=((3.14159265)+(((-1.0)*x554)));
6978 IkReal gconst9=((3.14159265358979)+(((-1.0)*x554)));
6979 IkReal gconst10=(x553*x555);
6980 IkReal gconst11=((1.0)*new_r01*x555);
6981 IkReal x557=new_r01*new_r01;
6982 IkReal x558=new_r11*new_r11;
6987 IkReal x559=x565.
value;
6988 IkReal x560=(x558*x559);
6993 IkReal x561=x566.
value;
6994 IkReal x562=((1.0)*x561);
6995 IkReal x563=(new_r11*x562);
6996 IkReal x564=(new_r01*x562);
6997 j3eval[0]=((
IKabs((((x559*(x557*x557)))+(((-1.0)*x560))+((x557*x560)))))+(
IKabs(((((-1.0)*new_r01*x563*(new_r11*new_r11)))+(((-1.0)*x563*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x563))))));
6998 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7002 bool bgotonextstatement =
true;
7005 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
7006 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7008 bgotonextstatement=
false;
7010 IkReal j3array[2], cj3array[2], sj3array[2];
7011 bool j3valid[2]={
false};
7017 sj3array[0]=(new_r10*(x567.
value));
7020 j3valid[0] = j3valid[1] =
true;
7021 j3array[0] =
IKasin(sj3array[0]);
7022 cj3array[0] =
IKcos(j3array[0]);
7023 sj3array[1] = sj3array[0];
7024 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
7025 cj3array[1] = -cj3array[0];
7027 else if( isnan(sj3array[0]) )
7031 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7033 for(
int ij3 = 0; ij3 < 2; ++ij3)
7039 _ij3[0] = ij3; _ij3[1] = -1;
7040 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
7044 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7047 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7050 IkReal x568=
IKcos(j3);
7051 IkReal x569=
IKsin(j3);
7052 IkReal x570=((-1.0)*x568);
7053 evalcond[0]=(new_r01*x568);
7054 evalcond[1]=(new_r10*x570);
7055 evalcond[2]=(gconst11*x570);
7056 evalcond[3]=(gconst11+((new_r01*x569)));
7057 evalcond[4]=(((gconst11*x569))+new_r01);
7058 evalcond[5]=((((-1.0)*gconst11))+((new_r10*x569)));
7066 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7067 vinfos[0].jointtype = 1;
7068 vinfos[0].foffset = j0;
7069 vinfos[0].indices[0] = _ij0[0];
7070 vinfos[0].indices[1] = _ij0[1];
7071 vinfos[0].maxsolutions = _nj0;
7072 vinfos[1].jointtype = 1;
7073 vinfos[1].foffset = j1;
7074 vinfos[1].indices[0] = _ij1[0];
7075 vinfos[1].indices[1] = _ij1[1];
7076 vinfos[1].maxsolutions = _nj1;
7077 vinfos[2].jointtype = 1;
7078 vinfos[2].foffset = j2;
7079 vinfos[2].indices[0] = _ij2[0];
7080 vinfos[2].indices[1] = _ij2[1];
7081 vinfos[2].maxsolutions = _nj2;
7082 vinfos[3].jointtype = 1;
7083 vinfos[3].foffset = j3;
7084 vinfos[3].indices[0] = _ij3[0];
7085 vinfos[3].indices[1] = _ij3[1];
7086 vinfos[3].maxsolutions = _nj3;
7087 vinfos[4].jointtype = 1;
7088 vinfos[4].foffset = j4;
7089 vinfos[4].indices[0] = _ij4[0];
7090 vinfos[4].indices[1] = _ij4[1];
7091 vinfos[4].maxsolutions = _nj4;
7092 vinfos[5].jointtype = 1;
7093 vinfos[5].foffset = j5;
7094 vinfos[5].indices[0] = _ij5[0];
7095 vinfos[5].indices[1] = _ij5[1];
7096 vinfos[5].maxsolutions = _nj5;
7097 std::vector<int> vfree(0);
7105 if( bgotonextstatement )
7107 bool bgotonextstatement =
true;
7110 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
7111 evalcond[1]=gconst10;
7112 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
7114 bgotonextstatement=
false;
7117 IkReal x571=((-1.0)*new_r11);
7122 IkReal x572=((1.0)*(x573.
value));
7128 j5=((3.14159265)+(((-1.0)*x572)));
7133 IkReal gconst9=((3.14159265358979)+(((-1.0)*x572)));
7134 IkReal gconst10=x571;
7135 IkReal gconst11=((1.0)*new_r01);
7137 j3eval[1]=((
IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(
IKabs(((1.0)*new_r01*new_r11))));
7139 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
7143 IkReal x574=((-1.0)*new_r11);
7148 IkReal x575=((1.0)*(x576.
value));
7154 j5=((3.14159265)+(((-1.0)*x575)));
7159 IkReal gconst9=((3.14159265358979)+(((-1.0)*x575)));
7160 IkReal gconst10=x574;
7161 IkReal gconst11=((1.0)*new_r01);
7166 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 ||
IKabs(j3eval[3]) < 0.0000010000000000 )
7170 IkReal x577=((-1.0)*new_r11);
7175 IkReal x578=((1.0)*(x579.
value));
7181 j5=((3.14159265)+(((-1.0)*x578)));
7186 IkReal gconst9=((3.14159265358979)+(((-1.0)*x578)));
7187 IkReal gconst10=x577;
7188 IkReal gconst11=((1.0)*new_r01);
7191 j3eval[2]=((
IKabs(((2.0)*new_r01*new_r11)))+(
IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7192 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
7199 IkReal j3array[1], cj3array[1], sj3array[1];
7200 bool j3valid[1]={
false};
7202 IkReal x580=((1.0)*new_r01);
7211 j3array[0]=((-1.5707963267949)+(x581.
value)+(((1.5707963267949)*(x582.
value))));
7212 sj3array[0]=
IKsin(j3array[0]);
7213 cj3array[0]=
IKcos(j3array[0]);
7214 if( j3array[0] >
IKPI )
7218 else if( j3array[0] < -
IKPI )
7219 { j3array[0]+=
IK2PI;
7222 for(
int ij3 = 0; ij3 < 1; ++ij3)
7228 _ij3[0] = ij3; _ij3[1] = -1;
7229 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
7233 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7236 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7239 IkReal x583=
IKcos(j3);
7240 IkReal x584=
IKsin(j3);
7241 IkReal x585=(gconst10*x584);
7242 IkReal x586=((1.0)*x583);
7243 IkReal x587=(gconst11*x584);
7244 IkReal x588=(gconst11*x586);
7245 evalcond[0]=(gconst10+((new_r01*x583))+((new_r11*x584)));
7246 evalcond[1]=(x587+new_r01+((gconst10*x583)));
7247 evalcond[2]=(x585+(((-1.0)*x588)));
7248 evalcond[3]=(gconst11+(((-1.0)*new_r11*x586))+((new_r01*x584)));
7249 evalcond[4]=(x585+new_r11+(((-1.0)*x588)));
7250 evalcond[5]=((((-1.0)*gconst10*x586))+(((-1.0)*x587)));
7258 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7259 vinfos[0].jointtype = 1;
7260 vinfos[0].foffset = j0;
7261 vinfos[0].indices[0] = _ij0[0];
7262 vinfos[0].indices[1] = _ij0[1];
7263 vinfos[0].maxsolutions = _nj0;
7264 vinfos[1].jointtype = 1;
7265 vinfos[1].foffset = j1;
7266 vinfos[1].indices[0] = _ij1[0];
7267 vinfos[1].indices[1] = _ij1[1];
7268 vinfos[1].maxsolutions = _nj1;
7269 vinfos[2].jointtype = 1;
7270 vinfos[2].foffset = j2;
7271 vinfos[2].indices[0] = _ij2[0];
7272 vinfos[2].indices[1] = _ij2[1];
7273 vinfos[2].maxsolutions = _nj2;
7274 vinfos[3].jointtype = 1;
7275 vinfos[3].foffset = j3;
7276 vinfos[3].indices[0] = _ij3[0];
7277 vinfos[3].indices[1] = _ij3[1];
7278 vinfos[3].maxsolutions = _nj3;
7279 vinfos[4].jointtype = 1;
7280 vinfos[4].foffset = j4;
7281 vinfos[4].indices[0] = _ij4[0];
7282 vinfos[4].indices[1] = _ij4[1];
7283 vinfos[4].maxsolutions = _nj4;
7284 vinfos[5].jointtype = 1;
7285 vinfos[5].foffset = j5;
7286 vinfos[5].indices[0] = _ij5[0];
7287 vinfos[5].indices[1] = _ij5[1];
7288 vinfos[5].maxsolutions = _nj5;
7289 std::vector<int> vfree(0);
7302 IkReal j3array[1], cj3array[1], sj3array[1];
7303 bool j3valid[1]={
false};
7313 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x589.
value)))+(x590.
value));
7314 sj3array[0]=
IKsin(j3array[0]);
7315 cj3array[0]=
IKcos(j3array[0]);
7316 if( j3array[0] >
IKPI )
7320 else if( j3array[0] < -
IKPI )
7321 { j3array[0]+=
IK2PI;
7324 for(
int ij3 = 0; ij3 < 1; ++ij3)
7330 _ij3[0] = ij3; _ij3[1] = -1;
7331 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
7335 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7338 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7341 IkReal x591=
IKcos(j3);
7342 IkReal x592=
IKsin(j3);
7343 IkReal x593=(gconst10*x592);
7344 IkReal x594=((1.0)*x591);
7345 IkReal x595=(gconst11*x592);
7346 IkReal x596=(gconst11*x594);
7347 evalcond[0]=(((new_r11*x592))+gconst10+((new_r01*x591)));
7348 evalcond[1]=(((gconst10*x591))+x595+new_r01);
7349 evalcond[2]=((((-1.0)*x596))+x593);
7350 evalcond[3]=((((-1.0)*new_r11*x594))+gconst11+((new_r01*x592)));
7351 evalcond[4]=((((-1.0)*x596))+x593+new_r11);
7352 evalcond[5]=((((-1.0)*x595))+(((-1.0)*gconst10*x594)));
7360 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7361 vinfos[0].jointtype = 1;
7362 vinfos[0].foffset = j0;
7363 vinfos[0].indices[0] = _ij0[0];
7364 vinfos[0].indices[1] = _ij0[1];
7365 vinfos[0].maxsolutions = _nj0;
7366 vinfos[1].jointtype = 1;
7367 vinfos[1].foffset = j1;
7368 vinfos[1].indices[0] = _ij1[0];
7369 vinfos[1].indices[1] = _ij1[1];
7370 vinfos[1].maxsolutions = _nj1;
7371 vinfos[2].jointtype = 1;
7372 vinfos[2].foffset = j2;
7373 vinfos[2].indices[0] = _ij2[0];
7374 vinfos[2].indices[1] = _ij2[1];
7375 vinfos[2].maxsolutions = _nj2;
7376 vinfos[3].jointtype = 1;
7377 vinfos[3].foffset = j3;
7378 vinfos[3].indices[0] = _ij3[0];
7379 vinfos[3].indices[1] = _ij3[1];
7380 vinfos[3].maxsolutions = _nj3;
7381 vinfos[4].jointtype = 1;
7382 vinfos[4].foffset = j4;
7383 vinfos[4].indices[0] = _ij4[0];
7384 vinfos[4].indices[1] = _ij4[1];
7385 vinfos[4].maxsolutions = _nj4;
7386 vinfos[5].jointtype = 1;
7387 vinfos[5].foffset = j5;
7388 vinfos[5].indices[0] = _ij5[0];
7389 vinfos[5].indices[1] = _ij5[1];
7390 vinfos[5].maxsolutions = _nj5;
7391 std::vector<int> vfree(0);
7404 IkReal j3array[1], cj3array[1], sj3array[1];
7405 bool j3valid[1]={
false};
7415 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x597.
value)))+(x598.
value));
7416 sj3array[0]=
IKsin(j3array[0]);
7417 cj3array[0]=
IKcos(j3array[0]);
7418 if( j3array[0] >
IKPI )
7422 else if( j3array[0] < -
IKPI )
7423 { j3array[0]+=
IK2PI;
7426 for(
int ij3 = 0; ij3 < 1; ++ij3)
7432 _ij3[0] = ij3; _ij3[1] = -1;
7433 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
7437 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7440 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7443 IkReal x599=
IKcos(j3);
7444 IkReal x600=
IKsin(j3);
7445 IkReal x601=(gconst10*x600);
7446 IkReal x602=((1.0)*x599);
7447 IkReal x603=(gconst11*x600);
7448 IkReal x604=(gconst11*x602);
7449 evalcond[0]=(gconst10+((new_r11*x600))+((new_r01*x599)));
7450 evalcond[1]=(((gconst10*x599))+x603+new_r01);
7451 evalcond[2]=(x601+(((-1.0)*x604)));
7452 evalcond[3]=((((-1.0)*new_r11*x602))+gconst11+((new_r01*x600)));
7453 evalcond[4]=(x601+(((-1.0)*x604))+new_r11);
7454 evalcond[5]=((((-1.0)*gconst10*x602))+(((-1.0)*x603)));
7462 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7463 vinfos[0].jointtype = 1;
7464 vinfos[0].foffset = j0;
7465 vinfos[0].indices[0] = _ij0[0];
7466 vinfos[0].indices[1] = _ij0[1];
7467 vinfos[0].maxsolutions = _nj0;
7468 vinfos[1].jointtype = 1;
7469 vinfos[1].foffset = j1;
7470 vinfos[1].indices[0] = _ij1[0];
7471 vinfos[1].indices[1] = _ij1[1];
7472 vinfos[1].maxsolutions = _nj1;
7473 vinfos[2].jointtype = 1;
7474 vinfos[2].foffset = j2;
7475 vinfos[2].indices[0] = _ij2[0];
7476 vinfos[2].indices[1] = _ij2[1];
7477 vinfos[2].maxsolutions = _nj2;
7478 vinfos[3].jointtype = 1;
7479 vinfos[3].foffset = j3;
7480 vinfos[3].indices[0] = _ij3[0];
7481 vinfos[3].indices[1] = _ij3[1];
7482 vinfos[3].maxsolutions = _nj3;
7483 vinfos[4].jointtype = 1;
7484 vinfos[4].foffset = j4;
7485 vinfos[4].indices[0] = _ij4[0];
7486 vinfos[4].indices[1] = _ij4[1];
7487 vinfos[4].maxsolutions = _nj4;
7488 vinfos[5].jointtype = 1;
7489 vinfos[5].foffset = j5;
7490 vinfos[5].indices[0] = _ij5[0];
7491 vinfos[5].indices[1] = _ij5[1];
7492 vinfos[5].maxsolutions = _nj5;
7493 std::vector<int> vfree(0);
7505 if( bgotonextstatement )
7507 bool bgotonextstatement =
true;
7510 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
7511 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7513 bgotonextstatement=
false;
7516 IkReal x605=((-1.0)*new_r11);
7521 IkReal x606=((1.0)*(x607.
value));
7527 j5=((3.14159265)+(((-1.0)*x606)));
7530 IkReal gconst9=((3.14159265358979)+(((-1.0)*x606)));
7531 IkReal x608 = new_r11*new_r11;
7535 IkReal gconst10=(x605*(
pow(x608,-0.5)));
7538 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7542 IkReal x609=((-1.0)*new_r11);
7547 IkReal x610=((1.0)*(x611.
value));
7553 j5=((3.14159265)+(((-1.0)*x610)));
7556 IkReal gconst9=((3.14159265358979)+(((-1.0)*x610)));
7557 IkReal x612 = new_r11*new_r11;
7561 IkReal gconst10=(x609*(
pow(x612,-0.5)));
7564 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7567 IkReal j3array[2], cj3array[2], sj3array[2];
7568 bool j3valid[2]={
false};
7574 sj3array[0]=((-1.0)*new_r00*(x613.
value));
7577 j3valid[0] = j3valid[1] =
true;
7578 j3array[0] =
IKasin(sj3array[0]);
7579 cj3array[0] =
IKcos(j3array[0]);
7580 sj3array[1] = sj3array[0];
7581 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
7582 cj3array[1] = -cj3array[0];
7584 else if( isnan(sj3array[0]) )
7588 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7590 for(
int ij3 = 0; ij3 < 2; ++ij3)
7596 _ij3[0] = ij3; _ij3[1] = -1;
7597 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
7601 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7604 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7607 IkReal x614=
IKcos(j3);
7608 IkReal x615=
IKsin(j3);
7609 evalcond[0]=(gconst10*x614);
7610 evalcond[1]=(new_r00*x614);
7611 evalcond[2]=((-1.0)*new_r11*x614);
7612 evalcond[3]=(gconst10+((new_r00*x615)));
7613 evalcond[4]=(gconst10+((new_r11*x615)));
7614 evalcond[5]=(new_r11+((gconst10*x615)));
7622 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7623 vinfos[0].jointtype = 1;
7624 vinfos[0].foffset = j0;
7625 vinfos[0].indices[0] = _ij0[0];
7626 vinfos[0].indices[1] = _ij0[1];
7627 vinfos[0].maxsolutions = _nj0;
7628 vinfos[1].jointtype = 1;
7629 vinfos[1].foffset = j1;
7630 vinfos[1].indices[0] = _ij1[0];
7631 vinfos[1].indices[1] = _ij1[1];
7632 vinfos[1].maxsolutions = _nj1;
7633 vinfos[2].jointtype = 1;
7634 vinfos[2].foffset = j2;
7635 vinfos[2].indices[0] = _ij2[0];
7636 vinfos[2].indices[1] = _ij2[1];
7637 vinfos[2].maxsolutions = _nj2;
7638 vinfos[3].jointtype = 1;
7639 vinfos[3].foffset = j3;
7640 vinfos[3].indices[0] = _ij3[0];
7641 vinfos[3].indices[1] = _ij3[1];
7642 vinfos[3].maxsolutions = _nj3;
7643 vinfos[4].jointtype = 1;
7644 vinfos[4].foffset = j4;
7645 vinfos[4].indices[0] = _ij4[0];
7646 vinfos[4].indices[1] = _ij4[1];
7647 vinfos[4].maxsolutions = _nj4;
7648 vinfos[5].jointtype = 1;
7649 vinfos[5].foffset = j5;
7650 vinfos[5].indices[0] = _ij5[0];
7651 vinfos[5].indices[1] = _ij5[1];
7652 vinfos[5].maxsolutions = _nj5;
7653 std::vector<int> vfree(0);
7662 IkReal j3array[2], cj3array[2], sj3array[2];
7663 bool j3valid[2]={
false};
7669 sj3array[0]=((-1.0)*gconst10*(x616.
value));
7672 j3valid[0] = j3valid[1] =
true;
7673 j3array[0] =
IKasin(sj3array[0]);
7674 cj3array[0] =
IKcos(j3array[0]);
7675 sj3array[1] = sj3array[0];
7676 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
7677 cj3array[1] = -cj3array[0];
7679 else if( isnan(sj3array[0]) )
7683 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7685 for(
int ij3 = 0; ij3 < 2; ++ij3)
7691 _ij3[0] = ij3; _ij3[1] = -1;
7692 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
7696 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7699 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7702 IkReal x617=
IKcos(j3);
7703 IkReal x618=
IKsin(j3);
7704 IkReal x619=(gconst10*x618);
7705 evalcond[0]=(gconst10*x617);
7706 evalcond[1]=(new_r00*x617);
7707 evalcond[2]=((-1.0)*new_r11*x617);
7708 evalcond[3]=(gconst10+((new_r00*x618)));
7709 evalcond[4]=(x619+new_r00);
7710 evalcond[5]=(x619+new_r11);
7718 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7719 vinfos[0].jointtype = 1;
7720 vinfos[0].foffset = j0;
7721 vinfos[0].indices[0] = _ij0[0];
7722 vinfos[0].indices[1] = _ij0[1];
7723 vinfos[0].maxsolutions = _nj0;
7724 vinfos[1].jointtype = 1;
7725 vinfos[1].foffset = j1;
7726 vinfos[1].indices[0] = _ij1[0];
7727 vinfos[1].indices[1] = _ij1[1];
7728 vinfos[1].maxsolutions = _nj1;
7729 vinfos[2].jointtype = 1;
7730 vinfos[2].foffset = j2;
7731 vinfos[2].indices[0] = _ij2[0];
7732 vinfos[2].indices[1] = _ij2[1];
7733 vinfos[2].maxsolutions = _nj2;
7734 vinfos[3].jointtype = 1;
7735 vinfos[3].foffset = j3;
7736 vinfos[3].indices[0] = _ij3[0];
7737 vinfos[3].indices[1] = _ij3[1];
7738 vinfos[3].maxsolutions = _nj3;
7739 vinfos[4].jointtype = 1;
7740 vinfos[4].foffset = j4;
7741 vinfos[4].indices[0] = _ij4[0];
7742 vinfos[4].indices[1] = _ij4[1];
7743 vinfos[4].maxsolutions = _nj4;
7744 vinfos[5].jointtype = 1;
7745 vinfos[5].foffset = j5;
7746 vinfos[5].indices[0] = _ij5[0];
7747 vinfos[5].indices[1] = _ij5[1];
7748 vinfos[5].maxsolutions = _nj5;
7749 std::vector<int> vfree(0);
7762 IkReal j3array[2], cj3array[2], sj3array[2];
7763 bool j3valid[2]={
false};
7769 sj3array[0]=((-1.0)*gconst10*(x620.
value));
7772 j3valid[0] = j3valid[1] =
true;
7773 j3array[0] =
IKasin(sj3array[0]);
7774 cj3array[0] =
IKcos(j3array[0]);
7775 sj3array[1] = sj3array[0];
7776 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
7777 cj3array[1] = -cj3array[0];
7779 else if( isnan(sj3array[0]) )
7783 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7785 for(
int ij3 = 0; ij3 < 2; ++ij3)
7791 _ij3[0] = ij3; _ij3[1] = -1;
7792 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
7796 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7799 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7802 IkReal x621=
IKcos(j3);
7803 IkReal x622=
IKsin(j3);
7804 IkReal x623=(gconst10*x622);
7805 evalcond[0]=(gconst10*x621);
7806 evalcond[1]=(new_r00*x621);
7807 evalcond[2]=((-1.0)*new_r11*x621);
7808 evalcond[3]=(gconst10+((new_r11*x622)));
7809 evalcond[4]=(x623+new_r00);
7810 evalcond[5]=(x623+new_r11);
7818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7819 vinfos[0].jointtype = 1;
7820 vinfos[0].foffset = j0;
7821 vinfos[0].indices[0] = _ij0[0];
7822 vinfos[0].indices[1] = _ij0[1];
7823 vinfos[0].maxsolutions = _nj0;
7824 vinfos[1].jointtype = 1;
7825 vinfos[1].foffset = j1;
7826 vinfos[1].indices[0] = _ij1[0];
7827 vinfos[1].indices[1] = _ij1[1];
7828 vinfos[1].maxsolutions = _nj1;
7829 vinfos[2].jointtype = 1;
7830 vinfos[2].foffset = j2;
7831 vinfos[2].indices[0] = _ij2[0];
7832 vinfos[2].indices[1] = _ij2[1];
7833 vinfos[2].maxsolutions = _nj2;
7834 vinfos[3].jointtype = 1;
7835 vinfos[3].foffset = j3;
7836 vinfos[3].indices[0] = _ij3[0];
7837 vinfos[3].indices[1] = _ij3[1];
7838 vinfos[3].maxsolutions = _nj3;
7839 vinfos[4].jointtype = 1;
7840 vinfos[4].foffset = j4;
7841 vinfos[4].indices[0] = _ij4[0];
7842 vinfos[4].indices[1] = _ij4[1];
7843 vinfos[4].maxsolutions = _nj4;
7844 vinfos[5].jointtype = 1;
7845 vinfos[5].foffset = j5;
7846 vinfos[5].indices[0] = _ij5[0];
7847 vinfos[5].indices[1] = _ij5[1];
7848 vinfos[5].maxsolutions = _nj5;
7849 std::vector<int> vfree(0);
7861 if( bgotonextstatement )
7863 bool bgotonextstatement =
true;
7866 evalcond[0]=
IKabs(new_r11);
7867 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7869 bgotonextstatement=
false;
7876 IkReal x624=((1.0)*(x625.
value));
7882 j5=((3.14159265)+(((-1.0)*x624)));
7884 IkReal gconst9=((3.14159265358979)+(((-1.0)*x624)));
7886 IkReal x626 = new_r01*new_r01;
7890 IkReal gconst11=((1.0)*new_r01*(
pow(x626,-0.5)));
7891 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
7892 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7900 IkReal x627=((1.0)*(x628.
value));
7906 j5=((3.14159265)+(((-1.0)*x627)));
7908 IkReal gconst9=((3.14159265358979)+(((-1.0)*x627)));
7910 IkReal x629 = new_r01*new_r01;
7914 IkReal gconst11=((1.0)*new_r01*(
pow(x629,-0.5)));
7915 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
7916 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7924 IkReal x630=((1.0)*(x631.
value));
7930 j5=((3.14159265)+(((-1.0)*x630)));
7932 IkReal gconst9=((3.14159265358979)+(((-1.0)*x630)));
7934 IkReal x632 = new_r01*new_r01;
7938 IkReal gconst11=((1.0)*new_r01*(
pow(x632,-0.5)));
7940 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
7947 IkReal j3array[1], cj3array[1], sj3array[1];
7948 bool j3valid[1]={
false};
7961 sj3array[0]=
IKsin(j3array[0]);
7962 cj3array[0]=
IKcos(j3array[0]);
7963 if( j3array[0] >
IKPI )
7967 else if( j3array[0] < -
IKPI )
7968 { j3array[0]+=
IK2PI;
7971 for(
int ij3 = 0; ij3 < 1; ++ij3)
7977 _ij3[0] = ij3; _ij3[1] = -1;
7978 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
7982 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
7985 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7988 IkReal x635=
IKcos(j3);
7989 IkReal x636=
IKsin(j3);
7990 IkReal x637=((1.0)*gconst11);
7991 IkReal x638=(gconst11*x635);
7992 evalcond[0]=(new_r01*x635);
7993 evalcond[1]=((-1.0)*x638);
7994 evalcond[2]=(gconst11+((new_r01*x636)));
7995 evalcond[3]=(((gconst11*x636))+new_r01);
7996 evalcond[4]=((((-1.0)*x635*x637))+new_r00);
7997 evalcond[5]=((((-1.0)*x636*x637))+new_r10);
7998 evalcond[6]=((((-1.0)*new_r10*x635))+((new_r00*x636)));
7999 evalcond[7]=((((-1.0)*x637))+((new_r00*x635))+((new_r10*x636)));
8007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8008 vinfos[0].jointtype = 1;
8009 vinfos[0].foffset = j0;
8010 vinfos[0].indices[0] = _ij0[0];
8011 vinfos[0].indices[1] = _ij0[1];
8012 vinfos[0].maxsolutions = _nj0;
8013 vinfos[1].jointtype = 1;
8014 vinfos[1].foffset = j1;
8015 vinfos[1].indices[0] = _ij1[0];
8016 vinfos[1].indices[1] = _ij1[1];
8017 vinfos[1].maxsolutions = _nj1;
8018 vinfos[2].jointtype = 1;
8019 vinfos[2].foffset = j2;
8020 vinfos[2].indices[0] = _ij2[0];
8021 vinfos[2].indices[1] = _ij2[1];
8022 vinfos[2].maxsolutions = _nj2;
8023 vinfos[3].jointtype = 1;
8024 vinfos[3].foffset = j3;
8025 vinfos[3].indices[0] = _ij3[0];
8026 vinfos[3].indices[1] = _ij3[1];
8027 vinfos[3].maxsolutions = _nj3;
8028 vinfos[4].jointtype = 1;
8029 vinfos[4].foffset = j4;
8030 vinfos[4].indices[0] = _ij4[0];
8031 vinfos[4].indices[1] = _ij4[1];
8032 vinfos[4].maxsolutions = _nj4;
8033 vinfos[5].jointtype = 1;
8034 vinfos[5].foffset = j5;
8035 vinfos[5].indices[0] = _ij5[0];
8036 vinfos[5].indices[1] = _ij5[1];
8037 vinfos[5].maxsolutions = _nj5;
8038 std::vector<int> vfree(0);
8051 IkReal j3array[1], cj3array[1], sj3array[1];
8052 bool j3valid[1]={
false};
8062 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x639.
value)))+(x640.
value));
8063 sj3array[0]=
IKsin(j3array[0]);
8064 cj3array[0]=
IKcos(j3array[0]);
8065 if( j3array[0] >
IKPI )
8069 else if( j3array[0] < -
IKPI )
8070 { j3array[0]+=
IK2PI;
8073 for(
int ij3 = 0; ij3 < 1; ++ij3)
8079 _ij3[0] = ij3; _ij3[1] = -1;
8080 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8084 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8087 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8090 IkReal x641=
IKcos(j3);
8091 IkReal x642=
IKsin(j3);
8092 IkReal x643=((1.0)*gconst11);
8093 IkReal x644=(gconst11*x641);
8094 evalcond[0]=(new_r01*x641);
8095 evalcond[1]=((-1.0)*x644);
8096 evalcond[2]=(gconst11+((new_r01*x642)));
8097 evalcond[3]=(((gconst11*x642))+new_r01);
8098 evalcond[4]=((((-1.0)*x641*x643))+new_r00);
8099 evalcond[5]=((((-1.0)*x642*x643))+new_r10);
8100 evalcond[6]=((((-1.0)*new_r10*x641))+((new_r00*x642)));
8101 evalcond[7]=((((-1.0)*x643))+((new_r10*x642))+((new_r00*x641)));
8109 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8110 vinfos[0].jointtype = 1;
8111 vinfos[0].foffset = j0;
8112 vinfos[0].indices[0] = _ij0[0];
8113 vinfos[0].indices[1] = _ij0[1];
8114 vinfos[0].maxsolutions = _nj0;
8115 vinfos[1].jointtype = 1;
8116 vinfos[1].foffset = j1;
8117 vinfos[1].indices[0] = _ij1[0];
8118 vinfos[1].indices[1] = _ij1[1];
8119 vinfos[1].maxsolutions = _nj1;
8120 vinfos[2].jointtype = 1;
8121 vinfos[2].foffset = j2;
8122 vinfos[2].indices[0] = _ij2[0];
8123 vinfos[2].indices[1] = _ij2[1];
8124 vinfos[2].maxsolutions = _nj2;
8125 vinfos[3].jointtype = 1;
8126 vinfos[3].foffset = j3;
8127 vinfos[3].indices[0] = _ij3[0];
8128 vinfos[3].indices[1] = _ij3[1];
8129 vinfos[3].maxsolutions = _nj3;
8130 vinfos[4].jointtype = 1;
8131 vinfos[4].foffset = j4;
8132 vinfos[4].indices[0] = _ij4[0];
8133 vinfos[4].indices[1] = _ij4[1];
8134 vinfos[4].maxsolutions = _nj4;
8135 vinfos[5].jointtype = 1;
8136 vinfos[5].foffset = j5;
8137 vinfos[5].indices[0] = _ij5[0];
8138 vinfos[5].indices[1] = _ij5[1];
8139 vinfos[5].maxsolutions = _nj5;
8140 std::vector<int> vfree(0);
8153 IkReal j3array[1], cj3array[1], sj3array[1];
8154 bool j3valid[1]={
false};
8164 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x645.
value)))+(x646.
value));
8165 sj3array[0]=
IKsin(j3array[0]);
8166 cj3array[0]=
IKcos(j3array[0]);
8167 if( j3array[0] >
IKPI )
8171 else if( j3array[0] < -
IKPI )
8172 { j3array[0]+=
IK2PI;
8175 for(
int ij3 = 0; ij3 < 1; ++ij3)
8181 _ij3[0] = ij3; _ij3[1] = -1;
8182 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8186 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8189 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8192 IkReal x647=
IKcos(j3);
8193 IkReal x648=
IKsin(j3);
8194 IkReal x649=((1.0)*gconst11);
8195 IkReal x650=(gconst11*x647);
8196 evalcond[0]=(new_r01*x647);
8197 evalcond[1]=((-1.0)*x650);
8198 evalcond[2]=(gconst11+((new_r01*x648)));
8199 evalcond[3]=(((gconst11*x648))+new_r01);
8200 evalcond[4]=((((-1.0)*x647*x649))+new_r00);
8201 evalcond[5]=((((-1.0)*x648*x649))+new_r10);
8202 evalcond[6]=((((-1.0)*new_r10*x647))+((new_r00*x648)));
8203 evalcond[7]=((((-1.0)*x649))+((new_r10*x648))+((new_r00*x647)));
8211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8212 vinfos[0].jointtype = 1;
8213 vinfos[0].foffset = j0;
8214 vinfos[0].indices[0] = _ij0[0];
8215 vinfos[0].indices[1] = _ij0[1];
8216 vinfos[0].maxsolutions = _nj0;
8217 vinfos[1].jointtype = 1;
8218 vinfos[1].foffset = j1;
8219 vinfos[1].indices[0] = _ij1[0];
8220 vinfos[1].indices[1] = _ij1[1];
8221 vinfos[1].maxsolutions = _nj1;
8222 vinfos[2].jointtype = 1;
8223 vinfos[2].foffset = j2;
8224 vinfos[2].indices[0] = _ij2[0];
8225 vinfos[2].indices[1] = _ij2[1];
8226 vinfos[2].maxsolutions = _nj2;
8227 vinfos[3].jointtype = 1;
8228 vinfos[3].foffset = j3;
8229 vinfos[3].indices[0] = _ij3[0];
8230 vinfos[3].indices[1] = _ij3[1];
8231 vinfos[3].maxsolutions = _nj3;
8232 vinfos[4].jointtype = 1;
8233 vinfos[4].foffset = j4;
8234 vinfos[4].indices[0] = _ij4[0];
8235 vinfos[4].indices[1] = _ij4[1];
8236 vinfos[4].maxsolutions = _nj4;
8237 vinfos[5].jointtype = 1;
8238 vinfos[5].foffset = j5;
8239 vinfos[5].indices[0] = _ij5[0];
8240 vinfos[5].indices[1] = _ij5[1];
8241 vinfos[5].maxsolutions = _nj5;
8242 std::vector<int> vfree(0);
8254 if( bgotonextstatement )
8256 bool bgotonextstatement =
true;
8261 bgotonextstatement=
false;
8266 if( bgotonextstatement )
8278 IkReal j3array[1], cj3array[1], sj3array[1];
8279 bool j3valid[1]={
false};
8281 IkReal x651=((1.0)*new_r01);
8290 j3array[0]=((-1.5707963267949)+(x652.
value)+(((1.5707963267949)*(x653.
value))));
8291 sj3array[0]=
IKsin(j3array[0]);
8292 cj3array[0]=
IKcos(j3array[0]);
8293 if( j3array[0] >
IKPI )
8297 else if( j3array[0] < -
IKPI )
8298 { j3array[0]+=
IK2PI;
8301 for(
int ij3 = 0; ij3 < 1; ++ij3)
8307 _ij3[0] = ij3; _ij3[1] = -1;
8308 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8312 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8315 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8318 IkReal x654=
IKcos(j3);
8319 IkReal x655=
IKsin(j3);
8320 IkReal x656=((1.0)*gconst11);
8321 IkReal x657=(gconst10*x655);
8322 IkReal x658=((1.0)*x654);
8323 IkReal x659=(x654*x656);
8324 evalcond[0]=(gconst10+((new_r01*x654))+((new_r11*x655)));
8325 evalcond[1]=(((gconst10*x654))+((gconst11*x655))+new_r01);
8326 evalcond[2]=(gconst10+(((-1.0)*new_r10*x658))+((new_r00*x655)));
8327 evalcond[3]=(gconst11+(((-1.0)*new_r11*x658))+((new_r01*x655)));
8328 evalcond[4]=((((-1.0)*x659))+x657+new_r00);
8329 evalcond[5]=((((-1.0)*x659))+x657+new_r11);
8330 evalcond[6]=((((-1.0)*x656))+((new_r10*x655))+((new_r00*x654)));
8331 evalcond[7]=((((-1.0)*gconst10*x658))+(((-1.0)*x655*x656))+new_r10);
8339 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8340 vinfos[0].jointtype = 1;
8341 vinfos[0].foffset = j0;
8342 vinfos[0].indices[0] = _ij0[0];
8343 vinfos[0].indices[1] = _ij0[1];
8344 vinfos[0].maxsolutions = _nj0;
8345 vinfos[1].jointtype = 1;
8346 vinfos[1].foffset = j1;
8347 vinfos[1].indices[0] = _ij1[0];
8348 vinfos[1].indices[1] = _ij1[1];
8349 vinfos[1].maxsolutions = _nj1;
8350 vinfos[2].jointtype = 1;
8351 vinfos[2].foffset = j2;
8352 vinfos[2].indices[0] = _ij2[0];
8353 vinfos[2].indices[1] = _ij2[1];
8354 vinfos[2].maxsolutions = _nj2;
8355 vinfos[3].jointtype = 1;
8356 vinfos[3].foffset = j3;
8357 vinfos[3].indices[0] = _ij3[0];
8358 vinfos[3].indices[1] = _ij3[1];
8359 vinfos[3].maxsolutions = _nj3;
8360 vinfos[4].jointtype = 1;
8361 vinfos[4].foffset = j4;
8362 vinfos[4].indices[0] = _ij4[0];
8363 vinfos[4].indices[1] = _ij4[1];
8364 vinfos[4].maxsolutions = _nj4;
8365 vinfos[5].jointtype = 1;
8366 vinfos[5].foffset = j5;
8367 vinfos[5].indices[0] = _ij5[0];
8368 vinfos[5].indices[1] = _ij5[1];
8369 vinfos[5].maxsolutions = _nj5;
8370 std::vector<int> vfree(0);
8383 IkReal j3array[1], cj3array[1], sj3array[1];
8384 bool j3valid[1]={
false};
8386 IkReal x660=((1.0)*gconst10);
8395 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x661.
value)))+(x662.
value));
8396 sj3array[0]=
IKsin(j3array[0]);
8397 cj3array[0]=
IKcos(j3array[0]);
8398 if( j3array[0] >
IKPI )
8402 else if( j3array[0] < -
IKPI )
8403 { j3array[0]+=
IK2PI;
8406 for(
int ij3 = 0; ij3 < 1; ++ij3)
8412 _ij3[0] = ij3; _ij3[1] = -1;
8413 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8417 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8420 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8423 IkReal x663=
IKcos(j3);
8424 IkReal x664=
IKsin(j3);
8425 IkReal x665=((1.0)*gconst11);
8426 IkReal x666=(gconst10*x664);
8427 IkReal x667=((1.0)*x663);
8428 IkReal x668=(x663*x665);
8429 evalcond[0]=(gconst10+((new_r11*x664))+((new_r01*x663)));
8430 evalcond[1]=(new_r01+((gconst10*x663))+((gconst11*x664)));
8431 evalcond[2]=(gconst10+(((-1.0)*new_r10*x667))+((new_r00*x664)));
8432 evalcond[3]=((((-1.0)*new_r11*x667))+gconst11+((new_r01*x664)));
8433 evalcond[4]=((((-1.0)*x668))+x666+new_r00);
8434 evalcond[5]=((((-1.0)*x668))+x666+new_r11);
8435 evalcond[6]=(((new_r10*x664))+(((-1.0)*x665))+((new_r00*x663)));
8436 evalcond[7]=((((-1.0)*gconst10*x667))+new_r10+(((-1.0)*x664*x665)));
8444 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8445 vinfos[0].jointtype = 1;
8446 vinfos[0].foffset = j0;
8447 vinfos[0].indices[0] = _ij0[0];
8448 vinfos[0].indices[1] = _ij0[1];
8449 vinfos[0].maxsolutions = _nj0;
8450 vinfos[1].jointtype = 1;
8451 vinfos[1].foffset = j1;
8452 vinfos[1].indices[0] = _ij1[0];
8453 vinfos[1].indices[1] = _ij1[1];
8454 vinfos[1].maxsolutions = _nj1;
8455 vinfos[2].jointtype = 1;
8456 vinfos[2].foffset = j2;
8457 vinfos[2].indices[0] = _ij2[0];
8458 vinfos[2].indices[1] = _ij2[1];
8459 vinfos[2].maxsolutions = _nj2;
8460 vinfos[3].jointtype = 1;
8461 vinfos[3].foffset = j3;
8462 vinfos[3].indices[0] = _ij3[0];
8463 vinfos[3].indices[1] = _ij3[1];
8464 vinfos[3].maxsolutions = _nj3;
8465 vinfos[4].jointtype = 1;
8466 vinfos[4].foffset = j4;
8467 vinfos[4].indices[0] = _ij4[0];
8468 vinfos[4].indices[1] = _ij4[1];
8469 vinfos[4].maxsolutions = _nj4;
8470 vinfos[5].jointtype = 1;
8471 vinfos[5].foffset = j5;
8472 vinfos[5].indices[0] = _ij5[0];
8473 vinfos[5].indices[1] = _ij5[1];
8474 vinfos[5].maxsolutions = _nj5;
8475 std::vector<int> vfree(0);
8488 IkReal j3array[1], cj3array[1], sj3array[1];
8489 bool j3valid[1]={
false};
8491 IkReal x669=((1.0)*new_r11);
8500 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x670.
value)))+(x671.
value));
8501 sj3array[0]=
IKsin(j3array[0]);
8502 cj3array[0]=
IKcos(j3array[0]);
8503 if( j3array[0] >
IKPI )
8507 else if( j3array[0] < -
IKPI )
8508 { j3array[0]+=
IK2PI;
8511 for(
int ij3 = 0; ij3 < 1; ++ij3)
8517 _ij3[0] = ij3; _ij3[1] = -1;
8518 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8522 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8525 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8528 IkReal x672=
IKcos(j3);
8529 IkReal x673=
IKsin(j3);
8530 IkReal x674=((1.0)*gconst11);
8531 IkReal x675=(gconst10*x673);
8532 IkReal x676=((1.0)*x672);
8533 IkReal x677=(x672*x674);
8534 evalcond[0]=(((new_r11*x673))+((new_r01*x672))+gconst10);
8535 evalcond[1]=(((gconst11*x673))+((gconst10*x672))+new_r01);
8536 evalcond[2]=(((new_r00*x673))+gconst10+(((-1.0)*new_r10*x676)));
8537 evalcond[3]=(((new_r01*x673))+gconst11+(((-1.0)*new_r11*x676)));
8538 evalcond[4]=(x675+new_r00+(((-1.0)*x677)));
8539 evalcond[5]=(x675+new_r11+(((-1.0)*x677)));
8540 evalcond[6]=(((new_r00*x672))+((new_r10*x673))+(((-1.0)*x674)));
8541 evalcond[7]=(new_r10+(((-1.0)*x673*x674))+(((-1.0)*gconst10*x676)));
8549 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8550 vinfos[0].jointtype = 1;
8551 vinfos[0].foffset = j0;
8552 vinfos[0].indices[0] = _ij0[0];
8553 vinfos[0].indices[1] = _ij0[1];
8554 vinfos[0].maxsolutions = _nj0;
8555 vinfos[1].jointtype = 1;
8556 vinfos[1].foffset = j1;
8557 vinfos[1].indices[0] = _ij1[0];
8558 vinfos[1].indices[1] = _ij1[1];
8559 vinfos[1].maxsolutions = _nj1;
8560 vinfos[2].jointtype = 1;
8561 vinfos[2].foffset = j2;
8562 vinfos[2].indices[0] = _ij2[0];
8563 vinfos[2].indices[1] = _ij2[1];
8564 vinfos[2].maxsolutions = _nj2;
8565 vinfos[3].jointtype = 1;
8566 vinfos[3].foffset = j3;
8567 vinfos[3].indices[0] = _ij3[0];
8568 vinfos[3].indices[1] = _ij3[1];
8569 vinfos[3].maxsolutions = _nj3;
8570 vinfos[4].jointtype = 1;
8571 vinfos[4].foffset = j4;
8572 vinfos[4].indices[0] = _ij4[0];
8573 vinfos[4].indices[1] = _ij4[1];
8574 vinfos[4].maxsolutions = _nj4;
8575 vinfos[5].jointtype = 1;
8576 vinfos[5].foffset = j5;
8577 vinfos[5].indices[0] = _ij5[0];
8578 vinfos[5].indices[1] = _ij5[1];
8579 vinfos[5].maxsolutions = _nj5;
8580 std::vector<int> vfree(0);
8592 if( bgotonextstatement )
8594 bool bgotonextstatement =
true;
8597 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
8598 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8600 bgotonextstatement=
false;
8602 IkReal j3array[1], cj3array[1], sj3array[1];
8603 bool j3valid[1]={
false};
8607 j3array[0]=
IKatan2(((-1.0)*new_r01), new_r00);
8608 sj3array[0]=
IKsin(j3array[0]);
8609 cj3array[0]=
IKcos(j3array[0]);
8610 if( j3array[0] >
IKPI )
8614 else if( j3array[0] < -
IKPI )
8615 { j3array[0]+=
IK2PI;
8618 for(
int ij3 = 0; ij3 < 1; ++ij3)
8624 _ij3[0] = ij3; _ij3[1] = -1;
8625 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8629 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8632 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8635 IkReal x678=
IKsin(j3);
8636 IkReal x679=
IKcos(j3);
8637 IkReal x680=((1.0)*x679);
8638 evalcond[0]=(x678+new_r01);
8639 evalcond[1]=((((-1.0)*x680))+new_r00);
8640 evalcond[2]=((((-1.0)*x680))+new_r11);
8641 evalcond[3]=((((-1.0)*x678))+new_r10);
8642 evalcond[4]=(((new_r11*x678))+((new_r01*x679)));
8643 evalcond[5]=((((-1.0)*new_r10*x680))+((new_r00*x678)));
8644 evalcond[6]=((-1.0)+((new_r00*x679))+((new_r10*x678)));
8645 evalcond[7]=((1.0)+((new_r01*x678))+(((-1.0)*new_r11*x680)));
8653 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8654 vinfos[0].jointtype = 1;
8655 vinfos[0].foffset = j0;
8656 vinfos[0].indices[0] = _ij0[0];
8657 vinfos[0].indices[1] = _ij0[1];
8658 vinfos[0].maxsolutions = _nj0;
8659 vinfos[1].jointtype = 1;
8660 vinfos[1].foffset = j1;
8661 vinfos[1].indices[0] = _ij1[0];
8662 vinfos[1].indices[1] = _ij1[1];
8663 vinfos[1].maxsolutions = _nj1;
8664 vinfos[2].jointtype = 1;
8665 vinfos[2].foffset = j2;
8666 vinfos[2].indices[0] = _ij2[0];
8667 vinfos[2].indices[1] = _ij2[1];
8668 vinfos[2].maxsolutions = _nj2;
8669 vinfos[3].jointtype = 1;
8670 vinfos[3].foffset = j3;
8671 vinfos[3].indices[0] = _ij3[0];
8672 vinfos[3].indices[1] = _ij3[1];
8673 vinfos[3].maxsolutions = _nj3;
8674 vinfos[4].jointtype = 1;
8675 vinfos[4].foffset = j4;
8676 vinfos[4].indices[0] = _ij4[0];
8677 vinfos[4].indices[1] = _ij4[1];
8678 vinfos[4].maxsolutions = _nj4;
8679 vinfos[5].jointtype = 1;
8680 vinfos[5].foffset = j5;
8681 vinfos[5].indices[0] = _ij5[0];
8682 vinfos[5].indices[1] = _ij5[1];
8683 vinfos[5].maxsolutions = _nj5;
8684 std::vector<int> vfree(0);
8692 if( bgotonextstatement )
8694 bool bgotonextstatement =
true;
8697 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8698 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8700 bgotonextstatement=
false;
8702 IkReal j3array[1], cj3array[1], sj3array[1];
8703 bool j3valid[1]={
false};
8707 j3array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8708 sj3array[0]=
IKsin(j3array[0]);
8709 cj3array[0]=
IKcos(j3array[0]);
8710 if( j3array[0] >
IKPI )
8714 else if( j3array[0] < -
IKPI )
8715 { j3array[0]+=
IK2PI;
8718 for(
int ij3 = 0; ij3 < 1; ++ij3)
8724 _ij3[0] = ij3; _ij3[1] = -1;
8725 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
8729 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8732 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8735 IkReal x681=
IKcos(j3);
8736 IkReal x682=
IKsin(j3);
8737 IkReal x683=((1.0)*x681);
8738 evalcond[0]=(x681+new_r00);
8739 evalcond[1]=(x681+new_r11);
8740 evalcond[2]=(x682+new_r10);
8741 evalcond[3]=(new_r01+(((-1.0)*x682)));
8742 evalcond[4]=(((new_r01*x681))+((new_r11*x682)));
8743 evalcond[5]=((((-1.0)*new_r10*x683))+((new_r00*x682)));
8744 evalcond[6]=((1.0)+((new_r00*x681))+((new_r10*x682)));
8745 evalcond[7]=((-1.0)+((new_r01*x682))+(((-1.0)*new_r11*x683)));
8753 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8754 vinfos[0].jointtype = 1;
8755 vinfos[0].foffset = j0;
8756 vinfos[0].indices[0] = _ij0[0];
8757 vinfos[0].indices[1] = _ij0[1];
8758 vinfos[0].maxsolutions = _nj0;
8759 vinfos[1].jointtype = 1;
8760 vinfos[1].foffset = j1;
8761 vinfos[1].indices[0] = _ij1[0];
8762 vinfos[1].indices[1] = _ij1[1];
8763 vinfos[1].maxsolutions = _nj1;
8764 vinfos[2].jointtype = 1;
8765 vinfos[2].foffset = j2;
8766 vinfos[2].indices[0] = _ij2[0];
8767 vinfos[2].indices[1] = _ij2[1];
8768 vinfos[2].maxsolutions = _nj2;
8769 vinfos[3].jointtype = 1;
8770 vinfos[3].foffset = j3;
8771 vinfos[3].indices[0] = _ij3[0];
8772 vinfos[3].indices[1] = _ij3[1];
8773 vinfos[3].maxsolutions = _nj3;
8774 vinfos[4].jointtype = 1;
8775 vinfos[4].foffset = j4;
8776 vinfos[4].indices[0] = _ij4[0];
8777 vinfos[4].indices[1] = _ij4[1];
8778 vinfos[4].maxsolutions = _nj4;
8779 vinfos[5].jointtype = 1;
8780 vinfos[5].foffset = j5;
8781 vinfos[5].indices[0] = _ij5[0];
8782 vinfos[5].indices[1] = _ij5[1];
8783 vinfos[5].maxsolutions = _nj5;
8784 std::vector<int> vfree(0);
8792 if( bgotonextstatement )
8794 bool bgotonextstatement =
true;
8797 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
8798 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8800 bgotonextstatement=
false;
8809 j3eval[1]=
IKsign(new_r01);
8811 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
8822 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
8833 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
8837 bool bgotonextstatement =
true;
8840 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
8841 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8843 bgotonextstatement=
false;
8845 IkReal j3array[2], cj3array[2], sj3array[2];
8846 bool j3valid[2]={
false};
8848 sj3array[0]=new_r10;
8851 j3valid[0] = j3valid[1] =
true;
8852 j3array[0] =
IKasin(sj3array[0]);
8853 cj3array[0] =
IKcos(j3array[0]);
8854 sj3array[1] = sj3array[0];
8855 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
8856 cj3array[1] = -cj3array[0];
8858 else if( isnan(sj3array[0]) )
8862 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8864 for(
int ij3 = 0; ij3 < 2; ++ij3)
8870 _ij3[0] = ij3; _ij3[1] = -1;
8871 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
8875 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8878 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8881 IkReal x684=
IKcos(j3);
8882 IkReal x685=
IKsin(j3);
8883 IkReal x686=((-1.0)*x684);
8884 evalcond[0]=(new_r01*x684);
8885 evalcond[1]=(x685+new_r01);
8887 evalcond[3]=(new_r10*x686);
8888 evalcond[4]=((1.0)+((new_r01*x685)));
8889 evalcond[5]=((-1.0)+((new_r10*x685)));
8897 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8898 vinfos[0].jointtype = 1;
8899 vinfos[0].foffset = j0;
8900 vinfos[0].indices[0] = _ij0[0];
8901 vinfos[0].indices[1] = _ij0[1];
8902 vinfos[0].maxsolutions = _nj0;
8903 vinfos[1].jointtype = 1;
8904 vinfos[1].foffset = j1;
8905 vinfos[1].indices[0] = _ij1[0];
8906 vinfos[1].indices[1] = _ij1[1];
8907 vinfos[1].maxsolutions = _nj1;
8908 vinfos[2].jointtype = 1;
8909 vinfos[2].foffset = j2;
8910 vinfos[2].indices[0] = _ij2[0];
8911 vinfos[2].indices[1] = _ij2[1];
8912 vinfos[2].maxsolutions = _nj2;
8913 vinfos[3].jointtype = 1;
8914 vinfos[3].foffset = j3;
8915 vinfos[3].indices[0] = _ij3[0];
8916 vinfos[3].indices[1] = _ij3[1];
8917 vinfos[3].maxsolutions = _nj3;
8918 vinfos[4].jointtype = 1;
8919 vinfos[4].foffset = j4;
8920 vinfos[4].indices[0] = _ij4[0];
8921 vinfos[4].indices[1] = _ij4[1];
8922 vinfos[4].maxsolutions = _nj4;
8923 vinfos[5].jointtype = 1;
8924 vinfos[5].foffset = j5;
8925 vinfos[5].indices[0] = _ij5[0];
8926 vinfos[5].indices[1] = _ij5[1];
8927 vinfos[5].maxsolutions = _nj5;
8928 std::vector<int> vfree(0);
8936 if( bgotonextstatement )
8938 bool bgotonextstatement =
true;
8941 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8942 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8944 bgotonextstatement=
false;
8946 IkReal j3array[2], cj3array[2], sj3array[2];
8947 bool j3valid[2]={
false};
8949 sj3array[0]=new_r01;
8952 j3valid[0] = j3valid[1] =
true;
8953 j3array[0] =
IKasin(sj3array[0]);
8954 cj3array[0] =
IKcos(j3array[0]);
8955 sj3array[1] = sj3array[0];
8956 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
8957 cj3array[1] = -cj3array[0];
8959 else if( isnan(sj3array[0]) )
8963 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8965 for(
int ij3 = 0; ij3 < 2; ++ij3)
8971 _ij3[0] = ij3; _ij3[1] = -1;
8972 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
8976 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
8979 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8982 IkReal x687=
IKcos(j3);
8983 IkReal x688=
IKsin(j3);
8985 evalcond[1]=(new_r01*x687);
8986 evalcond[2]=(x688+new_r10);
8987 evalcond[3]=((-1.0)*new_r10*x687);
8988 evalcond[4]=((-1.0)+((new_r01*x688)));
8989 evalcond[5]=((1.0)+((new_r10*x688)));
8997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8998 vinfos[0].jointtype = 1;
8999 vinfos[0].foffset = j0;
9000 vinfos[0].indices[0] = _ij0[0];
9001 vinfos[0].indices[1] = _ij0[1];
9002 vinfos[0].maxsolutions = _nj0;
9003 vinfos[1].jointtype = 1;
9004 vinfos[1].foffset = j1;
9005 vinfos[1].indices[0] = _ij1[0];
9006 vinfos[1].indices[1] = _ij1[1];
9007 vinfos[1].maxsolutions = _nj1;
9008 vinfos[2].jointtype = 1;
9009 vinfos[2].foffset = j2;
9010 vinfos[2].indices[0] = _ij2[0];
9011 vinfos[2].indices[1] = _ij2[1];
9012 vinfos[2].maxsolutions = _nj2;
9013 vinfos[3].jointtype = 1;
9014 vinfos[3].foffset = j3;
9015 vinfos[3].indices[0] = _ij3[0];
9016 vinfos[3].indices[1] = _ij3[1];
9017 vinfos[3].maxsolutions = _nj3;
9018 vinfos[4].jointtype = 1;
9019 vinfos[4].foffset = j4;
9020 vinfos[4].indices[0] = _ij4[0];
9021 vinfos[4].indices[1] = _ij4[1];
9022 vinfos[4].maxsolutions = _nj4;
9023 vinfos[5].jointtype = 1;
9024 vinfos[5].foffset = j5;
9025 vinfos[5].indices[0] = _ij5[0];
9026 vinfos[5].indices[1] = _ij5[1];
9027 vinfos[5].maxsolutions = _nj5;
9028 std::vector<int> vfree(0);
9036 if( bgotonextstatement )
9038 bool bgotonextstatement =
true;
9043 bgotonextstatement=
false;
9048 if( bgotonextstatement )
9058 IkReal j3array[1], cj3array[1], sj3array[1];
9059 bool j3valid[1]={
false};
9065 IkReal x689=x690.
value;
9076 j3array[0]=
IKatan2(((-1.0)*cj5*x689), (x689*(x691.
value)*(((((-1.0)*(x692.
value)))+(cj5*cj5)))));
9077 sj3array[0]=
IKsin(j3array[0]);
9078 cj3array[0]=
IKcos(j3array[0]);
9079 if( j3array[0] >
IKPI )
9083 else if( j3array[0] < -
IKPI )
9084 { j3array[0]+=
IK2PI;
9087 for(
int ij3 = 0; ij3 < 1; ++ij3)
9093 _ij3[0] = ij3; _ij3[1] = -1;
9094 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9098 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9101 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9104 IkReal x693=
IKcos(j3);
9105 IkReal x694=
IKsin(j3);
9106 IkReal x695=((1.0)*cj5);
9107 IkReal x696=(sj5*x693);
9108 evalcond[0]=(cj5+((new_r01*x694)));
9109 evalcond[1]=(sj5+((new_r01*x693)));
9110 evalcond[2]=(sj5+(((-1.0)*new_r10*x693)));
9111 evalcond[3]=(((new_r10*x694))+(((-1.0)*x695)));
9112 evalcond[4]=(x696+new_r01+((cj5*x694)));
9113 evalcond[5]=((((-1.0)*x693*x695))+((sj5*x694)));
9114 evalcond[6]=((((-1.0)*x694*x695))+new_r10+(((-1.0)*x696)));
9122 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9123 vinfos[0].jointtype = 1;
9124 vinfos[0].foffset = j0;
9125 vinfos[0].indices[0] = _ij0[0];
9126 vinfos[0].indices[1] = _ij0[1];
9127 vinfos[0].maxsolutions = _nj0;
9128 vinfos[1].jointtype = 1;
9129 vinfos[1].foffset = j1;
9130 vinfos[1].indices[0] = _ij1[0];
9131 vinfos[1].indices[1] = _ij1[1];
9132 vinfos[1].maxsolutions = _nj1;
9133 vinfos[2].jointtype = 1;
9134 vinfos[2].foffset = j2;
9135 vinfos[2].indices[0] = _ij2[0];
9136 vinfos[2].indices[1] = _ij2[1];
9137 vinfos[2].maxsolutions = _nj2;
9138 vinfos[3].jointtype = 1;
9139 vinfos[3].foffset = j3;
9140 vinfos[3].indices[0] = _ij3[0];
9141 vinfos[3].indices[1] = _ij3[1];
9142 vinfos[3].maxsolutions = _nj3;
9143 vinfos[4].jointtype = 1;
9144 vinfos[4].foffset = j4;
9145 vinfos[4].indices[0] = _ij4[0];
9146 vinfos[4].indices[1] = _ij4[1];
9147 vinfos[4].maxsolutions = _nj4;
9148 vinfos[5].jointtype = 1;
9149 vinfos[5].foffset = j5;
9150 vinfos[5].indices[0] = _ij5[0];
9151 vinfos[5].indices[1] = _ij5[1];
9152 vinfos[5].maxsolutions = _nj5;
9153 std::vector<int> vfree(0);
9166 IkReal j3array[1], cj3array[1], sj3array[1];
9167 bool j3valid[1]={
false};
9180 sj3array[0]=
IKsin(j3array[0]);
9181 cj3array[0]=
IKcos(j3array[0]);
9182 if( j3array[0] >
IKPI )
9186 else if( j3array[0] < -
IKPI )
9187 { j3array[0]+=
IK2PI;
9190 for(
int ij3 = 0; ij3 < 1; ++ij3)
9196 _ij3[0] = ij3; _ij3[1] = -1;
9197 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9201 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9204 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9207 IkReal x699=
IKcos(j3);
9208 IkReal x700=
IKsin(j3);
9209 IkReal x701=((1.0)*cj5);
9210 IkReal x702=(sj5*x699);
9211 evalcond[0]=(cj5+((new_r01*x700)));
9212 evalcond[1]=(sj5+((new_r01*x699)));
9213 evalcond[2]=(sj5+(((-1.0)*new_r10*x699)));
9214 evalcond[3]=((((-1.0)*x701))+((new_r10*x700)));
9215 evalcond[4]=(((cj5*x700))+x702+new_r01);
9216 evalcond[5]=(((sj5*x700))+(((-1.0)*x699*x701)));
9217 evalcond[6]=((((-1.0)*x702))+new_r10+(((-1.0)*x700*x701)));
9225 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9226 vinfos[0].jointtype = 1;
9227 vinfos[0].foffset = j0;
9228 vinfos[0].indices[0] = _ij0[0];
9229 vinfos[0].indices[1] = _ij0[1];
9230 vinfos[0].maxsolutions = _nj0;
9231 vinfos[1].jointtype = 1;
9232 vinfos[1].foffset = j1;
9233 vinfos[1].indices[0] = _ij1[0];
9234 vinfos[1].indices[1] = _ij1[1];
9235 vinfos[1].maxsolutions = _nj1;
9236 vinfos[2].jointtype = 1;
9237 vinfos[2].foffset = j2;
9238 vinfos[2].indices[0] = _ij2[0];
9239 vinfos[2].indices[1] = _ij2[1];
9240 vinfos[2].maxsolutions = _nj2;
9241 vinfos[3].jointtype = 1;
9242 vinfos[3].foffset = j3;
9243 vinfos[3].indices[0] = _ij3[0];
9244 vinfos[3].indices[1] = _ij3[1];
9245 vinfos[3].maxsolutions = _nj3;
9246 vinfos[4].jointtype = 1;
9247 vinfos[4].foffset = j4;
9248 vinfos[4].indices[0] = _ij4[0];
9249 vinfos[4].indices[1] = _ij4[1];
9250 vinfos[4].maxsolutions = _nj4;
9251 vinfos[5].jointtype = 1;
9252 vinfos[5].foffset = j5;
9253 vinfos[5].indices[0] = _ij5[0];
9254 vinfos[5].indices[1] = _ij5[1];
9255 vinfos[5].maxsolutions = _nj5;
9256 std::vector<int> vfree(0);
9269 IkReal j3array[1], cj3array[1], sj3array[1];
9270 bool j3valid[1]={
false};
9280 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x703.
value)))+(x704.
value));
9281 sj3array[0]=
IKsin(j3array[0]);
9282 cj3array[0]=
IKcos(j3array[0]);
9283 if( j3array[0] >
IKPI )
9287 else if( j3array[0] < -
IKPI )
9288 { j3array[0]+=
IK2PI;
9291 for(
int ij3 = 0; ij3 < 1; ++ij3)
9297 _ij3[0] = ij3; _ij3[1] = -1;
9298 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9302 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9305 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9308 IkReal x705=
IKcos(j3);
9309 IkReal x706=
IKsin(j3);
9310 IkReal x707=((1.0)*cj5);
9311 IkReal x708=(sj5*x705);
9312 evalcond[0]=(cj5+((new_r01*x706)));
9313 evalcond[1]=(sj5+((new_r01*x705)));
9314 evalcond[2]=(sj5+(((-1.0)*new_r10*x705)));
9315 evalcond[3]=((((-1.0)*x707))+((new_r10*x706)));
9316 evalcond[4]=(((cj5*x706))+x708+new_r01);
9317 evalcond[5]=(((sj5*x706))+(((-1.0)*x705*x707)));
9318 evalcond[6]=((((-1.0)*x706*x707))+(((-1.0)*x708))+new_r10);
9326 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9327 vinfos[0].jointtype = 1;
9328 vinfos[0].foffset = j0;
9329 vinfos[0].indices[0] = _ij0[0];
9330 vinfos[0].indices[1] = _ij0[1];
9331 vinfos[0].maxsolutions = _nj0;
9332 vinfos[1].jointtype = 1;
9333 vinfos[1].foffset = j1;
9334 vinfos[1].indices[0] = _ij1[0];
9335 vinfos[1].indices[1] = _ij1[1];
9336 vinfos[1].maxsolutions = _nj1;
9337 vinfos[2].jointtype = 1;
9338 vinfos[2].foffset = j2;
9339 vinfos[2].indices[0] = _ij2[0];
9340 vinfos[2].indices[1] = _ij2[1];
9341 vinfos[2].maxsolutions = _nj2;
9342 vinfos[3].jointtype = 1;
9343 vinfos[3].foffset = j3;
9344 vinfos[3].indices[0] = _ij3[0];
9345 vinfos[3].indices[1] = _ij3[1];
9346 vinfos[3].maxsolutions = _nj3;
9347 vinfos[4].jointtype = 1;
9348 vinfos[4].foffset = j4;
9349 vinfos[4].indices[0] = _ij4[0];
9350 vinfos[4].indices[1] = _ij4[1];
9351 vinfos[4].maxsolutions = _nj4;
9352 vinfos[5].jointtype = 1;
9353 vinfos[5].foffset = j5;
9354 vinfos[5].indices[0] = _ij5[0];
9355 vinfos[5].indices[1] = _ij5[1];
9356 vinfos[5].maxsolutions = _nj5;
9357 std::vector<int> vfree(0);
9369 if( bgotonextstatement )
9371 bool bgotonextstatement =
true;
9374 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
9375 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9377 bgotonextstatement=
false;
9387 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
9388 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
9395 IkReal j3array[2], cj3array[2], sj3array[2];
9396 bool j3valid[2]={
false};
9402 IkReal x709=x710.
value;
9403 j3array[0]=((-1.0)*x709);
9404 sj3array[0]=
IKsin(j3array[0]);
9405 cj3array[0]=
IKcos(j3array[0]);
9406 j3array[1]=((3.14159265358979)+(((-1.0)*x709)));
9407 sj3array[1]=
IKsin(j3array[1]);
9408 cj3array[1]=
IKcos(j3array[1]);
9409 if( j3array[0] >
IKPI )
9413 else if( j3array[0] < -
IKPI )
9414 { j3array[0]+=
IK2PI;
9417 if( j3array[1] >
IKPI )
9421 else if( j3array[1] < -
IKPI )
9422 { j3array[1]+=
IK2PI;
9425 for(
int ij3 = 0; ij3 < 2; ++ij3)
9431 _ij3[0] = ij3; _ij3[1] = -1;
9432 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
9436 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9439 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9442 evalcond[0]=(((new_r00*(
IKsin(j3))))+(((-1.0)*new_r10*(
IKcos(j3)))));
9450 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9451 vinfos[0].jointtype = 1;
9452 vinfos[0].foffset = j0;
9453 vinfos[0].indices[0] = _ij0[0];
9454 vinfos[0].indices[1] = _ij0[1];
9455 vinfos[0].maxsolutions = _nj0;
9456 vinfos[1].jointtype = 1;
9457 vinfos[1].foffset = j1;
9458 vinfos[1].indices[0] = _ij1[0];
9459 vinfos[1].indices[1] = _ij1[1];
9460 vinfos[1].maxsolutions = _nj1;
9461 vinfos[2].jointtype = 1;
9462 vinfos[2].foffset = j2;
9463 vinfos[2].indices[0] = _ij2[0];
9464 vinfos[2].indices[1] = _ij2[1];
9465 vinfos[2].maxsolutions = _nj2;
9466 vinfos[3].jointtype = 1;
9467 vinfos[3].foffset = j3;
9468 vinfos[3].indices[0] = _ij3[0];
9469 vinfos[3].indices[1] = _ij3[1];
9470 vinfos[3].maxsolutions = _nj3;
9471 vinfos[4].jointtype = 1;
9472 vinfos[4].foffset = j4;
9473 vinfos[4].indices[0] = _ij4[0];
9474 vinfos[4].indices[1] = _ij4[1];
9475 vinfos[4].maxsolutions = _nj4;
9476 vinfos[5].jointtype = 1;
9477 vinfos[5].foffset = j5;
9478 vinfos[5].indices[0] = _ij5[0];
9479 vinfos[5].indices[1] = _ij5[1];
9480 vinfos[5].maxsolutions = _nj5;
9481 std::vector<int> vfree(0);
9493 if( bgotonextstatement )
9495 bool bgotonextstatement =
true;
9498 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
9499 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9501 bgotonextstatement=
false;
9511 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
9512 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
9519 IkReal j3array[2], cj3array[2], sj3array[2];
9520 bool j3valid[2]={
false};
9526 IkReal x711=x712.
value;
9527 j3array[0]=((-1.0)*x711);
9528 sj3array[0]=
IKsin(j3array[0]);
9529 cj3array[0]=
IKcos(j3array[0]);
9530 j3array[1]=((3.14159265358979)+(((-1.0)*x711)));
9531 sj3array[1]=
IKsin(j3array[1]);
9532 cj3array[1]=
IKcos(j3array[1]);
9533 if( j3array[0] >
IKPI )
9537 else if( j3array[0] < -
IKPI )
9538 { j3array[0]+=
IK2PI;
9541 if( j3array[1] >
IKPI )
9545 else if( j3array[1] < -
IKPI )
9546 { j3array[1]+=
IK2PI;
9549 for(
int ij3 = 0; ij3 < 2; ++ij3)
9555 _ij3[0] = ij3; _ij3[1] = -1;
9556 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
9560 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9563 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9566 evalcond[0]=(((new_r01*(
IKsin(j3))))+(((-1.0)*new_r11*(
IKcos(j3)))));
9574 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9575 vinfos[0].jointtype = 1;
9576 vinfos[0].foffset = j0;
9577 vinfos[0].indices[0] = _ij0[0];
9578 vinfos[0].indices[1] = _ij0[1];
9579 vinfos[0].maxsolutions = _nj0;
9580 vinfos[1].jointtype = 1;
9581 vinfos[1].foffset = j1;
9582 vinfos[1].indices[0] = _ij1[0];
9583 vinfos[1].indices[1] = _ij1[1];
9584 vinfos[1].maxsolutions = _nj1;
9585 vinfos[2].jointtype = 1;
9586 vinfos[2].foffset = j2;
9587 vinfos[2].indices[0] = _ij2[0];
9588 vinfos[2].indices[1] = _ij2[1];
9589 vinfos[2].maxsolutions = _nj2;
9590 vinfos[3].jointtype = 1;
9591 vinfos[3].foffset = j3;
9592 vinfos[3].indices[0] = _ij3[0];
9593 vinfos[3].indices[1] = _ij3[1];
9594 vinfos[3].maxsolutions = _nj3;
9595 vinfos[4].jointtype = 1;
9596 vinfos[4].foffset = j4;
9597 vinfos[4].indices[0] = _ij4[0];
9598 vinfos[4].indices[1] = _ij4[1];
9599 vinfos[4].maxsolutions = _nj4;
9600 vinfos[5].jointtype = 1;
9601 vinfos[5].foffset = j5;
9602 vinfos[5].indices[0] = _ij5[0];
9603 vinfos[5].indices[1] = _ij5[1];
9604 vinfos[5].maxsolutions = _nj5;
9605 std::vector<int> vfree(0);
9617 if( bgotonextstatement )
9619 bool bgotonextstatement =
true;
9622 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
9623 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9625 bgotonextstatement=
false;
9634 j3eval[1]=
IKsign(new_r11);
9636 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
9647 j3eval[2]=
IKsign(new_r00);
9648 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
9659 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
9666 IkReal j3array[1], cj3array[1], sj3array[1];
9667 bool j3valid[1]={
false};
9680 sj3array[0]=
IKsin(j3array[0]);
9681 cj3array[0]=
IKcos(j3array[0]);
9682 if( j3array[0] >
IKPI )
9686 else if( j3array[0] < -
IKPI )
9687 { j3array[0]+=
IK2PI;
9690 for(
int ij3 = 0; ij3 < 1; ++ij3)
9696 _ij3[0] = ij3; _ij3[1] = -1;
9697 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9701 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9707 IkReal x715=
IKsin(j3);
9708 IkReal x716=
IKcos(j3);
9709 IkReal x717=(sj5*x715);
9710 IkReal x718=((1.0)*x716);
9711 IkReal x719=(cj5*x718);
9712 evalcond[0]=(sj5+((new_r00*x715)));
9713 evalcond[1]=(sj5+((new_r11*x715)));
9714 evalcond[2]=(cj5+(((-1.0)*new_r11*x718)));
9715 evalcond[3]=(((new_r00*x716))+(((-1.0)*cj5)));
9716 evalcond[4]=(((cj5*x715))+((sj5*x716)));
9717 evalcond[5]=((((-1.0)*x719))+x717+new_r00);
9718 evalcond[6]=((((-1.0)*x719))+x717+new_r11);
9726 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9727 vinfos[0].jointtype = 1;
9728 vinfos[0].foffset = j0;
9729 vinfos[0].indices[0] = _ij0[0];
9730 vinfos[0].indices[1] = _ij0[1];
9731 vinfos[0].maxsolutions = _nj0;
9732 vinfos[1].jointtype = 1;
9733 vinfos[1].foffset = j1;
9734 vinfos[1].indices[0] = _ij1[0];
9735 vinfos[1].indices[1] = _ij1[1];
9736 vinfos[1].maxsolutions = _nj1;
9737 vinfos[2].jointtype = 1;
9738 vinfos[2].foffset = j2;
9739 vinfos[2].indices[0] = _ij2[0];
9740 vinfos[2].indices[1] = _ij2[1];
9741 vinfos[2].maxsolutions = _nj2;
9742 vinfos[3].jointtype = 1;
9743 vinfos[3].foffset = j3;
9744 vinfos[3].indices[0] = _ij3[0];
9745 vinfos[3].indices[1] = _ij3[1];
9746 vinfos[3].maxsolutions = _nj3;
9747 vinfos[4].jointtype = 1;
9748 vinfos[4].foffset = j4;
9749 vinfos[4].indices[0] = _ij4[0];
9750 vinfos[4].indices[1] = _ij4[1];
9751 vinfos[4].maxsolutions = _nj4;
9752 vinfos[5].jointtype = 1;
9753 vinfos[5].foffset = j5;
9754 vinfos[5].indices[0] = _ij5[0];
9755 vinfos[5].indices[1] = _ij5[1];
9756 vinfos[5].maxsolutions = _nj5;
9757 std::vector<int> vfree(0);
9770 IkReal j3array[1], cj3array[1], sj3array[1];
9771 bool j3valid[1]={
false};
9781 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x720.
value)))+(x721.
value));
9782 sj3array[0]=
IKsin(j3array[0]);
9783 cj3array[0]=
IKcos(j3array[0]);
9784 if( j3array[0] >
IKPI )
9788 else if( j3array[0] < -
IKPI )
9789 { j3array[0]+=
IK2PI;
9792 for(
int ij3 = 0; ij3 < 1; ++ij3)
9798 _ij3[0] = ij3; _ij3[1] = -1;
9799 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9803 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9806 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9809 IkReal x722=
IKsin(j3);
9810 IkReal x723=
IKcos(j3);
9811 IkReal x724=(sj5*x722);
9812 IkReal x725=((1.0)*x723);
9813 IkReal x726=(cj5*x725);
9814 evalcond[0]=(sj5+((new_r00*x722)));
9815 evalcond[1]=(sj5+((new_r11*x722)));
9816 evalcond[2]=(cj5+(((-1.0)*new_r11*x725)));
9817 evalcond[3]=(((new_r00*x723))+(((-1.0)*cj5)));
9818 evalcond[4]=(((cj5*x722))+((sj5*x723)));
9819 evalcond[5]=((((-1.0)*x726))+x724+new_r00);
9820 evalcond[6]=((((-1.0)*x726))+x724+new_r11);
9828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9829 vinfos[0].jointtype = 1;
9830 vinfos[0].foffset = j0;
9831 vinfos[0].indices[0] = _ij0[0];
9832 vinfos[0].indices[1] = _ij0[1];
9833 vinfos[0].maxsolutions = _nj0;
9834 vinfos[1].jointtype = 1;
9835 vinfos[1].foffset = j1;
9836 vinfos[1].indices[0] = _ij1[0];
9837 vinfos[1].indices[1] = _ij1[1];
9838 vinfos[1].maxsolutions = _nj1;
9839 vinfos[2].jointtype = 1;
9840 vinfos[2].foffset = j2;
9841 vinfos[2].indices[0] = _ij2[0];
9842 vinfos[2].indices[1] = _ij2[1];
9843 vinfos[2].maxsolutions = _nj2;
9844 vinfos[3].jointtype = 1;
9845 vinfos[3].foffset = j3;
9846 vinfos[3].indices[0] = _ij3[0];
9847 vinfos[3].indices[1] = _ij3[1];
9848 vinfos[3].maxsolutions = _nj3;
9849 vinfos[4].jointtype = 1;
9850 vinfos[4].foffset = j4;
9851 vinfos[4].indices[0] = _ij4[0];
9852 vinfos[4].indices[1] = _ij4[1];
9853 vinfos[4].maxsolutions = _nj4;
9854 vinfos[5].jointtype = 1;
9855 vinfos[5].foffset = j5;
9856 vinfos[5].indices[0] = _ij5[0];
9857 vinfos[5].indices[1] = _ij5[1];
9858 vinfos[5].maxsolutions = _nj5;
9859 std::vector<int> vfree(0);
9872 IkReal j3array[1], cj3array[1], sj3array[1];
9873 bool j3valid[1]={
false};
9883 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x727.
value)))+(x728.
value));
9884 sj3array[0]=
IKsin(j3array[0]);
9885 cj3array[0]=
IKcos(j3array[0]);
9886 if( j3array[0] >
IKPI )
9890 else if( j3array[0] < -
IKPI )
9891 { j3array[0]+=
IK2PI;
9894 for(
int ij3 = 0; ij3 < 1; ++ij3)
9900 _ij3[0] = ij3; _ij3[1] = -1;
9901 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
9905 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
9908 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9911 IkReal x729=
IKsin(j3);
9912 IkReal x730=
IKcos(j3);
9913 IkReal x731=(sj5*x729);
9914 IkReal x732=((1.0)*x730);
9915 IkReal x733=(cj5*x732);
9916 evalcond[0]=(sj5+((new_r00*x729)));
9917 evalcond[1]=(sj5+((new_r11*x729)));
9918 evalcond[2]=(cj5+(((-1.0)*new_r11*x732)));
9919 evalcond[3]=(((new_r00*x730))+(((-1.0)*cj5)));
9920 evalcond[4]=(((cj5*x729))+((sj5*x730)));
9921 evalcond[5]=((((-1.0)*x733))+x731+new_r00);
9922 evalcond[6]=((((-1.0)*x733))+x731+new_r11);
9930 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9931 vinfos[0].jointtype = 1;
9932 vinfos[0].foffset = j0;
9933 vinfos[0].indices[0] = _ij0[0];
9934 vinfos[0].indices[1] = _ij0[1];
9935 vinfos[0].maxsolutions = _nj0;
9936 vinfos[1].jointtype = 1;
9937 vinfos[1].foffset = j1;
9938 vinfos[1].indices[0] = _ij1[0];
9939 vinfos[1].indices[1] = _ij1[1];
9940 vinfos[1].maxsolutions = _nj1;
9941 vinfos[2].jointtype = 1;
9942 vinfos[2].foffset = j2;
9943 vinfos[2].indices[0] = _ij2[0];
9944 vinfos[2].indices[1] = _ij2[1];
9945 vinfos[2].maxsolutions = _nj2;
9946 vinfos[3].jointtype = 1;
9947 vinfos[3].foffset = j3;
9948 vinfos[3].indices[0] = _ij3[0];
9949 vinfos[3].indices[1] = _ij3[1];
9950 vinfos[3].maxsolutions = _nj3;
9951 vinfos[4].jointtype = 1;
9952 vinfos[4].foffset = j4;
9953 vinfos[4].indices[0] = _ij4[0];
9954 vinfos[4].indices[1] = _ij4[1];
9955 vinfos[4].maxsolutions = _nj4;
9956 vinfos[5].jointtype = 1;
9957 vinfos[5].foffset = j5;
9958 vinfos[5].indices[0] = _ij5[0];
9959 vinfos[5].indices[1] = _ij5[1];
9960 vinfos[5].maxsolutions = _nj5;
9961 std::vector<int> vfree(0);
9973 if( bgotonextstatement )
9975 bool bgotonextstatement =
true;
9980 bgotonextstatement=
false;
9985 if( bgotonextstatement )
10001 IkReal j3array[1], cj3array[1], sj3array[1];
10002 bool j3valid[1]={
false};
10004 IkReal x734=((1.0)*new_r11);
10013 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x735.
value)))+(x736.
value));
10014 sj3array[0]=
IKsin(j3array[0]);
10015 cj3array[0]=
IKcos(j3array[0]);
10016 if( j3array[0] >
IKPI )
10020 else if( j3array[0] < -
IKPI )
10021 { j3array[0]+=
IK2PI;
10024 for(
int ij3 = 0; ij3 < 1; ++ij3)
10026 if( !j3valid[ij3] )
10030 _ij3[0] = ij3; _ij3[1] = -1;
10031 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
10035 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10038 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10040 IkReal evalcond[8];
10041 IkReal x737=
IKcos(j3);
10042 IkReal x738=
IKsin(j3);
10043 IkReal x739=(sj5*x738);
10044 IkReal x740=(cj5*x738);
10045 IkReal x741=(sj5*x737);
10046 IkReal x742=((1.0)*x737);
10047 IkReal x743=(cj5*x742);
10048 evalcond[0]=(sj5+((new_r01*x737))+((new_r11*x738)));
10049 evalcond[1]=(x740+x741+new_r01);
10050 evalcond[2]=(sj5+((new_r00*x738))+(((-1.0)*new_r10*x742)));
10051 evalcond[3]=(cj5+((new_r01*x738))+(((-1.0)*new_r11*x742)));
10052 evalcond[4]=(x739+new_r00+(((-1.0)*x743)));
10053 evalcond[5]=(x739+new_r11+(((-1.0)*x743)));
10054 evalcond[6]=(((new_r00*x737))+((new_r10*x738))+(((-1.0)*cj5)));
10055 evalcond[7]=((((-1.0)*x740))+(((-1.0)*x741))+new_r10);
10063 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10064 vinfos[0].jointtype = 1;
10065 vinfos[0].foffset = j0;
10066 vinfos[0].indices[0] = _ij0[0];
10067 vinfos[0].indices[1] = _ij0[1];
10068 vinfos[0].maxsolutions = _nj0;
10069 vinfos[1].jointtype = 1;
10070 vinfos[1].foffset = j1;
10071 vinfos[1].indices[0] = _ij1[0];
10072 vinfos[1].indices[1] = _ij1[1];
10073 vinfos[1].maxsolutions = _nj1;
10074 vinfos[2].jointtype = 1;
10075 vinfos[2].foffset = j2;
10076 vinfos[2].indices[0] = _ij2[0];
10077 vinfos[2].indices[1] = _ij2[1];
10078 vinfos[2].maxsolutions = _nj2;
10079 vinfos[3].jointtype = 1;
10080 vinfos[3].foffset = j3;
10081 vinfos[3].indices[0] = _ij3[0];
10082 vinfos[3].indices[1] = _ij3[1];
10083 vinfos[3].maxsolutions = _nj3;
10084 vinfos[4].jointtype = 1;
10085 vinfos[4].foffset = j4;
10086 vinfos[4].indices[0] = _ij4[0];
10087 vinfos[4].indices[1] = _ij4[1];
10088 vinfos[4].maxsolutions = _nj4;
10089 vinfos[5].jointtype = 1;
10090 vinfos[5].foffset = j5;
10091 vinfos[5].indices[0] = _ij5[0];
10092 vinfos[5].indices[1] = _ij5[1];
10093 vinfos[5].maxsolutions = _nj5;
10094 std::vector<int> vfree(0);
10107 IkReal j3array[1], cj3array[1], sj3array[1];
10108 bool j3valid[1]={
false};
10118 j3array[0]=((-1.5707963267949)+(x744.
value)+(((1.5707963267949)*(x745.
value))));
10119 sj3array[0]=
IKsin(j3array[0]);
10120 cj3array[0]=
IKcos(j3array[0]);
10121 if( j3array[0] >
IKPI )
10125 else if( j3array[0] < -
IKPI )
10126 { j3array[0]+=
IK2PI;
10129 for(
int ij3 = 0; ij3 < 1; ++ij3)
10131 if( !j3valid[ij3] )
10135 _ij3[0] = ij3; _ij3[1] = -1;
10136 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
10140 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10143 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10145 IkReal evalcond[8];
10146 IkReal x746=
IKcos(j3);
10147 IkReal x747=
IKsin(j3);
10148 IkReal x748=(sj5*x747);
10149 IkReal x749=(cj5*x747);
10150 IkReal x750=(sj5*x746);
10151 IkReal x751=((1.0)*x746);
10152 IkReal x752=(cj5*x751);
10153 evalcond[0]=(sj5+((new_r01*x746))+((new_r11*x747)));
10154 evalcond[1]=(x750+x749+new_r01);
10155 evalcond[2]=(sj5+(((-1.0)*new_r10*x751))+((new_r00*x747)));
10156 evalcond[3]=(cj5+(((-1.0)*new_r11*x751))+((new_r01*x747)));
10157 evalcond[4]=(x748+new_r00+(((-1.0)*x752)));
10158 evalcond[5]=(x748+new_r11+(((-1.0)*x752)));
10159 evalcond[6]=(((new_r10*x747))+((new_r00*x746))+(((-1.0)*cj5)));
10160 evalcond[7]=((((-1.0)*x750))+(((-1.0)*x749))+new_r10);
10168 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10169 vinfos[0].jointtype = 1;
10170 vinfos[0].foffset = j0;
10171 vinfos[0].indices[0] = _ij0[0];
10172 vinfos[0].indices[1] = _ij0[1];
10173 vinfos[0].maxsolutions = _nj0;
10174 vinfos[1].jointtype = 1;
10175 vinfos[1].foffset = j1;
10176 vinfos[1].indices[0] = _ij1[0];
10177 vinfos[1].indices[1] = _ij1[1];
10178 vinfos[1].maxsolutions = _nj1;
10179 vinfos[2].jointtype = 1;
10180 vinfos[2].foffset = j2;
10181 vinfos[2].indices[0] = _ij2[0];
10182 vinfos[2].indices[1] = _ij2[1];
10183 vinfos[2].maxsolutions = _nj2;
10184 vinfos[3].jointtype = 1;
10185 vinfos[3].foffset = j3;
10186 vinfos[3].indices[0] = _ij3[0];
10187 vinfos[3].indices[1] = _ij3[1];
10188 vinfos[3].maxsolutions = _nj3;
10189 vinfos[4].jointtype = 1;
10190 vinfos[4].foffset = j4;
10191 vinfos[4].indices[0] = _ij4[0];
10192 vinfos[4].indices[1] = _ij4[1];
10193 vinfos[4].maxsolutions = _nj4;
10194 vinfos[5].jointtype = 1;
10195 vinfos[5].foffset = j5;
10196 vinfos[5].indices[0] = _ij5[0];
10197 vinfos[5].indices[1] = _ij5[1];
10198 vinfos[5].maxsolutions = _nj5;
10199 std::vector<int> vfree(0);
10212 IkReal j3array[1], cj3array[1], sj3array[1];
10213 bool j3valid[1]={
false};
10215 IkReal x753=((1.0)*new_r11);
10224 j3array[0]=((-1.5707963267949)+(x754.
value)+(((1.5707963267949)*(x755.
value))));
10225 sj3array[0]=
IKsin(j3array[0]);
10226 cj3array[0]=
IKcos(j3array[0]);
10227 if( j3array[0] >
IKPI )
10231 else if( j3array[0] < -
IKPI )
10232 { j3array[0]+=
IK2PI;
10235 for(
int ij3 = 0; ij3 < 1; ++ij3)
10237 if( !j3valid[ij3] )
10241 _ij3[0] = ij3; _ij3[1] = -1;
10242 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
10246 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10249 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10251 IkReal evalcond[8];
10252 IkReal x756=
IKcos(j3);
10253 IkReal x757=
IKsin(j3);
10254 IkReal x758=(sj5*x757);
10255 IkReal x759=(cj5*x757);
10256 IkReal x760=(sj5*x756);
10257 IkReal x761=((1.0)*x756);
10258 IkReal x762=(cj5*x761);
10259 evalcond[0]=(sj5+((new_r11*x757))+((new_r01*x756)));
10260 evalcond[1]=(x759+x760+new_r01);
10261 evalcond[2]=(sj5+(((-1.0)*new_r10*x761))+((new_r00*x757)));
10262 evalcond[3]=(cj5+(((-1.0)*new_r11*x761))+((new_r01*x757)));
10263 evalcond[4]=((((-1.0)*x762))+x758+new_r00);
10264 evalcond[5]=((((-1.0)*x762))+x758+new_r11);
10265 evalcond[6]=(((new_r00*x756))+((new_r10*x757))+(((-1.0)*cj5)));
10266 evalcond[7]=((((-1.0)*x760))+(((-1.0)*x759))+new_r10);
10274 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10275 vinfos[0].jointtype = 1;
10276 vinfos[0].foffset = j0;
10277 vinfos[0].indices[0] = _ij0[0];
10278 vinfos[0].indices[1] = _ij0[1];
10279 vinfos[0].maxsolutions = _nj0;
10280 vinfos[1].jointtype = 1;
10281 vinfos[1].foffset = j1;
10282 vinfos[1].indices[0] = _ij1[0];
10283 vinfos[1].indices[1] = _ij1[1];
10284 vinfos[1].maxsolutions = _nj1;
10285 vinfos[2].jointtype = 1;
10286 vinfos[2].foffset = j2;
10287 vinfos[2].indices[0] = _ij2[0];
10288 vinfos[2].indices[1] = _ij2[1];
10289 vinfos[2].maxsolutions = _nj2;
10290 vinfos[3].jointtype = 1;
10291 vinfos[3].foffset = j3;
10292 vinfos[3].indices[0] = _ij3[0];
10293 vinfos[3].indices[1] = _ij3[1];
10294 vinfos[3].maxsolutions = _nj3;
10295 vinfos[4].jointtype = 1;
10296 vinfos[4].foffset = j4;
10297 vinfos[4].indices[0] = _ij4[0];
10298 vinfos[4].indices[1] = _ij4[1];
10299 vinfos[4].maxsolutions = _nj4;
10300 vinfos[5].jointtype = 1;
10301 vinfos[5].foffset = j5;
10302 vinfos[5].indices[0] = _ij5[0];
10303 vinfos[5].indices[1] = _ij5[1];
10304 vinfos[5].maxsolutions = _nj5;
10305 std::vector<int> vfree(0);
10317 if( bgotonextstatement )
10319 bool bgotonextstatement =
true;
10322 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10323 evalcond[1]=new_r02;
10324 evalcond[2]=new_r12;
10325 evalcond[3]=new_r21;
10326 evalcond[4]=new_r20;
10327 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 )
10329 bgotonextstatement=
false;
10334 j4=3.14159265358979;
10335 IkReal x763=((1.0)*new_r10);
10336 IkReal x764=((((-1.0)*new_r11*x763))+(((-1.0)*new_r00*new_r01)));
10338 j3eval[1]=((
IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(
IKabs((((cj5*new_r01))+(((-1.0)*cj5*x763))))));
10340 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10346 j4=3.14159265358979;
10347 IkReal x765=((1.0)*new_r10);
10348 IkReal x766=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x765)));
10351 j3eval[2]=((
IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(
IKabs(((cj5*cj5)+(((-1.0)*new_r01*x765))))));
10352 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10358 j4=3.14159265358979;
10359 IkReal x767=((1.0)*new_r00);
10360 IkReal x768=(((cj5*new_r10))+(((-1.0)*sj5*x767)));
10362 j3eval[1]=((
IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))))+(
IKabs(((((-1.0)*new_r10*x767))+((cj5*sj5))))));
10364 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10367 IkReal evalcond[1];
10368 bool bgotonextstatement =
true;
10371 IkReal x769=((-1.0)*new_r00);
10372 IkReal x771 = ((new_r10*new_r10)+(new_r00*new_r00));
10373 if(
IKabs(x771)==0){
10376 IkReal x770=
pow(x771,-0.5);
10381 IkReal gconst12=((-1.0)*(x772.
value));
10382 IkReal gconst13=((-1.0)*new_r10*x770);
10383 IkReal gconst14=(x769*x770);
10388 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((j5+(x773.
value))))), 6.28318530717959)));
10389 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
10391 bgotonextstatement=
false;
10394 IkReal x774=((-1.0)*new_r00);
10399 IkReal x775=((-1.0)*(x777.
value));
10403 j4=3.14159265358979;
10407 IkReal gconst12=x775;
10408 IkReal gconst13=((-1.0)*new_r10*x776);
10409 IkReal gconst14=(x774*x776);
10410 IkReal x778=new_r00*new_r00;
10411 IkReal x779=((1.0)*new_r11);
10412 IkReal x780=((1.0)*new_r00*new_r01);
10413 IkReal x781=((((-1.0)*x780))+(((-1.0)*new_r10*x779)));
10415 IkReal x783=(new_r00*x782);
10417 j3eval[1]=((
IKabs((((new_r10*x783))+(((-1.0)*x780*x782)))))+(
IKabs(((((-1.0)*x778*x782))+(((-1.0)*x779*x783))))));
10419 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10423 IkReal x784=((-1.0)*new_r00);
10428 IkReal x785=((-1.0)*(x787.
value));
10432 j4=3.14159265358979;
10436 IkReal gconst12=x785;
10437 IkReal gconst13=((-1.0)*new_r10*x786);
10438 IkReal gconst14=(x784*x786);
10439 IkReal x788=new_r10*new_r10;
10440 IkReal x789=new_r00*new_r00;
10445 IkReal x790=x792.
value;
10446 IkReal x791=(new_r00*x790);
10447 j3eval[0]=((
IKabs((((new_r01*x791*(new_r00*new_r00)))+((new_r10*x791))+((new_r01*x788*x791)))))+(
IKabs(((((-1.0)*new_r01*new_r10))+((x789*x790))))));
10448 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
10452 IkReal x793=((-1.0)*new_r00);
10457 IkReal x794=((-1.0)*(x796.
value));
10461 j4=3.14159265358979;
10465 IkReal gconst12=x794;
10466 IkReal gconst13=((-1.0)*new_r10*x795);
10467 IkReal gconst14=(x793*x795);
10468 IkReal x797=new_r00*new_r00;
10469 IkReal x798=new_r10*new_r10;
10474 IkReal x799=x802.
value;
10475 IkReal x800=(new_r10*x799);
10476 IkReal x801=((1.0)*x799);
10477 j3eval[0]=((
IKabs(((((-1.0)*x797*x798*x801))+((x797*x799))+(((-1.0)*x801*(x798*x798))))))+(
IKabs((((x800*(new_r00*new_r00*new_r00)))+((new_r00*x800*(new_r10*new_r10)))+((new_r00*x800))))));
10478 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
10481 IkReal evalcond[3];
10482 bool bgotonextstatement =
true;
10485 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
10486 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
10488 bgotonextstatement=
false;
10495 IkReal x803=((-1.0)*(x804.
value));
10498 j4=3.14159265358979;
10504 IkReal gconst12=x803;
10505 IkReal x805 = new_r10*new_r10;
10506 if(
IKabs(x805)==0){
10509 IkReal gconst13=((-1.0)*new_r10*(
pow(x805,-0.5)));
10512 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
10515 IkReal j3array[2], cj3array[2], sj3array[2];
10516 bool j3valid[2]={
false};
10522 cj3array[0]=(new_r01*(x806.
value));
10525 j3valid[0] = j3valid[1] =
true;
10526 j3array[0] =
IKacos(cj3array[0]);
10527 sj3array[0] =
IKsin(j3array[0]);
10528 cj3array[1] = cj3array[0];
10529 j3array[1] = -j3array[0];
10530 sj3array[1] = -sj3array[0];
10532 else if( isnan(cj3array[0]) )
10536 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10538 for(
int ij3 = 0; ij3 < 2; ++ij3)
10540 if( !j3valid[ij3] )
10544 _ij3[0] = ij3; _ij3[1] = -1;
10545 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
10549 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10552 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10554 IkReal evalcond[6];
10555 IkReal x807=
IKsin(j3);
10556 IkReal x808=
IKcos(j3);
10557 IkReal x809=((1.0)*gconst13);
10558 evalcond[0]=(new_r01*x807);
10559 evalcond[1]=(new_r10*x807);
10560 evalcond[2]=(gconst13*x807);
10561 evalcond[3]=(gconst13+(((-1.0)*new_r10*x808)));
10562 evalcond[4]=((((-1.0)*x808*x809))+new_r10);
10563 evalcond[5]=(((new_r01*x808))+(((-1.0)*x809)));
10571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10572 vinfos[0].jointtype = 1;
10573 vinfos[0].foffset = j0;
10574 vinfos[0].indices[0] = _ij0[0];
10575 vinfos[0].indices[1] = _ij0[1];
10576 vinfos[0].maxsolutions = _nj0;
10577 vinfos[1].jointtype = 1;
10578 vinfos[1].foffset = j1;
10579 vinfos[1].indices[0] = _ij1[0];
10580 vinfos[1].indices[1] = _ij1[1];
10581 vinfos[1].maxsolutions = _nj1;
10582 vinfos[2].jointtype = 1;
10583 vinfos[2].foffset = j2;
10584 vinfos[2].indices[0] = _ij2[0];
10585 vinfos[2].indices[1] = _ij2[1];
10586 vinfos[2].maxsolutions = _nj2;
10587 vinfos[3].jointtype = 1;
10588 vinfos[3].foffset = j3;
10589 vinfos[3].indices[0] = _ij3[0];
10590 vinfos[3].indices[1] = _ij3[1];
10591 vinfos[3].maxsolutions = _nj3;
10592 vinfos[4].jointtype = 1;
10593 vinfos[4].foffset = j4;
10594 vinfos[4].indices[0] = _ij4[0];
10595 vinfos[4].indices[1] = _ij4[1];
10596 vinfos[4].maxsolutions = _nj4;
10597 vinfos[5].jointtype = 1;
10598 vinfos[5].foffset = j5;
10599 vinfos[5].indices[0] = _ij5[0];
10600 vinfos[5].indices[1] = _ij5[1];
10601 vinfos[5].maxsolutions = _nj5;
10602 std::vector<int> vfree(0);
10611 IkReal j3array[2], cj3array[2], sj3array[2];
10612 bool j3valid[2]={
false};
10618 cj3array[0]=(gconst13*(x810.
value));
10621 j3valid[0] = j3valid[1] =
true;
10622 j3array[0] =
IKacos(cj3array[0]);
10623 sj3array[0] =
IKsin(j3array[0]);
10624 cj3array[1] = cj3array[0];
10625 j3array[1] = -j3array[0];
10626 sj3array[1] = -sj3array[0];
10628 else if( isnan(cj3array[0]) )
10632 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10634 for(
int ij3 = 0; ij3 < 2; ++ij3)
10636 if( !j3valid[ij3] )
10640 _ij3[0] = ij3; _ij3[1] = -1;
10641 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
10645 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10648 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10650 IkReal evalcond[6];
10651 IkReal x811=
IKsin(j3);
10652 IkReal x812=
IKcos(j3);
10653 IkReal x813=((1.0)*gconst13);
10654 IkReal x814=(x812*x813);
10655 evalcond[0]=(new_r01*x811);
10656 evalcond[1]=(new_r10*x811);
10657 evalcond[2]=(gconst13*x811);
10658 evalcond[3]=((((-1.0)*x814))+new_r01);
10659 evalcond[4]=((((-1.0)*x814))+new_r10);
10660 evalcond[5]=(((new_r01*x812))+(((-1.0)*x813)));
10668 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10669 vinfos[0].jointtype = 1;
10670 vinfos[0].foffset = j0;
10671 vinfos[0].indices[0] = _ij0[0];
10672 vinfos[0].indices[1] = _ij0[1];
10673 vinfos[0].maxsolutions = _nj0;
10674 vinfos[1].jointtype = 1;
10675 vinfos[1].foffset = j1;
10676 vinfos[1].indices[0] = _ij1[0];
10677 vinfos[1].indices[1] = _ij1[1];
10678 vinfos[1].maxsolutions = _nj1;
10679 vinfos[2].jointtype = 1;
10680 vinfos[2].foffset = j2;
10681 vinfos[2].indices[0] = _ij2[0];
10682 vinfos[2].indices[1] = _ij2[1];
10683 vinfos[2].maxsolutions = _nj2;
10684 vinfos[3].jointtype = 1;
10685 vinfos[3].foffset = j3;
10686 vinfos[3].indices[0] = _ij3[0];
10687 vinfos[3].indices[1] = _ij3[1];
10688 vinfos[3].maxsolutions = _nj3;
10689 vinfos[4].jointtype = 1;
10690 vinfos[4].foffset = j4;
10691 vinfos[4].indices[0] = _ij4[0];
10692 vinfos[4].indices[1] = _ij4[1];
10693 vinfos[4].maxsolutions = _nj4;
10694 vinfos[5].jointtype = 1;
10695 vinfos[5].foffset = j5;
10696 vinfos[5].indices[0] = _ij5[0];
10697 vinfos[5].indices[1] = _ij5[1];
10698 vinfos[5].maxsolutions = _nj5;
10699 std::vector<int> vfree(0);
10711 if( bgotonextstatement )
10713 bool bgotonextstatement =
true;
10716 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
10717 evalcond[1]=gconst14;
10718 evalcond[2]=gconst13;
10719 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
10721 bgotonextstatement=
false;
10724 IkReal x815=((-1.0)*new_r00);
10729 IkReal x816=((-1.0)*(x817.
value));
10732 j4=3.14159265358979;
10740 IkReal gconst12=x816;
10741 IkReal gconst13=((-1.0)*new_r10);
10742 IkReal gconst14=x815;
10745 j3eval[2]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs((new_r00*new_r10))));
10746 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10750 IkReal x818=((-1.0)*new_r00);
10755 IkReal x819=((-1.0)*(x820.
value));
10758 j4=3.14159265358979;
10766 IkReal gconst12=x819;
10767 IkReal gconst13=((-1.0)*new_r10);
10768 IkReal gconst14=x818;
10771 j3eval[2]=((
IKabs(((-1.0)+(new_r10*new_r10))))+(
IKabs((new_r00*new_r10))));
10772 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10776 IkReal x821=((-1.0)*new_r00);
10781 IkReal x822=((-1.0)*(x823.
value));
10784 j4=3.14159265358979;
10792 IkReal gconst12=x822;
10793 IkReal gconst13=((-1.0)*new_r10);
10794 IkReal gconst14=x821;
10796 j3eval[1]=((((0.5)*(
IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(
IKabs((new_r00*new_r10))));
10798 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
10805 IkReal j3array[1], cj3array[1], sj3array[1];
10806 bool j3valid[1]={
false};
10816 j3array[0]=((-1.5707963267949)+(x824.
value)+(((1.5707963267949)*(x825.
value))));
10817 sj3array[0]=
IKsin(j3array[0]);
10818 cj3array[0]=
IKcos(j3array[0]);
10819 if( j3array[0] >
IKPI )
10823 else if( j3array[0] < -
IKPI )
10824 { j3array[0]+=
IK2PI;
10827 for(
int ij3 = 0; ij3 < 1; ++ij3)
10829 if( !j3valid[ij3] )
10833 _ij3[0] = ij3; _ij3[1] = -1;
10834 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
10838 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10841 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10843 IkReal evalcond[6];
10844 IkReal x826=
IKsin(j3);
10845 IkReal x827=
IKcos(j3);
10846 IkReal x828=((1.0)*gconst13);
10847 IkReal x829=(gconst14*x826);
10848 IkReal x830=(gconst14*x827);
10849 IkReal x831=(x827*x828);
10850 evalcond[0]=((((-1.0)*x831))+x829);
10851 evalcond[1]=(((new_r10*x826))+gconst14+((new_r00*x827)));
10852 evalcond[2]=(((gconst13*x826))+new_r00+x830);
10853 evalcond[3]=((((-1.0)*new_r10*x827))+gconst13+((new_r00*x826)));
10854 evalcond[4]=((((-1.0)*x826*x828))+(((-1.0)*x830)));
10855 evalcond[5]=((((-1.0)*x831))+new_r10+x829);
10863 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10864 vinfos[0].jointtype = 1;
10865 vinfos[0].foffset = j0;
10866 vinfos[0].indices[0] = _ij0[0];
10867 vinfos[0].indices[1] = _ij0[1];
10868 vinfos[0].maxsolutions = _nj0;
10869 vinfos[1].jointtype = 1;
10870 vinfos[1].foffset = j1;
10871 vinfos[1].indices[0] = _ij1[0];
10872 vinfos[1].indices[1] = _ij1[1];
10873 vinfos[1].maxsolutions = _nj1;
10874 vinfos[2].jointtype = 1;
10875 vinfos[2].foffset = j2;
10876 vinfos[2].indices[0] = _ij2[0];
10877 vinfos[2].indices[1] = _ij2[1];
10878 vinfos[2].maxsolutions = _nj2;
10879 vinfos[3].jointtype = 1;
10880 vinfos[3].foffset = j3;
10881 vinfos[3].indices[0] = _ij3[0];
10882 vinfos[3].indices[1] = _ij3[1];
10883 vinfos[3].maxsolutions = _nj3;
10884 vinfos[4].jointtype = 1;
10885 vinfos[4].foffset = j4;
10886 vinfos[4].indices[0] = _ij4[0];
10887 vinfos[4].indices[1] = _ij4[1];
10888 vinfos[4].maxsolutions = _nj4;
10889 vinfos[5].jointtype = 1;
10890 vinfos[5].foffset = j5;
10891 vinfos[5].indices[0] = _ij5[0];
10892 vinfos[5].indices[1] = _ij5[1];
10893 vinfos[5].maxsolutions = _nj5;
10894 std::vector<int> vfree(0);
10907 IkReal j3array[1], cj3array[1], sj3array[1];
10908 bool j3valid[1]={
false};
10918 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x832.
value)))+(x833.
value));
10919 sj3array[0]=
IKsin(j3array[0]);
10920 cj3array[0]=
IKcos(j3array[0]);
10921 if( j3array[0] >
IKPI )
10925 else if( j3array[0] < -
IKPI )
10926 { j3array[0]+=
IK2PI;
10929 for(
int ij3 = 0; ij3 < 1; ++ij3)
10931 if( !j3valid[ij3] )
10935 _ij3[0] = ij3; _ij3[1] = -1;
10936 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
10940 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
10943 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10945 IkReal evalcond[6];
10946 IkReal x834=
IKsin(j3);
10947 IkReal x835=
IKcos(j3);
10948 IkReal x836=((1.0)*gconst13);
10949 IkReal x837=(gconst14*x834);
10950 IkReal x838=(gconst14*x835);
10951 IkReal x839=(x835*x836);
10952 evalcond[0]=((((-1.0)*x839))+x837);
10953 evalcond[1]=(gconst14+((new_r00*x835))+((new_r10*x834)));
10954 evalcond[2]=(((gconst13*x834))+new_r00+x838);
10955 evalcond[3]=(gconst13+((new_r00*x834))+(((-1.0)*new_r10*x835)));
10956 evalcond[4]=((((-1.0)*x834*x836))+(((-1.0)*x838)));
10957 evalcond[5]=((((-1.0)*x839))+new_r10+x837);
10965 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10966 vinfos[0].jointtype = 1;
10967 vinfos[0].foffset = j0;
10968 vinfos[0].indices[0] = _ij0[0];
10969 vinfos[0].indices[1] = _ij0[1];
10970 vinfos[0].maxsolutions = _nj0;
10971 vinfos[1].jointtype = 1;
10972 vinfos[1].foffset = j1;
10973 vinfos[1].indices[0] = _ij1[0];
10974 vinfos[1].indices[1] = _ij1[1];
10975 vinfos[1].maxsolutions = _nj1;
10976 vinfos[2].jointtype = 1;
10977 vinfos[2].foffset = j2;
10978 vinfos[2].indices[0] = _ij2[0];
10979 vinfos[2].indices[1] = _ij2[1];
10980 vinfos[2].maxsolutions = _nj2;
10981 vinfos[3].jointtype = 1;
10982 vinfos[3].foffset = j3;
10983 vinfos[3].indices[0] = _ij3[0];
10984 vinfos[3].indices[1] = _ij3[1];
10985 vinfos[3].maxsolutions = _nj3;
10986 vinfos[4].jointtype = 1;
10987 vinfos[4].foffset = j4;
10988 vinfos[4].indices[0] = _ij4[0];
10989 vinfos[4].indices[1] = _ij4[1];
10990 vinfos[4].maxsolutions = _nj4;
10991 vinfos[5].jointtype = 1;
10992 vinfos[5].foffset = j5;
10993 vinfos[5].indices[0] = _ij5[0];
10994 vinfos[5].indices[1] = _ij5[1];
10995 vinfos[5].maxsolutions = _nj5;
10996 std::vector<int> vfree(0);
11009 IkReal j3array[1], cj3array[1], sj3array[1];
11010 bool j3valid[1]={
false};
11020 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x840.
value)))+(x841.
value));
11021 sj3array[0]=
IKsin(j3array[0]);
11022 cj3array[0]=
IKcos(j3array[0]);
11023 if( j3array[0] >
IKPI )
11027 else if( j3array[0] < -
IKPI )
11028 { j3array[0]+=
IK2PI;
11031 for(
int ij3 = 0; ij3 < 1; ++ij3)
11033 if( !j3valid[ij3] )
11037 _ij3[0] = ij3; _ij3[1] = -1;
11038 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11042 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11045 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11047 IkReal evalcond[6];
11048 IkReal x842=
IKsin(j3);
11049 IkReal x843=
IKcos(j3);
11050 IkReal x844=((1.0)*gconst13);
11051 IkReal x845=(gconst14*x842);
11052 IkReal x846=(gconst14*x843);
11053 IkReal x847=(x843*x844);
11054 evalcond[0]=((((-1.0)*x847))+x845);
11055 evalcond[1]=(((new_r10*x842))+((new_r00*x843))+gconst14);
11056 evalcond[2]=(((gconst13*x842))+new_r00+x846);
11057 evalcond[3]=(((new_r00*x842))+gconst13+(((-1.0)*new_r10*x843)));
11058 evalcond[4]=((((-1.0)*x842*x844))+(((-1.0)*x846)));
11059 evalcond[5]=((((-1.0)*x847))+new_r10+x845);
11067 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11068 vinfos[0].jointtype = 1;
11069 vinfos[0].foffset = j0;
11070 vinfos[0].indices[0] = _ij0[0];
11071 vinfos[0].indices[1] = _ij0[1];
11072 vinfos[0].maxsolutions = _nj0;
11073 vinfos[1].jointtype = 1;
11074 vinfos[1].foffset = j1;
11075 vinfos[1].indices[0] = _ij1[0];
11076 vinfos[1].indices[1] = _ij1[1];
11077 vinfos[1].maxsolutions = _nj1;
11078 vinfos[2].jointtype = 1;
11079 vinfos[2].foffset = j2;
11080 vinfos[2].indices[0] = _ij2[0];
11081 vinfos[2].indices[1] = _ij2[1];
11082 vinfos[2].maxsolutions = _nj2;
11083 vinfos[3].jointtype = 1;
11084 vinfos[3].foffset = j3;
11085 vinfos[3].indices[0] = _ij3[0];
11086 vinfos[3].indices[1] = _ij3[1];
11087 vinfos[3].maxsolutions = _nj3;
11088 vinfos[4].jointtype = 1;
11089 vinfos[4].foffset = j4;
11090 vinfos[4].indices[0] = _ij4[0];
11091 vinfos[4].indices[1] = _ij4[1];
11092 vinfos[4].maxsolutions = _nj4;
11093 vinfos[5].jointtype = 1;
11094 vinfos[5].foffset = j5;
11095 vinfos[5].indices[0] = _ij5[0];
11096 vinfos[5].indices[1] = _ij5[1];
11097 vinfos[5].maxsolutions = _nj5;
11098 std::vector<int> vfree(0);
11110 if( bgotonextstatement )
11112 bool bgotonextstatement =
true;
11115 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
11116 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
11118 bgotonextstatement=
false;
11121 IkReal x848=((-1.0)*new_r00);
11126 IkReal x849=((-1.0)*(x850.
value));
11129 j4=3.14159265358979;
11135 IkReal gconst12=x849;
11137 IkReal x851 = new_r00*new_r00;
11138 if(
IKabs(x851)==0){
11141 IkReal gconst14=(x848*(
pow(x851,-0.5)));
11143 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
11146 IkReal j3array[2], cj3array[2], sj3array[2];
11147 bool j3valid[2]={
false};
11153 cj3array[0]=(new_r11*(x852.
value));
11156 j3valid[0] = j3valid[1] =
true;
11157 j3array[0] =
IKacos(cj3array[0]);
11158 sj3array[0] =
IKsin(j3array[0]);
11159 cj3array[1] = cj3array[0];
11160 j3array[1] = -j3array[0];
11161 sj3array[1] = -sj3array[0];
11163 else if( isnan(cj3array[0]) )
11167 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11169 for(
int ij3 = 0; ij3 < 2; ++ij3)
11171 if( !j3valid[ij3] )
11175 _ij3[0] = ij3; _ij3[1] = -1;
11176 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
11180 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11183 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11185 IkReal evalcond[6];
11186 IkReal x853=
IKsin(j3);
11187 IkReal x854=
IKcos(j3);
11188 evalcond[0]=(new_r00*x853);
11189 evalcond[1]=(new_r11*x853);
11190 evalcond[2]=(gconst14*x853);
11191 evalcond[3]=(gconst14+((new_r00*x854)));
11192 evalcond[4]=(new_r00+((gconst14*x854)));
11193 evalcond[5]=(gconst14+(((-1.0)*new_r11*x854)));
11201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11202 vinfos[0].jointtype = 1;
11203 vinfos[0].foffset = j0;
11204 vinfos[0].indices[0] = _ij0[0];
11205 vinfos[0].indices[1] = _ij0[1];
11206 vinfos[0].maxsolutions = _nj0;
11207 vinfos[1].jointtype = 1;
11208 vinfos[1].foffset = j1;
11209 vinfos[1].indices[0] = _ij1[0];
11210 vinfos[1].indices[1] = _ij1[1];
11211 vinfos[1].maxsolutions = _nj1;
11212 vinfos[2].jointtype = 1;
11213 vinfos[2].foffset = j2;
11214 vinfos[2].indices[0] = _ij2[0];
11215 vinfos[2].indices[1] = _ij2[1];
11216 vinfos[2].maxsolutions = _nj2;
11217 vinfos[3].jointtype = 1;
11218 vinfos[3].foffset = j3;
11219 vinfos[3].indices[0] = _ij3[0];
11220 vinfos[3].indices[1] = _ij3[1];
11221 vinfos[3].maxsolutions = _nj3;
11222 vinfos[4].jointtype = 1;
11223 vinfos[4].foffset = j4;
11224 vinfos[4].indices[0] = _ij4[0];
11225 vinfos[4].indices[1] = _ij4[1];
11226 vinfos[4].maxsolutions = _nj4;
11227 vinfos[5].jointtype = 1;
11228 vinfos[5].foffset = j5;
11229 vinfos[5].indices[0] = _ij5[0];
11230 vinfos[5].indices[1] = _ij5[1];
11231 vinfos[5].maxsolutions = _nj5;
11232 std::vector<int> vfree(0);
11241 IkReal j3array[2], cj3array[2], sj3array[2];
11242 bool j3valid[2]={
false};
11248 cj3array[0]=(gconst14*(x855.
value));
11251 j3valid[0] = j3valid[1] =
true;
11252 j3array[0] =
IKacos(cj3array[0]);
11253 sj3array[0] =
IKsin(j3array[0]);
11254 cj3array[1] = cj3array[0];
11255 j3array[1] = -j3array[0];
11256 sj3array[1] = -sj3array[0];
11258 else if( isnan(cj3array[0]) )
11262 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11264 for(
int ij3 = 0; ij3 < 2; ++ij3)
11266 if( !j3valid[ij3] )
11270 _ij3[0] = ij3; _ij3[1] = -1;
11271 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
11275 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11278 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11280 IkReal evalcond[6];
11281 IkReal x856=
IKsin(j3);
11282 IkReal x857=
IKcos(j3);
11283 IkReal x858=(gconst14*x857);
11284 evalcond[0]=(new_r00*x856);
11285 evalcond[1]=(new_r11*x856);
11286 evalcond[2]=(gconst14*x856);
11287 evalcond[3]=(gconst14+((new_r00*x857)));
11288 evalcond[4]=(new_r00+x858);
11289 evalcond[5]=((((-1.0)*x858))+new_r11);
11297 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11298 vinfos[0].jointtype = 1;
11299 vinfos[0].foffset = j0;
11300 vinfos[0].indices[0] = _ij0[0];
11301 vinfos[0].indices[1] = _ij0[1];
11302 vinfos[0].maxsolutions = _nj0;
11303 vinfos[1].jointtype = 1;
11304 vinfos[1].foffset = j1;
11305 vinfos[1].indices[0] = _ij1[0];
11306 vinfos[1].indices[1] = _ij1[1];
11307 vinfos[1].maxsolutions = _nj1;
11308 vinfos[2].jointtype = 1;
11309 vinfos[2].foffset = j2;
11310 vinfos[2].indices[0] = _ij2[0];
11311 vinfos[2].indices[1] = _ij2[1];
11312 vinfos[2].maxsolutions = _nj2;
11313 vinfos[3].jointtype = 1;
11314 vinfos[3].foffset = j3;
11315 vinfos[3].indices[0] = _ij3[0];
11316 vinfos[3].indices[1] = _ij3[1];
11317 vinfos[3].maxsolutions = _nj3;
11318 vinfos[4].jointtype = 1;
11319 vinfos[4].foffset = j4;
11320 vinfos[4].indices[0] = _ij4[0];
11321 vinfos[4].indices[1] = _ij4[1];
11322 vinfos[4].maxsolutions = _nj4;
11323 vinfos[5].jointtype = 1;
11324 vinfos[5].foffset = j5;
11325 vinfos[5].indices[0] = _ij5[0];
11326 vinfos[5].indices[1] = _ij5[1];
11327 vinfos[5].maxsolutions = _nj5;
11328 std::vector<int> vfree(0);
11340 if( bgotonextstatement )
11342 bool bgotonextstatement =
true;
11345 evalcond[0]=
IKabs(new_r00);
11346 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
11348 bgotonextstatement=
false;
11355 IkReal x859=((-1.0)*(x860.
value));
11358 j4=3.14159265358979;
11363 IkReal gconst12=x859;
11364 IkReal x861 = new_r10*new_r10;
11365 if(
IKabs(x861)==0){
11368 IkReal gconst13=((-1.0)*new_r10*(
pow(x861,-0.5)));
11370 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
11371 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
11379 IkReal x862=((-1.0)*(x863.
value));
11382 j4=3.14159265358979;
11387 IkReal gconst12=x862;
11388 IkReal x864 = new_r10*new_r10;
11389 if(
IKabs(x864)==0){
11392 IkReal gconst13=((-1.0)*new_r10*(
pow(x864,-0.5)));
11394 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
11395 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
11403 IkReal x865=((-1.0)*(x866.
value));
11406 j4=3.14159265358979;
11411 IkReal gconst12=x865;
11412 IkReal x867 = new_r10*new_r10;
11413 if(
IKabs(x867)==0){
11416 IkReal gconst13=((-1.0)*new_r10*(
pow(x867,-0.5)));
11419 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
11426 IkReal j3array[1], cj3array[1], sj3array[1];
11427 bool j3valid[1]={
false};
11440 sj3array[0]=
IKsin(j3array[0]);
11441 cj3array[0]=
IKcos(j3array[0]);
11442 if( j3array[0] >
IKPI )
11446 else if( j3array[0] < -
IKPI )
11447 { j3array[0]+=
IK2PI;
11450 for(
int ij3 = 0; ij3 < 1; ++ij3)
11452 if( !j3valid[ij3] )
11456 _ij3[0] = ij3; _ij3[1] = -1;
11457 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11461 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11464 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11466 IkReal evalcond[8];
11467 IkReal x870=
IKsin(j3);
11468 IkReal x871=
IKcos(j3);
11469 IkReal x872=((1.0)*x871);
11470 IkReal x873=(gconst13*x870);
11471 IkReal x874=(gconst13*x872);
11472 evalcond[0]=(new_r10*x870);
11474 evalcond[2]=(gconst13+(((-1.0)*new_r10*x872)));
11475 evalcond[3]=(new_r01+(((-1.0)*x874)));
11476 evalcond[4]=((((-1.0)*x873))+new_r11);
11477 evalcond[5]=(new_r10+(((-1.0)*x874)));
11478 evalcond[6]=(((new_r01*x870))+(((-1.0)*new_r11*x872)));
11479 evalcond[7]=(((new_r01*x871))+((new_r11*x870))+(((-1.0)*gconst13)));
11487 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11488 vinfos[0].jointtype = 1;
11489 vinfos[0].foffset = j0;
11490 vinfos[0].indices[0] = _ij0[0];
11491 vinfos[0].indices[1] = _ij0[1];
11492 vinfos[0].maxsolutions = _nj0;
11493 vinfos[1].jointtype = 1;
11494 vinfos[1].foffset = j1;
11495 vinfos[1].indices[0] = _ij1[0];
11496 vinfos[1].indices[1] = _ij1[1];
11497 vinfos[1].maxsolutions = _nj1;
11498 vinfos[2].jointtype = 1;
11499 vinfos[2].foffset = j2;
11500 vinfos[2].indices[0] = _ij2[0];
11501 vinfos[2].indices[1] = _ij2[1];
11502 vinfos[2].maxsolutions = _nj2;
11503 vinfos[3].jointtype = 1;
11504 vinfos[3].foffset = j3;
11505 vinfos[3].indices[0] = _ij3[0];
11506 vinfos[3].indices[1] = _ij3[1];
11507 vinfos[3].maxsolutions = _nj3;
11508 vinfos[4].jointtype = 1;
11509 vinfos[4].foffset = j4;
11510 vinfos[4].indices[0] = _ij4[0];
11511 vinfos[4].indices[1] = _ij4[1];
11512 vinfos[4].maxsolutions = _nj4;
11513 vinfos[5].jointtype = 1;
11514 vinfos[5].foffset = j5;
11515 vinfos[5].indices[0] = _ij5[0];
11516 vinfos[5].indices[1] = _ij5[1];
11517 vinfos[5].maxsolutions = _nj5;
11518 std::vector<int> vfree(0);
11531 IkReal j3array[1], cj3array[1], sj3array[1];
11532 bool j3valid[1]={
false};
11542 j3array[0]=((-1.5707963267949)+(x875.
value)+(((1.5707963267949)*(x876.
value))));
11543 sj3array[0]=
IKsin(j3array[0]);
11544 cj3array[0]=
IKcos(j3array[0]);
11545 if( j3array[0] >
IKPI )
11549 else if( j3array[0] < -
IKPI )
11550 { j3array[0]+=
IK2PI;
11553 for(
int ij3 = 0; ij3 < 1; ++ij3)
11555 if( !j3valid[ij3] )
11559 _ij3[0] = ij3; _ij3[1] = -1;
11560 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11564 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11567 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11569 IkReal evalcond[8];
11570 IkReal x877=
IKsin(j3);
11571 IkReal x878=
IKcos(j3);
11572 IkReal x879=((1.0)*x878);
11573 IkReal x880=(gconst13*x877);
11574 IkReal x881=(gconst13*x879);
11575 evalcond[0]=(new_r10*x877);
11577 evalcond[2]=(gconst13+(((-1.0)*new_r10*x879)));
11578 evalcond[3]=((((-1.0)*x881))+new_r01);
11579 evalcond[4]=((((-1.0)*x880))+new_r11);
11580 evalcond[5]=((((-1.0)*x881))+new_r10);
11581 evalcond[6]=(((new_r01*x877))+(((-1.0)*new_r11*x879)));
11582 evalcond[7]=(((new_r01*x878))+((new_r11*x877))+(((-1.0)*gconst13)));
11590 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11591 vinfos[0].jointtype = 1;
11592 vinfos[0].foffset = j0;
11593 vinfos[0].indices[0] = _ij0[0];
11594 vinfos[0].indices[1] = _ij0[1];
11595 vinfos[0].maxsolutions = _nj0;
11596 vinfos[1].jointtype = 1;
11597 vinfos[1].foffset = j1;
11598 vinfos[1].indices[0] = _ij1[0];
11599 vinfos[1].indices[1] = _ij1[1];
11600 vinfos[1].maxsolutions = _nj1;
11601 vinfos[2].jointtype = 1;
11602 vinfos[2].foffset = j2;
11603 vinfos[2].indices[0] = _ij2[0];
11604 vinfos[2].indices[1] = _ij2[1];
11605 vinfos[2].maxsolutions = _nj2;
11606 vinfos[3].jointtype = 1;
11607 vinfos[3].foffset = j3;
11608 vinfos[3].indices[0] = _ij3[0];
11609 vinfos[3].indices[1] = _ij3[1];
11610 vinfos[3].maxsolutions = _nj3;
11611 vinfos[4].jointtype = 1;
11612 vinfos[4].foffset = j4;
11613 vinfos[4].indices[0] = _ij4[0];
11614 vinfos[4].indices[1] = _ij4[1];
11615 vinfos[4].maxsolutions = _nj4;
11616 vinfos[5].jointtype = 1;
11617 vinfos[5].foffset = j5;
11618 vinfos[5].indices[0] = _ij5[0];
11619 vinfos[5].indices[1] = _ij5[1];
11620 vinfos[5].maxsolutions = _nj5;
11621 std::vector<int> vfree(0);
11634 IkReal j3array[1], cj3array[1], sj3array[1];
11635 bool j3valid[1]={
false};
11645 j3array[0]=((-1.5707963267949)+(x882.
value)+(((1.5707963267949)*(x883.
value))));
11646 sj3array[0]=
IKsin(j3array[0]);
11647 cj3array[0]=
IKcos(j3array[0]);
11648 if( j3array[0] >
IKPI )
11652 else if( j3array[0] < -
IKPI )
11653 { j3array[0]+=
IK2PI;
11656 for(
int ij3 = 0; ij3 < 1; ++ij3)
11658 if( !j3valid[ij3] )
11662 _ij3[0] = ij3; _ij3[1] = -1;
11663 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11667 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11670 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11672 IkReal evalcond[8];
11673 IkReal x884=
IKsin(j3);
11674 IkReal x885=
IKcos(j3);
11675 IkReal x886=((1.0)*x885);
11676 IkReal x887=(gconst13*x884);
11677 IkReal x888=(gconst13*x886);
11678 evalcond[0]=(new_r10*x884);
11680 evalcond[2]=((((-1.0)*new_r10*x886))+gconst13);
11681 evalcond[3]=((((-1.0)*x888))+new_r01);
11682 evalcond[4]=((((-1.0)*x887))+new_r11);
11683 evalcond[5]=((((-1.0)*x888))+new_r10);
11684 evalcond[6]=(((new_r01*x884))+(((-1.0)*new_r11*x886)));
11685 evalcond[7]=(((new_r01*x885))+((new_r11*x884))+(((-1.0)*gconst13)));
11693 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11694 vinfos[0].jointtype = 1;
11695 vinfos[0].foffset = j0;
11696 vinfos[0].indices[0] = _ij0[0];
11697 vinfos[0].indices[1] = _ij0[1];
11698 vinfos[0].maxsolutions = _nj0;
11699 vinfos[1].jointtype = 1;
11700 vinfos[1].foffset = j1;
11701 vinfos[1].indices[0] = _ij1[0];
11702 vinfos[1].indices[1] = _ij1[1];
11703 vinfos[1].maxsolutions = _nj1;
11704 vinfos[2].jointtype = 1;
11705 vinfos[2].foffset = j2;
11706 vinfos[2].indices[0] = _ij2[0];
11707 vinfos[2].indices[1] = _ij2[1];
11708 vinfos[2].maxsolutions = _nj2;
11709 vinfos[3].jointtype = 1;
11710 vinfos[3].foffset = j3;
11711 vinfos[3].indices[0] = _ij3[0];
11712 vinfos[3].indices[1] = _ij3[1];
11713 vinfos[3].maxsolutions = _nj3;
11714 vinfos[4].jointtype = 1;
11715 vinfos[4].foffset = j4;
11716 vinfos[4].indices[0] = _ij4[0];
11717 vinfos[4].indices[1] = _ij4[1];
11718 vinfos[4].maxsolutions = _nj4;
11719 vinfos[5].jointtype = 1;
11720 vinfos[5].foffset = j5;
11721 vinfos[5].indices[0] = _ij5[0];
11722 vinfos[5].indices[1] = _ij5[1];
11723 vinfos[5].maxsolutions = _nj5;
11724 std::vector<int> vfree(0);
11736 if( bgotonextstatement )
11738 bool bgotonextstatement =
true;
11743 bgotonextstatement=
false;
11748 if( bgotonextstatement )
11760 IkReal j3array[1], cj3array[1], sj3array[1];
11761 bool j3valid[1]={
false};
11771 j3array[0]=((-1.5707963267949)+(x889.
value)+(((1.5707963267949)*(x890.
value))));
11772 sj3array[0]=
IKsin(j3array[0]);
11773 cj3array[0]=
IKcos(j3array[0]);
11774 if( j3array[0] >
IKPI )
11778 else if( j3array[0] < -
IKPI )
11779 { j3array[0]+=
IK2PI;
11782 for(
int ij3 = 0; ij3 < 1; ++ij3)
11784 if( !j3valid[ij3] )
11788 _ij3[0] = ij3; _ij3[1] = -1;
11789 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11793 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11796 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11798 IkReal evalcond[8];
11799 IkReal x891=
IKsin(j3);
11800 IkReal x892=
IKcos(j3);
11801 IkReal x893=(gconst14*x891);
11802 IkReal x894=((1.0)*x892);
11803 IkReal x895=(gconst13*x891);
11804 IkReal x896=(gconst13*x894);
11805 evalcond[0]=(((new_r00*x892))+((new_r10*x891))+gconst14);
11806 evalcond[1]=(((gconst14*x892))+new_r00+x895);
11807 evalcond[2]=(((new_r00*x891))+(((-1.0)*new_r10*x894))+gconst13);
11808 evalcond[3]=(((new_r01*x891))+(((-1.0)*new_r11*x894))+gconst14);
11809 evalcond[4]=((((-1.0)*x896))+new_r01+x893);
11810 evalcond[5]=((((-1.0)*x896))+new_r10+x893);
11811 evalcond[6]=(((new_r11*x891))+((new_r01*x892))+(((-1.0)*gconst13)));
11812 evalcond[7]=((((-1.0)*x895))+new_r11+(((-1.0)*gconst14*x894)));
11820 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11821 vinfos[0].jointtype = 1;
11822 vinfos[0].foffset = j0;
11823 vinfos[0].indices[0] = _ij0[0];
11824 vinfos[0].indices[1] = _ij0[1];
11825 vinfos[0].maxsolutions = _nj0;
11826 vinfos[1].jointtype = 1;
11827 vinfos[1].foffset = j1;
11828 vinfos[1].indices[0] = _ij1[0];
11829 vinfos[1].indices[1] = _ij1[1];
11830 vinfos[1].maxsolutions = _nj1;
11831 vinfos[2].jointtype = 1;
11832 vinfos[2].foffset = j2;
11833 vinfos[2].indices[0] = _ij2[0];
11834 vinfos[2].indices[1] = _ij2[1];
11835 vinfos[2].maxsolutions = _nj2;
11836 vinfos[3].jointtype = 1;
11837 vinfos[3].foffset = j3;
11838 vinfos[3].indices[0] = _ij3[0];
11839 vinfos[3].indices[1] = _ij3[1];
11840 vinfos[3].maxsolutions = _nj3;
11841 vinfos[4].jointtype = 1;
11842 vinfos[4].foffset = j4;
11843 vinfos[4].indices[0] = _ij4[0];
11844 vinfos[4].indices[1] = _ij4[1];
11845 vinfos[4].maxsolutions = _nj4;
11846 vinfos[5].jointtype = 1;
11847 vinfos[5].foffset = j5;
11848 vinfos[5].indices[0] = _ij5[0];
11849 vinfos[5].indices[1] = _ij5[1];
11850 vinfos[5].maxsolutions = _nj5;
11851 std::vector<int> vfree(0);
11864 IkReal j3array[1], cj3array[1], sj3array[1];
11865 bool j3valid[1]={
false};
11867 IkReal x897=((1.0)*new_r10);
11876 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x898.
value)))+(x899.
value));
11877 sj3array[0]=
IKsin(j3array[0]);
11878 cj3array[0]=
IKcos(j3array[0]);
11879 if( j3array[0] >
IKPI )
11883 else if( j3array[0] < -
IKPI )
11884 { j3array[0]+=
IK2PI;
11887 for(
int ij3 = 0; ij3 < 1; ++ij3)
11889 if( !j3valid[ij3] )
11893 _ij3[0] = ij3; _ij3[1] = -1;
11894 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
11898 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
11901 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11903 IkReal evalcond[8];
11904 IkReal x900=
IKsin(j3);
11905 IkReal x901=
IKcos(j3);
11906 IkReal x902=(gconst14*x900);
11907 IkReal x903=((1.0)*x901);
11908 IkReal x904=(gconst13*x900);
11909 IkReal x905=(gconst13*x903);
11910 evalcond[0]=(gconst14+((new_r10*x900))+((new_r00*x901)));
11911 evalcond[1]=(new_r00+x904+((gconst14*x901)));
11912 evalcond[2]=(gconst13+((new_r00*x900))+(((-1.0)*new_r10*x903)));
11913 evalcond[3]=((((-1.0)*new_r11*x903))+gconst14+((new_r01*x900)));
11914 evalcond[4]=((((-1.0)*x905))+new_r01+x902);
11915 evalcond[5]=((((-1.0)*x905))+new_r10+x902);
11916 evalcond[6]=(((new_r11*x900))+((new_r01*x901))+(((-1.0)*gconst13)));
11917 evalcond[7]=((((-1.0)*gconst14*x903))+(((-1.0)*x904))+new_r11);
11925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11926 vinfos[0].jointtype = 1;
11927 vinfos[0].foffset = j0;
11928 vinfos[0].indices[0] = _ij0[0];
11929 vinfos[0].indices[1] = _ij0[1];
11930 vinfos[0].maxsolutions = _nj0;
11931 vinfos[1].jointtype = 1;
11932 vinfos[1].foffset = j1;
11933 vinfos[1].indices[0] = _ij1[0];
11934 vinfos[1].indices[1] = _ij1[1];
11935 vinfos[1].maxsolutions = _nj1;
11936 vinfos[2].jointtype = 1;
11937 vinfos[2].foffset = j2;
11938 vinfos[2].indices[0] = _ij2[0];
11939 vinfos[2].indices[1] = _ij2[1];
11940 vinfos[2].maxsolutions = _nj2;
11941 vinfos[3].jointtype = 1;
11942 vinfos[3].foffset = j3;
11943 vinfos[3].indices[0] = _ij3[0];
11944 vinfos[3].indices[1] = _ij3[1];
11945 vinfos[3].maxsolutions = _nj3;
11946 vinfos[4].jointtype = 1;
11947 vinfos[4].foffset = j4;
11948 vinfos[4].indices[0] = _ij4[0];
11949 vinfos[4].indices[1] = _ij4[1];
11950 vinfos[4].maxsolutions = _nj4;
11951 vinfos[5].jointtype = 1;
11952 vinfos[5].foffset = j5;
11953 vinfos[5].indices[0] = _ij5[0];
11954 vinfos[5].indices[1] = _ij5[1];
11955 vinfos[5].maxsolutions = _nj5;
11956 std::vector<int> vfree(0);
11969 IkReal j3array[1], cj3array[1], sj3array[1];
11970 bool j3valid[1]={
false};
11972 IkReal x906=((1.0)*new_r10);
11981 j3array[0]=((-1.5707963267949)+(x907.
value)+(((1.5707963267949)*(x908.
value))));
11982 sj3array[0]=
IKsin(j3array[0]);
11983 cj3array[0]=
IKcos(j3array[0]);
11984 if( j3array[0] >
IKPI )
11988 else if( j3array[0] < -
IKPI )
11989 { j3array[0]+=
IK2PI;
11992 for(
int ij3 = 0; ij3 < 1; ++ij3)
11994 if( !j3valid[ij3] )
11998 _ij3[0] = ij3; _ij3[1] = -1;
11999 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
12003 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12006 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12008 IkReal evalcond[8];
12009 IkReal x909=
IKsin(j3);
12010 IkReal x910=
IKcos(j3);
12011 IkReal x911=(gconst14*x909);
12012 IkReal x912=((1.0)*x910);
12013 IkReal x913=(gconst13*x909);
12014 IkReal x914=(gconst13*x912);
12015 evalcond[0]=(gconst14+((new_r10*x909))+((new_r00*x910)));
12016 evalcond[1]=(((gconst14*x910))+new_r00+x913);
12017 evalcond[2]=(gconst13+((new_r00*x909))+(((-1.0)*new_r10*x912)));
12018 evalcond[3]=(gconst14+((new_r01*x909))+(((-1.0)*new_r11*x912)));
12019 evalcond[4]=((((-1.0)*x914))+new_r01+x911);
12020 evalcond[5]=((((-1.0)*x914))+new_r10+x911);
12021 evalcond[6]=(((new_r11*x909))+((new_r01*x910))+(((-1.0)*gconst13)));
12022 evalcond[7]=((((-1.0)*gconst14*x912))+(((-1.0)*x913))+new_r11);
12030 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12031 vinfos[0].jointtype = 1;
12032 vinfos[0].foffset = j0;
12033 vinfos[0].indices[0] = _ij0[0];
12034 vinfos[0].indices[1] = _ij0[1];
12035 vinfos[0].maxsolutions = _nj0;
12036 vinfos[1].jointtype = 1;
12037 vinfos[1].foffset = j1;
12038 vinfos[1].indices[0] = _ij1[0];
12039 vinfos[1].indices[1] = _ij1[1];
12040 vinfos[1].maxsolutions = _nj1;
12041 vinfos[2].jointtype = 1;
12042 vinfos[2].foffset = j2;
12043 vinfos[2].indices[0] = _ij2[0];
12044 vinfos[2].indices[1] = _ij2[1];
12045 vinfos[2].maxsolutions = _nj2;
12046 vinfos[3].jointtype = 1;
12047 vinfos[3].foffset = j3;
12048 vinfos[3].indices[0] = _ij3[0];
12049 vinfos[3].indices[1] = _ij3[1];
12050 vinfos[3].maxsolutions = _nj3;
12051 vinfos[4].jointtype = 1;
12052 vinfos[4].foffset = j4;
12053 vinfos[4].indices[0] = _ij4[0];
12054 vinfos[4].indices[1] = _ij4[1];
12055 vinfos[4].maxsolutions = _nj4;
12056 vinfos[5].jointtype = 1;
12057 vinfos[5].foffset = j5;
12058 vinfos[5].indices[0] = _ij5[0];
12059 vinfos[5].indices[1] = _ij5[1];
12060 vinfos[5].maxsolutions = _nj5;
12061 std::vector<int> vfree(0);
12073 if( bgotonextstatement )
12075 bool bgotonextstatement =
true;
12078 IkReal x917 = ((new_r10*new_r10)+(new_r00*new_r00));
12079 if(
IKabs(x917)==0){
12082 IkReal x915=
pow(x917,-0.5);
12083 IkReal x916=((1.0)*x915);
12088 IkReal gconst15=((3.14159265358979)+(((-1.0)*(x918.
value))));
12089 IkReal gconst16=(new_r10*x916);
12090 IkReal gconst17=(new_r00*x916);
12095 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5+(x919.
value))))), 6.28318530717959)));
12096 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12098 bgotonextstatement=
false;
12105 IkReal x920=((1.0)*(x923.
value));
12107 IkReal x922=((1.0)*x921);
12110 j4=3.14159265358979;
12113 j5=((3.14159265)+(((-1.0)*x920)));
12114 IkReal gconst15=((3.14159265358979)+(((-1.0)*x920)));
12115 IkReal gconst16=(new_r10*x922);
12116 IkReal gconst17=(new_r00*x922);
12117 IkReal x924=new_r00*new_r00;
12118 IkReal x925=((1.0)*new_r00);
12119 IkReal x926=((((-1.0)*new_r01*x925))+(((-1.0)*new_r10*new_r11)));
12121 IkReal x928=(new_r00*x927);
12123 j3eval[1]=((
IKabs(((((-1.0)*new_r10*x925*x927))+((new_r01*x928)))))+(
IKabs((((x924*x927))+((new_r11*x928))))));
12125 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
12133 IkReal x929=((1.0)*(x932.
value));
12135 IkReal x931=((1.0)*x930);
12138 j4=3.14159265358979;
12141 j5=((3.14159265)+(((-1.0)*x929)));
12142 IkReal gconst15=((3.14159265358979)+(((-1.0)*x929)));
12143 IkReal gconst16=(new_r10*x931);
12144 IkReal gconst17=(new_r00*x931);
12145 IkReal x933=new_r10*new_r10;
12146 IkReal x934=new_r00*new_r00;
12147 IkReal x935=((1.0)*new_r01);
12152 IkReal x936=x939.
value;
12153 IkReal x937=(new_r10*x936);
12154 IkReal x938=(new_r01*x936);
12155 j3eval[0]=((
IKabs(((((-1.0)*x935*x937*(new_r10*new_r10)))+(((-1.0)*x934*x935*x937))+((x934*x936)))))+(
IKabs((((x938*(new_r00*new_r00*new_r00)))+((new_r00*x933*x938))+((new_r00*x937))))));
12156 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
12164 IkReal x940=((1.0)*(x943.
value));
12166 IkReal x942=((1.0)*x941);
12169 j4=3.14159265358979;
12172 j5=((3.14159265)+(((-1.0)*x940)));
12173 IkReal gconst15=((3.14159265358979)+(((-1.0)*x940)));
12174 IkReal gconst16=(new_r10*x942);
12175 IkReal gconst17=(new_r00*x942);
12176 IkReal x944=new_r00*new_r00;
12177 IkReal x945=new_r10*new_r10;
12182 IkReal x946=x949.
value;
12183 IkReal x947=(new_r10*x946);
12184 IkReal x948=((1.0)*x946);
12185 j3eval[0]=((
IKabs((((new_r00*x947))+((x947*(new_r00*new_r00*new_r00)))+((new_r00*x947*(new_r10*new_r10))))))+(
IKabs(((((-1.0)*x944*x945*x948))+((x944*x946))+(((-1.0)*x948*(x945*x945)))))));
12186 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
12189 IkReal evalcond[3];
12190 bool bgotonextstatement =
true;
12193 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
12194 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12196 bgotonextstatement=
false;
12203 IkReal x950=((1.0)*(x951.
value));
12206 j4=3.14159265358979;
12209 j5=((3.14159265)+(((-1.0)*x950)));
12212 IkReal gconst15=((3.14159265358979)+(((-1.0)*x950)));
12213 IkReal x952 = new_r10*new_r10;
12214 if(
IKabs(x952)==0){
12217 IkReal gconst16=((1.0)*new_r10*(
pow(x952,-0.5)));
12220 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
12223 IkReal j3array[2], cj3array[2], sj3array[2];
12224 bool j3valid[2]={
false};
12230 cj3array[0]=(new_r01*(x953.
value));
12233 j3valid[0] = j3valid[1] =
true;
12234 j3array[0] =
IKacos(cj3array[0]);
12235 sj3array[0] =
IKsin(j3array[0]);
12236 cj3array[1] = cj3array[0];
12237 j3array[1] = -j3array[0];
12238 sj3array[1] = -sj3array[0];
12240 else if( isnan(cj3array[0]) )
12244 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12246 for(
int ij3 = 0; ij3 < 2; ++ij3)
12248 if( !j3valid[ij3] )
12252 _ij3[0] = ij3; _ij3[1] = -1;
12253 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
12257 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12260 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12262 IkReal evalcond[6];
12263 IkReal x954=
IKsin(j3);
12264 IkReal x955=
IKcos(j3);
12265 IkReal x956=((1.0)*gconst16);
12266 evalcond[0]=(new_r01*x954);
12267 evalcond[1]=(new_r10*x954);
12268 evalcond[2]=(gconst16*x954);
12269 evalcond[3]=(gconst16+(((-1.0)*new_r10*x955)));
12270 evalcond[4]=(new_r10+(((-1.0)*x955*x956)));
12271 evalcond[5]=(((new_r01*x955))+(((-1.0)*x956)));
12279 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12280 vinfos[0].jointtype = 1;
12281 vinfos[0].foffset = j0;
12282 vinfos[0].indices[0] = _ij0[0];
12283 vinfos[0].indices[1] = _ij0[1];
12284 vinfos[0].maxsolutions = _nj0;
12285 vinfos[1].jointtype = 1;
12286 vinfos[1].foffset = j1;
12287 vinfos[1].indices[0] = _ij1[0];
12288 vinfos[1].indices[1] = _ij1[1];
12289 vinfos[1].maxsolutions = _nj1;
12290 vinfos[2].jointtype = 1;
12291 vinfos[2].foffset = j2;
12292 vinfos[2].indices[0] = _ij2[0];
12293 vinfos[2].indices[1] = _ij2[1];
12294 vinfos[2].maxsolutions = _nj2;
12295 vinfos[3].jointtype = 1;
12296 vinfos[3].foffset = j3;
12297 vinfos[3].indices[0] = _ij3[0];
12298 vinfos[3].indices[1] = _ij3[1];
12299 vinfos[3].maxsolutions = _nj3;
12300 vinfos[4].jointtype = 1;
12301 vinfos[4].foffset = j4;
12302 vinfos[4].indices[0] = _ij4[0];
12303 vinfos[4].indices[1] = _ij4[1];
12304 vinfos[4].maxsolutions = _nj4;
12305 vinfos[5].jointtype = 1;
12306 vinfos[5].foffset = j5;
12307 vinfos[5].indices[0] = _ij5[0];
12308 vinfos[5].indices[1] = _ij5[1];
12309 vinfos[5].maxsolutions = _nj5;
12310 std::vector<int> vfree(0);
12319 IkReal j3array[2], cj3array[2], sj3array[2];
12320 bool j3valid[2]={
false};
12326 cj3array[0]=(gconst16*(x957.
value));
12329 j3valid[0] = j3valid[1] =
true;
12330 j3array[0] =
IKacos(cj3array[0]);
12331 sj3array[0] =
IKsin(j3array[0]);
12332 cj3array[1] = cj3array[0];
12333 j3array[1] = -j3array[0];
12334 sj3array[1] = -sj3array[0];
12336 else if( isnan(cj3array[0]) )
12340 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12342 for(
int ij3 = 0; ij3 < 2; ++ij3)
12344 if( !j3valid[ij3] )
12348 _ij3[0] = ij3; _ij3[1] = -1;
12349 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
12353 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12356 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12358 IkReal evalcond[6];
12359 IkReal x958=
IKsin(j3);
12360 IkReal x959=
IKcos(j3);
12361 IkReal x960=((1.0)*gconst16);
12362 IkReal x961=(x959*x960);
12363 evalcond[0]=(new_r01*x958);
12364 evalcond[1]=(new_r10*x958);
12365 evalcond[2]=(gconst16*x958);
12366 evalcond[3]=(new_r01+(((-1.0)*x961)));
12367 evalcond[4]=(new_r10+(((-1.0)*x961)));
12368 evalcond[5]=(((new_r01*x959))+(((-1.0)*x960)));
12376 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12377 vinfos[0].jointtype = 1;
12378 vinfos[0].foffset = j0;
12379 vinfos[0].indices[0] = _ij0[0];
12380 vinfos[0].indices[1] = _ij0[1];
12381 vinfos[0].maxsolutions = _nj0;
12382 vinfos[1].jointtype = 1;
12383 vinfos[1].foffset = j1;
12384 vinfos[1].indices[0] = _ij1[0];
12385 vinfos[1].indices[1] = _ij1[1];
12386 vinfos[1].maxsolutions = _nj1;
12387 vinfos[2].jointtype = 1;
12388 vinfos[2].foffset = j2;
12389 vinfos[2].indices[0] = _ij2[0];
12390 vinfos[2].indices[1] = _ij2[1];
12391 vinfos[2].maxsolutions = _nj2;
12392 vinfos[3].jointtype = 1;
12393 vinfos[3].foffset = j3;
12394 vinfos[3].indices[0] = _ij3[0];
12395 vinfos[3].indices[1] = _ij3[1];
12396 vinfos[3].maxsolutions = _nj3;
12397 vinfos[4].jointtype = 1;
12398 vinfos[4].foffset = j4;
12399 vinfos[4].indices[0] = _ij4[0];
12400 vinfos[4].indices[1] = _ij4[1];
12401 vinfos[4].maxsolutions = _nj4;
12402 vinfos[5].jointtype = 1;
12403 vinfos[5].foffset = j5;
12404 vinfos[5].indices[0] = _ij5[0];
12405 vinfos[5].indices[1] = _ij5[1];
12406 vinfos[5].maxsolutions = _nj5;
12407 std::vector<int> vfree(0);
12419 if( bgotonextstatement )
12421 bool bgotonextstatement =
true;
12424 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
12425 evalcond[1]=gconst17;
12426 evalcond[2]=gconst16;
12427 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
12429 bgotonextstatement=
false;
12436 IkReal x962=((1.0)*(x963.
value));
12439 j4=3.14159265358979;
12442 j5=((3.14159265)+(((-1.0)*x962)));
12447 IkReal gconst15=((3.14159265358979)+(((-1.0)*x962)));
12448 IkReal gconst16=((1.0)*new_r10);
12449 IkReal gconst17=((1.0)*new_r00);
12451 j3eval[1]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs(((1.0)*new_r00*new_r10))));
12453 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
12461 IkReal x964=((1.0)*(x965.
value));
12464 j4=3.14159265358979;
12467 j5=((3.14159265)+(((-1.0)*x964)));
12472 IkReal gconst15=((3.14159265358979)+(((-1.0)*x964)));
12473 IkReal gconst16=((1.0)*new_r10);
12474 IkReal gconst17=((1.0)*new_r00);
12477 j3eval[2]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs(((1.0)*new_r00*new_r10))));
12478 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
12486 IkReal x966=((1.0)*(x967.
value));
12489 j4=3.14159265358979;
12492 j5=((3.14159265)+(((-1.0)*x966)));
12497 IkReal gconst15=((3.14159265358979)+(((-1.0)*x966)));
12498 IkReal gconst16=((1.0)*new_r10);
12499 IkReal gconst17=((1.0)*new_r00);
12501 j3eval[1]=((
IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(
IKabs(((2.0)*new_r00*new_r10))));
12503 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
12510 IkReal j3array[1], cj3array[1], sj3array[1];
12511 bool j3valid[1]={
false};
12521 j3array[0]=((-1.5707963267949)+(x968.
value)+(((1.5707963267949)*(x969.
value))));
12522 sj3array[0]=
IKsin(j3array[0]);
12523 cj3array[0]=
IKcos(j3array[0]);
12524 if( j3array[0] >
IKPI )
12528 else if( j3array[0] < -
IKPI )
12529 { j3array[0]+=
IK2PI;
12532 for(
int ij3 = 0; ij3 < 1; ++ij3)
12534 if( !j3valid[ij3] )
12538 _ij3[0] = ij3; _ij3[1] = -1;
12539 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
12543 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12546 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12548 IkReal evalcond[6];
12549 IkReal x970=
IKsin(j3);
12550 IkReal x971=
IKcos(j3);
12551 IkReal x972=(gconst17*x970);
12552 IkReal x973=(gconst16*x970);
12553 IkReal x974=(gconst17*x971);
12554 IkReal x975=((1.0)*x971);
12555 IkReal x976=(gconst16*x975);
12556 evalcond[0]=(x972+(((-1.0)*x976)));
12557 evalcond[1]=(gconst17+((new_r10*x970))+((new_r00*x971)));
12558 evalcond[2]=(new_r00+x973+x974);
12559 evalcond[3]=(gconst16+(((-1.0)*new_r10*x975))+((new_r00*x970)));
12560 evalcond[4]=((((-1.0)*x974))+(((-1.0)*x973)));
12561 evalcond[5]=(new_r10+x972+(((-1.0)*x976)));
12569 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12570 vinfos[0].jointtype = 1;
12571 vinfos[0].foffset = j0;
12572 vinfos[0].indices[0] = _ij0[0];
12573 vinfos[0].indices[1] = _ij0[1];
12574 vinfos[0].maxsolutions = _nj0;
12575 vinfos[1].jointtype = 1;
12576 vinfos[1].foffset = j1;
12577 vinfos[1].indices[0] = _ij1[0];
12578 vinfos[1].indices[1] = _ij1[1];
12579 vinfos[1].maxsolutions = _nj1;
12580 vinfos[2].jointtype = 1;
12581 vinfos[2].foffset = j2;
12582 vinfos[2].indices[0] = _ij2[0];
12583 vinfos[2].indices[1] = _ij2[1];
12584 vinfos[2].maxsolutions = _nj2;
12585 vinfos[3].jointtype = 1;
12586 vinfos[3].foffset = j3;
12587 vinfos[3].indices[0] = _ij3[0];
12588 vinfos[3].indices[1] = _ij3[1];
12589 vinfos[3].maxsolutions = _nj3;
12590 vinfos[4].jointtype = 1;
12591 vinfos[4].foffset = j4;
12592 vinfos[4].indices[0] = _ij4[0];
12593 vinfos[4].indices[1] = _ij4[1];
12594 vinfos[4].maxsolutions = _nj4;
12595 vinfos[5].jointtype = 1;
12596 vinfos[5].foffset = j5;
12597 vinfos[5].indices[0] = _ij5[0];
12598 vinfos[5].indices[1] = _ij5[1];
12599 vinfos[5].maxsolutions = _nj5;
12600 std::vector<int> vfree(0);
12613 IkReal j3array[1], cj3array[1], sj3array[1];
12614 bool j3valid[1]={
false};
12624 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x977.
value)))+(x978.
value));
12625 sj3array[0]=
IKsin(j3array[0]);
12626 cj3array[0]=
IKcos(j3array[0]);
12627 if( j3array[0] >
IKPI )
12631 else if( j3array[0] < -
IKPI )
12632 { j3array[0]+=
IK2PI;
12635 for(
int ij3 = 0; ij3 < 1; ++ij3)
12637 if( !j3valid[ij3] )
12641 _ij3[0] = ij3; _ij3[1] = -1;
12642 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
12646 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12649 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12651 IkReal evalcond[6];
12652 IkReal x979=
IKsin(j3);
12653 IkReal x980=
IKcos(j3);
12654 IkReal x981=(gconst17*x979);
12655 IkReal x982=(gconst16*x979);
12656 IkReal x983=(gconst17*x980);
12657 IkReal x984=((1.0)*x980);
12658 IkReal x985=(gconst16*x984);
12659 evalcond[0]=((((-1.0)*x985))+x981);
12660 evalcond[1]=(((new_r00*x980))+gconst17+((new_r10*x979)));
12661 evalcond[2]=(new_r00+x982+x983);
12662 evalcond[3]=(gconst16+(((-1.0)*new_r10*x984))+((new_r00*x979)));
12663 evalcond[4]=((((-1.0)*x983))+(((-1.0)*x982)));
12664 evalcond[5]=((((-1.0)*x985))+new_r10+x981);
12672 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12673 vinfos[0].jointtype = 1;
12674 vinfos[0].foffset = j0;
12675 vinfos[0].indices[0] = _ij0[0];
12676 vinfos[0].indices[1] = _ij0[1];
12677 vinfos[0].maxsolutions = _nj0;
12678 vinfos[1].jointtype = 1;
12679 vinfos[1].foffset = j1;
12680 vinfos[1].indices[0] = _ij1[0];
12681 vinfos[1].indices[1] = _ij1[1];
12682 vinfos[1].maxsolutions = _nj1;
12683 vinfos[2].jointtype = 1;
12684 vinfos[2].foffset = j2;
12685 vinfos[2].indices[0] = _ij2[0];
12686 vinfos[2].indices[1] = _ij2[1];
12687 vinfos[2].maxsolutions = _nj2;
12688 vinfos[3].jointtype = 1;
12689 vinfos[3].foffset = j3;
12690 vinfos[3].indices[0] = _ij3[0];
12691 vinfos[3].indices[1] = _ij3[1];
12692 vinfos[3].maxsolutions = _nj3;
12693 vinfos[4].jointtype = 1;
12694 vinfos[4].foffset = j4;
12695 vinfos[4].indices[0] = _ij4[0];
12696 vinfos[4].indices[1] = _ij4[1];
12697 vinfos[4].maxsolutions = _nj4;
12698 vinfos[5].jointtype = 1;
12699 vinfos[5].foffset = j5;
12700 vinfos[5].indices[0] = _ij5[0];
12701 vinfos[5].indices[1] = _ij5[1];
12702 vinfos[5].maxsolutions = _nj5;
12703 std::vector<int> vfree(0);
12716 IkReal j3array[1], cj3array[1], sj3array[1];
12717 bool j3valid[1]={
false};
12727 j3array[0]=((-1.5707963267949)+(x986.
value)+(((1.5707963267949)*(x987.
value))));
12728 sj3array[0]=
IKsin(j3array[0]);
12729 cj3array[0]=
IKcos(j3array[0]);
12730 if( j3array[0] >
IKPI )
12734 else if( j3array[0] < -
IKPI )
12735 { j3array[0]+=
IK2PI;
12738 for(
int ij3 = 0; ij3 < 1; ++ij3)
12740 if( !j3valid[ij3] )
12744 _ij3[0] = ij3; _ij3[1] = -1;
12745 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
12749 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12752 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12754 IkReal evalcond[6];
12755 IkReal x988=
IKsin(j3);
12756 IkReal x989=
IKcos(j3);
12757 IkReal x990=(gconst17*x988);
12758 IkReal x991=(gconst16*x988);
12759 IkReal x992=(gconst17*x989);
12760 IkReal x993=((1.0)*x989);
12761 IkReal x994=(gconst16*x993);
12762 evalcond[0]=((((-1.0)*x994))+x990);
12763 evalcond[1]=(((new_r00*x989))+gconst17+((new_r10*x988)));
12764 evalcond[2]=(new_r00+x992+x991);
12765 evalcond[3]=(((new_r00*x988))+(((-1.0)*new_r10*x993))+gconst16);
12766 evalcond[4]=((((-1.0)*x992))+(((-1.0)*x991)));
12767 evalcond[5]=((((-1.0)*x994))+new_r10+x990);
12775 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12776 vinfos[0].jointtype = 1;
12777 vinfos[0].foffset = j0;
12778 vinfos[0].indices[0] = _ij0[0];
12779 vinfos[0].indices[1] = _ij0[1];
12780 vinfos[0].maxsolutions = _nj0;
12781 vinfos[1].jointtype = 1;
12782 vinfos[1].foffset = j1;
12783 vinfos[1].indices[0] = _ij1[0];
12784 vinfos[1].indices[1] = _ij1[1];
12785 vinfos[1].maxsolutions = _nj1;
12786 vinfos[2].jointtype = 1;
12787 vinfos[2].foffset = j2;
12788 vinfos[2].indices[0] = _ij2[0];
12789 vinfos[2].indices[1] = _ij2[1];
12790 vinfos[2].maxsolutions = _nj2;
12791 vinfos[3].jointtype = 1;
12792 vinfos[3].foffset = j3;
12793 vinfos[3].indices[0] = _ij3[0];
12794 vinfos[3].indices[1] = _ij3[1];
12795 vinfos[3].maxsolutions = _nj3;
12796 vinfos[4].jointtype = 1;
12797 vinfos[4].foffset = j4;
12798 vinfos[4].indices[0] = _ij4[0];
12799 vinfos[4].indices[1] = _ij4[1];
12800 vinfos[4].maxsolutions = _nj4;
12801 vinfos[5].jointtype = 1;
12802 vinfos[5].foffset = j5;
12803 vinfos[5].indices[0] = _ij5[0];
12804 vinfos[5].indices[1] = _ij5[1];
12805 vinfos[5].maxsolutions = _nj5;
12806 std::vector<int> vfree(0);
12818 if( bgotonextstatement )
12820 bool bgotonextstatement =
true;
12823 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
12824 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12826 bgotonextstatement=
false;
12833 IkReal x995=((1.0)*(x996.
value));
12836 j4=3.14159265358979;
12839 j5=((3.14159265)+(((-1.0)*x995)));
12842 IkReal gconst15=((3.14159265358979)+(((-1.0)*x995)));
12844 IkReal x997 = new_r00*new_r00;
12845 if(
IKabs(x997)==0){
12848 IkReal gconst17=((1.0)*new_r00*(
pow(x997,-0.5)));
12850 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
12853 IkReal j3array[2], cj3array[2], sj3array[2];
12854 bool j3valid[2]={
false};
12860 cj3array[0]=(new_r11*(x998.
value));
12863 j3valid[0] = j3valid[1] =
true;
12864 j3array[0] =
IKacos(cj3array[0]);
12865 sj3array[0] =
IKsin(j3array[0]);
12866 cj3array[1] = cj3array[0];
12867 j3array[1] = -j3array[0];
12868 sj3array[1] = -sj3array[0];
12870 else if( isnan(cj3array[0]) )
12874 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12876 for(
int ij3 = 0; ij3 < 2; ++ij3)
12878 if( !j3valid[ij3] )
12882 _ij3[0] = ij3; _ij3[1] = -1;
12883 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
12887 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12890 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12892 IkReal evalcond[6];
12893 IkReal x999=
IKsin(j3);
12894 IkReal x1000=
IKcos(j3);
12895 evalcond[0]=(new_r00*x999);
12896 evalcond[1]=(new_r11*x999);
12897 evalcond[2]=(gconst17*x999);
12898 evalcond[3]=(gconst17+((new_r00*x1000)));
12899 evalcond[4]=(((gconst17*x1000))+new_r00);
12900 evalcond[5]=((((-1.0)*new_r11*x1000))+gconst17);
12908 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12909 vinfos[0].jointtype = 1;
12910 vinfos[0].foffset = j0;
12911 vinfos[0].indices[0] = _ij0[0];
12912 vinfos[0].indices[1] = _ij0[1];
12913 vinfos[0].maxsolutions = _nj0;
12914 vinfos[1].jointtype = 1;
12915 vinfos[1].foffset = j1;
12916 vinfos[1].indices[0] = _ij1[0];
12917 vinfos[1].indices[1] = _ij1[1];
12918 vinfos[1].maxsolutions = _nj1;
12919 vinfos[2].jointtype = 1;
12920 vinfos[2].foffset = j2;
12921 vinfos[2].indices[0] = _ij2[0];
12922 vinfos[2].indices[1] = _ij2[1];
12923 vinfos[2].maxsolutions = _nj2;
12924 vinfos[3].jointtype = 1;
12925 vinfos[3].foffset = j3;
12926 vinfos[3].indices[0] = _ij3[0];
12927 vinfos[3].indices[1] = _ij3[1];
12928 vinfos[3].maxsolutions = _nj3;
12929 vinfos[4].jointtype = 1;
12930 vinfos[4].foffset = j4;
12931 vinfos[4].indices[0] = _ij4[0];
12932 vinfos[4].indices[1] = _ij4[1];
12933 vinfos[4].maxsolutions = _nj4;
12934 vinfos[5].jointtype = 1;
12935 vinfos[5].foffset = j5;
12936 vinfos[5].indices[0] = _ij5[0];
12937 vinfos[5].indices[1] = _ij5[1];
12938 vinfos[5].maxsolutions = _nj5;
12939 std::vector<int> vfree(0);
12948 IkReal j3array[2], cj3array[2], sj3array[2];
12949 bool j3valid[2]={
false};
12955 cj3array[0]=(gconst17*(x1001.
value));
12958 j3valid[0] = j3valid[1] =
true;
12959 j3array[0] =
IKacos(cj3array[0]);
12960 sj3array[0] =
IKsin(j3array[0]);
12961 cj3array[1] = cj3array[0];
12962 j3array[1] = -j3array[0];
12963 sj3array[1] = -sj3array[0];
12965 else if( isnan(cj3array[0]) )
12969 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12971 for(
int ij3 = 0; ij3 < 2; ++ij3)
12973 if( !j3valid[ij3] )
12977 _ij3[0] = ij3; _ij3[1] = -1;
12978 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
12982 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
12985 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12987 IkReal evalcond[6];
12988 IkReal x1002=
IKsin(j3);
12989 IkReal x1003=
IKcos(j3);
12990 IkReal x1004=(gconst17*x1003);
12991 evalcond[0]=(new_r00*x1002);
12992 evalcond[1]=(new_r11*x1002);
12993 evalcond[2]=(gconst17*x1002);
12994 evalcond[3]=(gconst17+((new_r00*x1003)));
12995 evalcond[4]=(x1004+new_r00);
12996 evalcond[5]=((((-1.0)*x1004))+new_r11);
13004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13005 vinfos[0].jointtype = 1;
13006 vinfos[0].foffset = j0;
13007 vinfos[0].indices[0] = _ij0[0];
13008 vinfos[0].indices[1] = _ij0[1];
13009 vinfos[0].maxsolutions = _nj0;
13010 vinfos[1].jointtype = 1;
13011 vinfos[1].foffset = j1;
13012 vinfos[1].indices[0] = _ij1[0];
13013 vinfos[1].indices[1] = _ij1[1];
13014 vinfos[1].maxsolutions = _nj1;
13015 vinfos[2].jointtype = 1;
13016 vinfos[2].foffset = j2;
13017 vinfos[2].indices[0] = _ij2[0];
13018 vinfos[2].indices[1] = _ij2[1];
13019 vinfos[2].maxsolutions = _nj2;
13020 vinfos[3].jointtype = 1;
13021 vinfos[3].foffset = j3;
13022 vinfos[3].indices[0] = _ij3[0];
13023 vinfos[3].indices[1] = _ij3[1];
13024 vinfos[3].maxsolutions = _nj3;
13025 vinfos[4].jointtype = 1;
13026 vinfos[4].foffset = j4;
13027 vinfos[4].indices[0] = _ij4[0];
13028 vinfos[4].indices[1] = _ij4[1];
13029 vinfos[4].maxsolutions = _nj4;
13030 vinfos[5].jointtype = 1;
13031 vinfos[5].foffset = j5;
13032 vinfos[5].indices[0] = _ij5[0];
13033 vinfos[5].indices[1] = _ij5[1];
13034 vinfos[5].maxsolutions = _nj5;
13035 std::vector<int> vfree(0);
13047 if( bgotonextstatement )
13049 bool bgotonextstatement =
true;
13052 evalcond[0]=
IKabs(new_r00);
13053 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
13055 bgotonextstatement=
false;
13062 IkReal x1005=((1.0)*(x1006.
value));
13065 j4=3.14159265358979;
13068 j5=((3.14159265)+(((-1.0)*x1005)));
13070 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1005)));
13071 IkReal x1007 = new_r10*new_r10;
13072 if(
IKabs(x1007)==0){
13075 IkReal gconst16=((1.0)*new_r10*(
pow(x1007,-0.5)));
13077 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
13078 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13086 IkReal x1008=((1.0)*(x1009.
value));
13089 j4=3.14159265358979;
13092 j5=((3.14159265)+(((-1.0)*x1008)));
13094 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1008)));
13095 IkReal x1010 = new_r10*new_r10;
13096 if(
IKabs(x1010)==0){
13099 IkReal gconst16=((1.0)*new_r10*(
pow(x1010,-0.5)));
13101 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
13102 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13110 IkReal x1011=((1.0)*(x1012.
value));
13113 j4=3.14159265358979;
13116 j5=((3.14159265)+(((-1.0)*x1011)));
13118 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1011)));
13119 IkReal x1013 = new_r10*new_r10;
13120 if(
IKabs(x1013)==0){
13123 IkReal gconst16=((1.0)*new_r10*(
pow(x1013,-0.5)));
13126 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13133 IkReal j3array[1], cj3array[1], sj3array[1];
13134 bool j3valid[1]={
false};
13147 sj3array[0]=
IKsin(j3array[0]);
13148 cj3array[0]=
IKcos(j3array[0]);
13149 if( j3array[0] >
IKPI )
13153 else if( j3array[0] < -
IKPI )
13154 { j3array[0]+=
IK2PI;
13157 for(
int ij3 = 0; ij3 < 1; ++ij3)
13159 if( !j3valid[ij3] )
13163 _ij3[0] = ij3; _ij3[1] = -1;
13164 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13168 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13171 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13173 IkReal evalcond[8];
13174 IkReal x1016=
IKsin(j3);
13175 IkReal x1017=
IKcos(j3);
13176 IkReal x1018=((1.0)*gconst16);
13177 IkReal x1019=((1.0)*x1017);
13178 IkReal x1020=(x1017*x1018);
13179 evalcond[0]=(new_r10*x1016);
13180 evalcond[1]=(gconst16*x1016);
13181 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1019)));
13182 evalcond[3]=((((-1.0)*x1020))+new_r01);
13183 evalcond[4]=((((-1.0)*x1016*x1018))+new_r11);
13184 evalcond[5]=((((-1.0)*x1020))+new_r10);
13185 evalcond[6]=((((-1.0)*new_r11*x1019))+((new_r01*x1016)));
13186 evalcond[7]=(((new_r11*x1016))+(((-1.0)*x1018))+((new_r01*x1017)));
13194 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13195 vinfos[0].jointtype = 1;
13196 vinfos[0].foffset = j0;
13197 vinfos[0].indices[0] = _ij0[0];
13198 vinfos[0].indices[1] = _ij0[1];
13199 vinfos[0].maxsolutions = _nj0;
13200 vinfos[1].jointtype = 1;
13201 vinfos[1].foffset = j1;
13202 vinfos[1].indices[0] = _ij1[0];
13203 vinfos[1].indices[1] = _ij1[1];
13204 vinfos[1].maxsolutions = _nj1;
13205 vinfos[2].jointtype = 1;
13206 vinfos[2].foffset = j2;
13207 vinfos[2].indices[0] = _ij2[0];
13208 vinfos[2].indices[1] = _ij2[1];
13209 vinfos[2].maxsolutions = _nj2;
13210 vinfos[3].jointtype = 1;
13211 vinfos[3].foffset = j3;
13212 vinfos[3].indices[0] = _ij3[0];
13213 vinfos[3].indices[1] = _ij3[1];
13214 vinfos[3].maxsolutions = _nj3;
13215 vinfos[4].jointtype = 1;
13216 vinfos[4].foffset = j4;
13217 vinfos[4].indices[0] = _ij4[0];
13218 vinfos[4].indices[1] = _ij4[1];
13219 vinfos[4].maxsolutions = _nj4;
13220 vinfos[5].jointtype = 1;
13221 vinfos[5].foffset = j5;
13222 vinfos[5].indices[0] = _ij5[0];
13223 vinfos[5].indices[1] = _ij5[1];
13224 vinfos[5].maxsolutions = _nj5;
13225 std::vector<int> vfree(0);
13238 IkReal j3array[1], cj3array[1], sj3array[1];
13239 bool j3valid[1]={
false};
13249 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1021.
value)))+(x1022.
value));
13250 sj3array[0]=
IKsin(j3array[0]);
13251 cj3array[0]=
IKcos(j3array[0]);
13252 if( j3array[0] >
IKPI )
13256 else if( j3array[0] < -
IKPI )
13257 { j3array[0]+=
IK2PI;
13260 for(
int ij3 = 0; ij3 < 1; ++ij3)
13262 if( !j3valid[ij3] )
13266 _ij3[0] = ij3; _ij3[1] = -1;
13267 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13271 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13274 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13276 IkReal evalcond[8];
13277 IkReal x1023=
IKsin(j3);
13278 IkReal x1024=
IKcos(j3);
13279 IkReal x1025=((1.0)*gconst16);
13280 IkReal x1026=((1.0)*x1024);
13281 IkReal x1027=(x1024*x1025);
13282 evalcond[0]=(new_r10*x1023);
13283 evalcond[1]=(gconst16*x1023);
13284 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1026)));
13285 evalcond[3]=((((-1.0)*x1027))+new_r01);
13286 evalcond[4]=((((-1.0)*x1023*x1025))+new_r11);
13287 evalcond[5]=((((-1.0)*x1027))+new_r10);
13288 evalcond[6]=((((-1.0)*new_r11*x1026))+((new_r01*x1023)));
13289 evalcond[7]=(((new_r11*x1023))+(((-1.0)*x1025))+((new_r01*x1024)));
13297 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13298 vinfos[0].jointtype = 1;
13299 vinfos[0].foffset = j0;
13300 vinfos[0].indices[0] = _ij0[0];
13301 vinfos[0].indices[1] = _ij0[1];
13302 vinfos[0].maxsolutions = _nj0;
13303 vinfos[1].jointtype = 1;
13304 vinfos[1].foffset = j1;
13305 vinfos[1].indices[0] = _ij1[0];
13306 vinfos[1].indices[1] = _ij1[1];
13307 vinfos[1].maxsolutions = _nj1;
13308 vinfos[2].jointtype = 1;
13309 vinfos[2].foffset = j2;
13310 vinfos[2].indices[0] = _ij2[0];
13311 vinfos[2].indices[1] = _ij2[1];
13312 vinfos[2].maxsolutions = _nj2;
13313 vinfos[3].jointtype = 1;
13314 vinfos[3].foffset = j3;
13315 vinfos[3].indices[0] = _ij3[0];
13316 vinfos[3].indices[1] = _ij3[1];
13317 vinfos[3].maxsolutions = _nj3;
13318 vinfos[4].jointtype = 1;
13319 vinfos[4].foffset = j4;
13320 vinfos[4].indices[0] = _ij4[0];
13321 vinfos[4].indices[1] = _ij4[1];
13322 vinfos[4].maxsolutions = _nj4;
13323 vinfos[5].jointtype = 1;
13324 vinfos[5].foffset = j5;
13325 vinfos[5].indices[0] = _ij5[0];
13326 vinfos[5].indices[1] = _ij5[1];
13327 vinfos[5].maxsolutions = _nj5;
13328 std::vector<int> vfree(0);
13341 IkReal j3array[1], cj3array[1], sj3array[1];
13342 bool j3valid[1]={
false};
13352 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1028.
value)))+(x1029.
value));
13353 sj3array[0]=
IKsin(j3array[0]);
13354 cj3array[0]=
IKcos(j3array[0]);
13355 if( j3array[0] >
IKPI )
13359 else if( j3array[0] < -
IKPI )
13360 { j3array[0]+=
IK2PI;
13363 for(
int ij3 = 0; ij3 < 1; ++ij3)
13365 if( !j3valid[ij3] )
13369 _ij3[0] = ij3; _ij3[1] = -1;
13370 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13374 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13377 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13379 IkReal evalcond[8];
13380 IkReal x1030=
IKsin(j3);
13381 IkReal x1031=
IKcos(j3);
13382 IkReal x1032=((1.0)*gconst16);
13383 IkReal x1033=((1.0)*x1031);
13384 IkReal x1034=(x1031*x1032);
13385 evalcond[0]=(new_r10*x1030);
13386 evalcond[1]=(gconst16*x1030);
13387 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1033)));
13388 evalcond[3]=((((-1.0)*x1034))+new_r01);
13389 evalcond[4]=((((-1.0)*x1030*x1032))+new_r11);
13390 evalcond[5]=((((-1.0)*x1034))+new_r10);
13391 evalcond[6]=((((-1.0)*new_r11*x1033))+((new_r01*x1030)));
13392 evalcond[7]=((((-1.0)*x1032))+((new_r11*x1030))+((new_r01*x1031)));
13400 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13401 vinfos[0].jointtype = 1;
13402 vinfos[0].foffset = j0;
13403 vinfos[0].indices[0] = _ij0[0];
13404 vinfos[0].indices[1] = _ij0[1];
13405 vinfos[0].maxsolutions = _nj0;
13406 vinfos[1].jointtype = 1;
13407 vinfos[1].foffset = j1;
13408 vinfos[1].indices[0] = _ij1[0];
13409 vinfos[1].indices[1] = _ij1[1];
13410 vinfos[1].maxsolutions = _nj1;
13411 vinfos[2].jointtype = 1;
13412 vinfos[2].foffset = j2;
13413 vinfos[2].indices[0] = _ij2[0];
13414 vinfos[2].indices[1] = _ij2[1];
13415 vinfos[2].maxsolutions = _nj2;
13416 vinfos[3].jointtype = 1;
13417 vinfos[3].foffset = j3;
13418 vinfos[3].indices[0] = _ij3[0];
13419 vinfos[3].indices[1] = _ij3[1];
13420 vinfos[3].maxsolutions = _nj3;
13421 vinfos[4].jointtype = 1;
13422 vinfos[4].foffset = j4;
13423 vinfos[4].indices[0] = _ij4[0];
13424 vinfos[4].indices[1] = _ij4[1];
13425 vinfos[4].maxsolutions = _nj4;
13426 vinfos[5].jointtype = 1;
13427 vinfos[5].foffset = j5;
13428 vinfos[5].indices[0] = _ij5[0];
13429 vinfos[5].indices[1] = _ij5[1];
13430 vinfos[5].maxsolutions = _nj5;
13431 std::vector<int> vfree(0);
13443 if( bgotonextstatement )
13445 bool bgotonextstatement =
true;
13450 bgotonextstatement=
false;
13455 if( bgotonextstatement )
13467 IkReal j3array[1], cj3array[1], sj3array[1];
13468 bool j3valid[1]={
false};
13478 j3array[0]=((-1.5707963267949)+(x1035.
value)+(((1.5707963267949)*(x1036.
value))));
13479 sj3array[0]=
IKsin(j3array[0]);
13480 cj3array[0]=
IKcos(j3array[0]);
13481 if( j3array[0] >
IKPI )
13485 else if( j3array[0] < -
IKPI )
13486 { j3array[0]+=
IK2PI;
13489 for(
int ij3 = 0; ij3 < 1; ++ij3)
13491 if( !j3valid[ij3] )
13495 _ij3[0] = ij3; _ij3[1] = -1;
13496 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13500 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13503 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13505 IkReal evalcond[8];
13506 IkReal x1037=
IKsin(j3);
13507 IkReal x1038=
IKcos(j3);
13508 IkReal x1039=((1.0)*gconst16);
13509 IkReal x1040=(gconst17*x1037);
13510 IkReal x1041=(gconst17*x1038);
13511 IkReal x1042=((1.0)*x1038);
13512 IkReal x1043=(x1038*x1039);
13513 evalcond[0]=(gconst17+((new_r10*x1037))+((new_r00*x1038)));
13514 evalcond[1]=(x1041+((gconst16*x1037))+new_r00);
13515 evalcond[2]=(gconst16+((new_r00*x1037))+(((-1.0)*new_r10*x1042)));
13516 evalcond[3]=(gconst17+((new_r01*x1037))+(((-1.0)*new_r11*x1042)));
13517 evalcond[4]=(x1040+new_r01+(((-1.0)*x1043)));
13518 evalcond[5]=(x1040+new_r10+(((-1.0)*x1043)));
13519 evalcond[6]=((((-1.0)*x1039))+((new_r11*x1037))+((new_r01*x1038)));
13520 evalcond[7]=((((-1.0)*x1037*x1039))+(((-1.0)*x1041))+new_r11);
13528 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13529 vinfos[0].jointtype = 1;
13530 vinfos[0].foffset = j0;
13531 vinfos[0].indices[0] = _ij0[0];
13532 vinfos[0].indices[1] = _ij0[1];
13533 vinfos[0].maxsolutions = _nj0;
13534 vinfos[1].jointtype = 1;
13535 vinfos[1].foffset = j1;
13536 vinfos[1].indices[0] = _ij1[0];
13537 vinfos[1].indices[1] = _ij1[1];
13538 vinfos[1].maxsolutions = _nj1;
13539 vinfos[2].jointtype = 1;
13540 vinfos[2].foffset = j2;
13541 vinfos[2].indices[0] = _ij2[0];
13542 vinfos[2].indices[1] = _ij2[1];
13543 vinfos[2].maxsolutions = _nj2;
13544 vinfos[3].jointtype = 1;
13545 vinfos[3].foffset = j3;
13546 vinfos[3].indices[0] = _ij3[0];
13547 vinfos[3].indices[1] = _ij3[1];
13548 vinfos[3].maxsolutions = _nj3;
13549 vinfos[4].jointtype = 1;
13550 vinfos[4].foffset = j4;
13551 vinfos[4].indices[0] = _ij4[0];
13552 vinfos[4].indices[1] = _ij4[1];
13553 vinfos[4].maxsolutions = _nj4;
13554 vinfos[5].jointtype = 1;
13555 vinfos[5].foffset = j5;
13556 vinfos[5].indices[0] = _ij5[0];
13557 vinfos[5].indices[1] = _ij5[1];
13558 vinfos[5].maxsolutions = _nj5;
13559 std::vector<int> vfree(0);
13572 IkReal j3array[1], cj3array[1], sj3array[1];
13573 bool j3valid[1]={
false};
13575 IkReal x1044=((1.0)*new_r10);
13584 j3array[0]=((-1.5707963267949)+(x1045.
value)+(((1.5707963267949)*(x1046.
value))));
13585 sj3array[0]=
IKsin(j3array[0]);
13586 cj3array[0]=
IKcos(j3array[0]);
13587 if( j3array[0] >
IKPI )
13591 else if( j3array[0] < -
IKPI )
13592 { j3array[0]+=
IK2PI;
13595 for(
int ij3 = 0; ij3 < 1; ++ij3)
13597 if( !j3valid[ij3] )
13601 _ij3[0] = ij3; _ij3[1] = -1;
13602 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13606 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13609 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13611 IkReal evalcond[8];
13612 IkReal x1047=
IKsin(j3);
13613 IkReal x1048=
IKcos(j3);
13614 IkReal x1049=((1.0)*gconst16);
13615 IkReal x1050=(gconst17*x1047);
13616 IkReal x1051=(gconst17*x1048);
13617 IkReal x1052=((1.0)*x1048);
13618 IkReal x1053=(x1048*x1049);
13619 evalcond[0]=(gconst17+((new_r00*x1048))+((new_r10*x1047)));
13620 evalcond[1]=(x1051+new_r00+((gconst16*x1047)));
13621 evalcond[2]=(gconst16+((new_r00*x1047))+(((-1.0)*new_r10*x1052)));
13622 evalcond[3]=(gconst17+((new_r01*x1047))+(((-1.0)*new_r11*x1052)));
13623 evalcond[4]=((((-1.0)*x1053))+x1050+new_r01);
13624 evalcond[5]=((((-1.0)*x1053))+x1050+new_r10);
13625 evalcond[6]=(((new_r11*x1047))+((new_r01*x1048))+(((-1.0)*x1049)));
13626 evalcond[7]=((((-1.0)*x1051))+new_r11+(((-1.0)*x1047*x1049)));
13634 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13635 vinfos[0].jointtype = 1;
13636 vinfos[0].foffset = j0;
13637 vinfos[0].indices[0] = _ij0[0];
13638 vinfos[0].indices[1] = _ij0[1];
13639 vinfos[0].maxsolutions = _nj0;
13640 vinfos[1].jointtype = 1;
13641 vinfos[1].foffset = j1;
13642 vinfos[1].indices[0] = _ij1[0];
13643 vinfos[1].indices[1] = _ij1[1];
13644 vinfos[1].maxsolutions = _nj1;
13645 vinfos[2].jointtype = 1;
13646 vinfos[2].foffset = j2;
13647 vinfos[2].indices[0] = _ij2[0];
13648 vinfos[2].indices[1] = _ij2[1];
13649 vinfos[2].maxsolutions = _nj2;
13650 vinfos[3].jointtype = 1;
13651 vinfos[3].foffset = j3;
13652 vinfos[3].indices[0] = _ij3[0];
13653 vinfos[3].indices[1] = _ij3[1];
13654 vinfos[3].maxsolutions = _nj3;
13655 vinfos[4].jointtype = 1;
13656 vinfos[4].foffset = j4;
13657 vinfos[4].indices[0] = _ij4[0];
13658 vinfos[4].indices[1] = _ij4[1];
13659 vinfos[4].maxsolutions = _nj4;
13660 vinfos[5].jointtype = 1;
13661 vinfos[5].foffset = j5;
13662 vinfos[5].indices[0] = _ij5[0];
13663 vinfos[5].indices[1] = _ij5[1];
13664 vinfos[5].maxsolutions = _nj5;
13665 std::vector<int> vfree(0);
13678 IkReal j3array[1], cj3array[1], sj3array[1];
13679 bool j3valid[1]={
false};
13681 IkReal x1054=((1.0)*new_r10);
13690 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1055.
value)))+(x1056.
value));
13691 sj3array[0]=
IKsin(j3array[0]);
13692 cj3array[0]=
IKcos(j3array[0]);
13693 if( j3array[0] >
IKPI )
13697 else if( j3array[0] < -
IKPI )
13698 { j3array[0]+=
IK2PI;
13701 for(
int ij3 = 0; ij3 < 1; ++ij3)
13703 if( !j3valid[ij3] )
13707 _ij3[0] = ij3; _ij3[1] = -1;
13708 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
13712 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13715 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13717 IkReal evalcond[8];
13718 IkReal x1057=
IKsin(j3);
13719 IkReal x1058=
IKcos(j3);
13720 IkReal x1059=((1.0)*gconst16);
13721 IkReal x1060=(gconst17*x1057);
13722 IkReal x1061=(gconst17*x1058);
13723 IkReal x1062=((1.0)*x1058);
13724 IkReal x1063=(x1058*x1059);
13725 evalcond[0]=(gconst17+((new_r00*x1058))+((new_r10*x1057)));
13726 evalcond[1]=(x1061+new_r00+((gconst16*x1057)));
13727 evalcond[2]=((((-1.0)*new_r10*x1062))+gconst16+((new_r00*x1057)));
13728 evalcond[3]=(gconst17+((new_r01*x1057))+(((-1.0)*new_r11*x1062)));
13729 evalcond[4]=(x1060+new_r01+(((-1.0)*x1063)));
13730 evalcond[5]=(x1060+new_r10+(((-1.0)*x1063)));
13731 evalcond[6]=((((-1.0)*x1059))+((new_r01*x1058))+((new_r11*x1057)));
13732 evalcond[7]=((((-1.0)*x1057*x1059))+new_r11+(((-1.0)*x1061)));
13740 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13741 vinfos[0].jointtype = 1;
13742 vinfos[0].foffset = j0;
13743 vinfos[0].indices[0] = _ij0[0];
13744 vinfos[0].indices[1] = _ij0[1];
13745 vinfos[0].maxsolutions = _nj0;
13746 vinfos[1].jointtype = 1;
13747 vinfos[1].foffset = j1;
13748 vinfos[1].indices[0] = _ij1[0];
13749 vinfos[1].indices[1] = _ij1[1];
13750 vinfos[1].maxsolutions = _nj1;
13751 vinfos[2].jointtype = 1;
13752 vinfos[2].foffset = j2;
13753 vinfos[2].indices[0] = _ij2[0];
13754 vinfos[2].indices[1] = _ij2[1];
13755 vinfos[2].maxsolutions = _nj2;
13756 vinfos[3].jointtype = 1;
13757 vinfos[3].foffset = j3;
13758 vinfos[3].indices[0] = _ij3[0];
13759 vinfos[3].indices[1] = _ij3[1];
13760 vinfos[3].maxsolutions = _nj3;
13761 vinfos[4].jointtype = 1;
13762 vinfos[4].foffset = j4;
13763 vinfos[4].indices[0] = _ij4[0];
13764 vinfos[4].indices[1] = _ij4[1];
13765 vinfos[4].maxsolutions = _nj4;
13766 vinfos[5].jointtype = 1;
13767 vinfos[5].foffset = j5;
13768 vinfos[5].indices[0] = _ij5[0];
13769 vinfos[5].indices[1] = _ij5[1];
13770 vinfos[5].maxsolutions = _nj5;
13771 std::vector<int> vfree(0);
13783 if( bgotonextstatement )
13785 bool bgotonextstatement =
true;
13788 IkReal x1064=((-1.0)*new_r10);
13789 IkReal x1066 = ((new_r10*new_r10)+(new_r00*new_r00));
13790 if(
IKabs(x1066)==0){
13793 IkReal x1065=
pow(x1066,-0.5);
13798 IkReal gconst18=((-1.0)*(x1067.
value));
13799 IkReal gconst19=(new_r00*x1065);
13800 IkReal gconst20=(x1064*x1065);
13805 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((x1068.
value)+j5)))), 6.28318530717959)));
13806 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
13808 bgotonextstatement=
false;
13811 IkReal x1069=((-1.0)*new_r10);
13816 IkReal x1070=((-1.0)*(x1072.
value));
13817 IkReal x1071=x1065;
13820 j4=3.14159265358979;
13824 IkReal gconst18=x1070;
13825 IkReal gconst19=(new_r00*x1071);
13826 IkReal gconst20=(x1069*x1071);
13827 IkReal x1073=new_r10*new_r10;
13828 IkReal x1074=((1.0)*new_r00);
13829 IkReal x1075=((1.0)*new_r10*new_r11);
13830 IkReal x1076=((((-1.0)*new_r01*x1074))+(((-1.0)*x1075)));
13831 IkReal x1077=x1065;
13832 IkReal x1078=(new_r10*x1077);
13834 j3eval[1]=((
IKabs(((((-1.0)*x1075*x1077))+(((-1.0)*x1074*x1078)))))+(
IKabs((((x1073*x1077))+(((-1.0)*new_r01*x1078))))));
13835 j3eval[2]=
IKsign(x1076);
13836 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
13840 IkReal x1079=((-1.0)*new_r10);
13845 IkReal x1080=((-1.0)*(x1082.
value));
13846 IkReal x1081=x1065;
13849 j4=3.14159265358979;
13853 IkReal gconst18=x1080;
13854 IkReal gconst19=(new_r00*x1081);
13855 IkReal gconst20=(x1079*x1081);
13856 IkReal x1083=new_r10*new_r10;
13861 IkReal x1084=x1086.
value;
13862 IkReal x1085=(new_r00*x1084);
13863 j3eval[0]=((
IKabs((((new_r00*new_r11))+((x1083*x1084)))))+(
IKabs((((new_r01*x1083*x1085))+((new_r10*x1085))+((new_r01*x1085*(new_r00*new_r00)))))));
13864 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13868 IkReal x1087=((-1.0)*new_r10);
13873 IkReal x1088=((-1.0)*(x1090.
value));
13874 IkReal x1089=x1065;
13877 j4=3.14159265358979;
13881 IkReal gconst18=x1088;
13882 IkReal gconst19=(new_r00*x1089);
13883 IkReal gconst20=(x1087*x1089);
13884 IkReal x1091=new_r10*new_r10;
13885 IkReal x1092=new_r00*new_r00;
13890 IkReal x1093=x1096.
value;
13891 IkReal x1094=(new_r10*x1093);
13892 IkReal x1095=(x1091*x1093);
13893 j3eval[0]=((
IKabs((x1095+(((-1.0)*x1093*(x1092*x1092)))+(((-1.0)*x1092*x1095)))))+(
IKabs((((new_r00*x1094))+((x1094*(new_r00*new_r00*new_r00)))+((new_r00*x1094*(new_r10*new_r10)))))));
13894 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13897 IkReal evalcond[3];
13898 bool bgotonextstatement =
true;
13901 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
13902 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
13904 bgotonextstatement=
false;
13907 IkReal x1097=((-1.0)*new_r10);
13912 IkReal x1098=((-1.0)*(x1099.
value));
13915 j4=3.14159265358979;
13921 IkReal gconst18=x1098;
13923 IkReal x1100 = new_r10*new_r10;
13924 if(
IKabs(x1100)==0){
13927 IkReal gconst20=(x1097*(
pow(x1100,-0.5)));
13929 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
13932 IkReal j3array[2], cj3array[2], sj3array[2];
13933 bool j3valid[2]={
false};
13939 sj3array[0]=((-1.0)*new_r01*(x1101.
value));
13942 j3valid[0] = j3valid[1] =
true;
13943 j3array[0] =
IKasin(sj3array[0]);
13944 cj3array[0] =
IKcos(j3array[0]);
13945 sj3array[1] = sj3array[0];
13946 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
13947 cj3array[1] = -cj3array[0];
13949 else if( isnan(sj3array[0]) )
13953 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13955 for(
int ij3 = 0; ij3 < 2; ++ij3)
13957 if( !j3valid[ij3] )
13961 _ij3[0] = ij3; _ij3[1] = -1;
13962 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
13966 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
13969 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13971 IkReal evalcond[6];
13972 IkReal x1102=
IKcos(j3);
13973 IkReal x1103=
IKsin(j3);
13974 evalcond[0]=(new_r01*x1102);
13975 evalcond[1]=(gconst20*x1102);
13976 evalcond[2]=((-1.0)*new_r10*x1102);
13977 evalcond[3]=(gconst20+((new_r01*x1103)));
13978 evalcond[4]=(gconst20+((new_r10*x1103)));
13979 evalcond[5]=(((gconst20*x1103))+new_r10);
13987 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13988 vinfos[0].jointtype = 1;
13989 vinfos[0].foffset = j0;
13990 vinfos[0].indices[0] = _ij0[0];
13991 vinfos[0].indices[1] = _ij0[1];
13992 vinfos[0].maxsolutions = _nj0;
13993 vinfos[1].jointtype = 1;
13994 vinfos[1].foffset = j1;
13995 vinfos[1].indices[0] = _ij1[0];
13996 vinfos[1].indices[1] = _ij1[1];
13997 vinfos[1].maxsolutions = _nj1;
13998 vinfos[2].jointtype = 1;
13999 vinfos[2].foffset = j2;
14000 vinfos[2].indices[0] = _ij2[0];
14001 vinfos[2].indices[1] = _ij2[1];
14002 vinfos[2].maxsolutions = _nj2;
14003 vinfos[3].jointtype = 1;
14004 vinfos[3].foffset = j3;
14005 vinfos[3].indices[0] = _ij3[0];
14006 vinfos[3].indices[1] = _ij3[1];
14007 vinfos[3].maxsolutions = _nj3;
14008 vinfos[4].jointtype = 1;
14009 vinfos[4].foffset = j4;
14010 vinfos[4].indices[0] = _ij4[0];
14011 vinfos[4].indices[1] = _ij4[1];
14012 vinfos[4].maxsolutions = _nj4;
14013 vinfos[5].jointtype = 1;
14014 vinfos[5].foffset = j5;
14015 vinfos[5].indices[0] = _ij5[0];
14016 vinfos[5].indices[1] = _ij5[1];
14017 vinfos[5].maxsolutions = _nj5;
14018 std::vector<int> vfree(0);
14027 IkReal j3array[2], cj3array[2], sj3array[2];
14028 bool j3valid[2]={
false};
14034 sj3array[0]=((-1.0)*gconst20*(x1104.
value));
14037 j3valid[0] = j3valid[1] =
true;
14038 j3array[0] =
IKasin(sj3array[0]);
14039 cj3array[0] =
IKcos(j3array[0]);
14040 sj3array[1] = sj3array[0];
14041 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
14042 cj3array[1] = -cj3array[0];
14044 else if( isnan(sj3array[0]) )
14048 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14050 for(
int ij3 = 0; ij3 < 2; ++ij3)
14052 if( !j3valid[ij3] )
14056 _ij3[0] = ij3; _ij3[1] = -1;
14057 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
14061 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14064 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14066 IkReal evalcond[6];
14067 IkReal x1105=
IKcos(j3);
14068 IkReal x1106=
IKsin(j3);
14069 IkReal x1107=(gconst20*x1106);
14070 evalcond[0]=(new_r01*x1105);
14071 evalcond[1]=(gconst20*x1105);
14072 evalcond[2]=((-1.0)*new_r10*x1105);
14073 evalcond[3]=(x1107+new_r01);
14074 evalcond[4]=(gconst20+((new_r10*x1106)));
14075 evalcond[5]=(x1107+new_r10);
14083 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14084 vinfos[0].jointtype = 1;
14085 vinfos[0].foffset = j0;
14086 vinfos[0].indices[0] = _ij0[0];
14087 vinfos[0].indices[1] = _ij0[1];
14088 vinfos[0].maxsolutions = _nj0;
14089 vinfos[1].jointtype = 1;
14090 vinfos[1].foffset = j1;
14091 vinfos[1].indices[0] = _ij1[0];
14092 vinfos[1].indices[1] = _ij1[1];
14093 vinfos[1].maxsolutions = _nj1;
14094 vinfos[2].jointtype = 1;
14095 vinfos[2].foffset = j2;
14096 vinfos[2].indices[0] = _ij2[0];
14097 vinfos[2].indices[1] = _ij2[1];
14098 vinfos[2].maxsolutions = _nj2;
14099 vinfos[3].jointtype = 1;
14100 vinfos[3].foffset = j3;
14101 vinfos[3].indices[0] = _ij3[0];
14102 vinfos[3].indices[1] = _ij3[1];
14103 vinfos[3].maxsolutions = _nj3;
14104 vinfos[4].jointtype = 1;
14105 vinfos[4].foffset = j4;
14106 vinfos[4].indices[0] = _ij4[0];
14107 vinfos[4].indices[1] = _ij4[1];
14108 vinfos[4].maxsolutions = _nj4;
14109 vinfos[5].jointtype = 1;
14110 vinfos[5].foffset = j5;
14111 vinfos[5].indices[0] = _ij5[0];
14112 vinfos[5].indices[1] = _ij5[1];
14113 vinfos[5].maxsolutions = _nj5;
14114 std::vector<int> vfree(0);
14126 if( bgotonextstatement )
14128 bool bgotonextstatement =
true;
14131 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
14132 evalcond[1]=gconst20;
14133 evalcond[2]=gconst19;
14134 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
14136 bgotonextstatement=
false;
14139 IkReal x1108=((-1.0)*new_r10);
14144 IkReal x1109=((-1.0)*(x1110.
value));
14147 j4=3.14159265358979;
14155 IkReal gconst18=x1109;
14156 IkReal gconst19=new_r00;
14157 IkReal gconst20=x1108;
14159 j3eval[1]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs((new_r00*new_r10))));
14161 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
14165 IkReal x1111=((-1.0)*new_r10);
14170 IkReal x1112=((-1.0)*(x1113.
value));
14173 j4=3.14159265358979;
14181 IkReal gconst18=x1112;
14182 IkReal gconst19=new_r00;
14183 IkReal gconst20=x1111;
14185 j3eval[1]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs((new_r00*new_r10))));
14187 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
14191 IkReal x1114=((-1.0)*new_r10);
14196 IkReal x1115=((-1.0)*(x1116.
value));
14199 j4=3.14159265358979;
14207 IkReal gconst18=x1115;
14208 IkReal gconst19=new_r00;
14209 IkReal gconst20=x1114;
14211 j3eval[1]=((((0.5)*(
IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(
IKabs((new_r00*new_r10))));
14213 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
14220 IkReal j3array[1], cj3array[1], sj3array[1];
14221 bool j3valid[1]={
false};
14223 IkReal x1117=((1.0)*gconst20);
14232 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1118.
value)))+(x1119.
value));
14233 sj3array[0]=
IKsin(j3array[0]);
14234 cj3array[0]=
IKcos(j3array[0]);
14235 if( j3array[0] >
IKPI )
14239 else if( j3array[0] < -
IKPI )
14240 { j3array[0]+=
IK2PI;
14243 for(
int ij3 = 0; ij3 < 1; ++ij3)
14245 if( !j3valid[ij3] )
14249 _ij3[0] = ij3; _ij3[1] = -1;
14250 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14254 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14257 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14259 IkReal evalcond[6];
14260 IkReal x1120=
IKsin(j3);
14261 IkReal x1121=
IKcos(j3);
14262 IkReal x1122=(gconst20*x1120);
14263 IkReal x1123=(gconst19*x1120);
14264 IkReal x1124=(gconst20*x1121);
14265 IkReal x1125=((1.0)*x1121);
14266 IkReal x1126=(gconst19*x1125);
14267 evalcond[0]=(x1122+(((-1.0)*x1126)));
14268 evalcond[1]=(gconst20+((new_r10*x1120))+((new_r00*x1121)));
14269 evalcond[2]=(x1123+x1124+new_r00);
14270 evalcond[3]=((((-1.0)*new_r10*x1125))+gconst19+((new_r00*x1120)));
14271 evalcond[4]=((((-1.0)*x1124))+(((-1.0)*x1123)));
14272 evalcond[5]=(x1122+(((-1.0)*x1126))+new_r10);
14280 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14281 vinfos[0].jointtype = 1;
14282 vinfos[0].foffset = j0;
14283 vinfos[0].indices[0] = _ij0[0];
14284 vinfos[0].indices[1] = _ij0[1];
14285 vinfos[0].maxsolutions = _nj0;
14286 vinfos[1].jointtype = 1;
14287 vinfos[1].foffset = j1;
14288 vinfos[1].indices[0] = _ij1[0];
14289 vinfos[1].indices[1] = _ij1[1];
14290 vinfos[1].maxsolutions = _nj1;
14291 vinfos[2].jointtype = 1;
14292 vinfos[2].foffset = j2;
14293 vinfos[2].indices[0] = _ij2[0];
14294 vinfos[2].indices[1] = _ij2[1];
14295 vinfos[2].maxsolutions = _nj2;
14296 vinfos[3].jointtype = 1;
14297 vinfos[3].foffset = j3;
14298 vinfos[3].indices[0] = _ij3[0];
14299 vinfos[3].indices[1] = _ij3[1];
14300 vinfos[3].maxsolutions = _nj3;
14301 vinfos[4].jointtype = 1;
14302 vinfos[4].foffset = j4;
14303 vinfos[4].indices[0] = _ij4[0];
14304 vinfos[4].indices[1] = _ij4[1];
14305 vinfos[4].maxsolutions = _nj4;
14306 vinfos[5].jointtype = 1;
14307 vinfos[5].foffset = j5;
14308 vinfos[5].indices[0] = _ij5[0];
14309 vinfos[5].indices[1] = _ij5[1];
14310 vinfos[5].maxsolutions = _nj5;
14311 std::vector<int> vfree(0);
14324 IkReal j3array[1], cj3array[1], sj3array[1];
14325 bool j3valid[1]={
false};
14335 j3array[0]=((-1.5707963267949)+(x1127.
value)+(((1.5707963267949)*(x1128.
value))));
14336 sj3array[0]=
IKsin(j3array[0]);
14337 cj3array[0]=
IKcos(j3array[0]);
14338 if( j3array[0] >
IKPI )
14342 else if( j3array[0] < -
IKPI )
14343 { j3array[0]+=
IK2PI;
14346 for(
int ij3 = 0; ij3 < 1; ++ij3)
14348 if( !j3valid[ij3] )
14352 _ij3[0] = ij3; _ij3[1] = -1;
14353 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14357 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14360 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14362 IkReal evalcond[6];
14363 IkReal x1129=
IKsin(j3);
14364 IkReal x1130=
IKcos(j3);
14365 IkReal x1131=(gconst20*x1129);
14366 IkReal x1132=(gconst19*x1129);
14367 IkReal x1133=(gconst20*x1130);
14368 IkReal x1134=((1.0)*x1130);
14369 IkReal x1135=(gconst19*x1134);
14370 evalcond[0]=(x1131+(((-1.0)*x1135)));
14371 evalcond[1]=(gconst20+((new_r10*x1129))+((new_r00*x1130)));
14372 evalcond[2]=(x1133+x1132+new_r00);
14373 evalcond[3]=(gconst19+(((-1.0)*new_r10*x1134))+((new_r00*x1129)));
14374 evalcond[4]=((((-1.0)*x1132))+(((-1.0)*x1133)));
14375 evalcond[5]=(x1131+(((-1.0)*x1135))+new_r10);
14383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14384 vinfos[0].jointtype = 1;
14385 vinfos[0].foffset = j0;
14386 vinfos[0].indices[0] = _ij0[0];
14387 vinfos[0].indices[1] = _ij0[1];
14388 vinfos[0].maxsolutions = _nj0;
14389 vinfos[1].jointtype = 1;
14390 vinfos[1].foffset = j1;
14391 vinfos[1].indices[0] = _ij1[0];
14392 vinfos[1].indices[1] = _ij1[1];
14393 vinfos[1].maxsolutions = _nj1;
14394 vinfos[2].jointtype = 1;
14395 vinfos[2].foffset = j2;
14396 vinfos[2].indices[0] = _ij2[0];
14397 vinfos[2].indices[1] = _ij2[1];
14398 vinfos[2].maxsolutions = _nj2;
14399 vinfos[3].jointtype = 1;
14400 vinfos[3].foffset = j3;
14401 vinfos[3].indices[0] = _ij3[0];
14402 vinfos[3].indices[1] = _ij3[1];
14403 vinfos[3].maxsolutions = _nj3;
14404 vinfos[4].jointtype = 1;
14405 vinfos[4].foffset = j4;
14406 vinfos[4].indices[0] = _ij4[0];
14407 vinfos[4].indices[1] = _ij4[1];
14408 vinfos[4].maxsolutions = _nj4;
14409 vinfos[5].jointtype = 1;
14410 vinfos[5].foffset = j5;
14411 vinfos[5].indices[0] = _ij5[0];
14412 vinfos[5].indices[1] = _ij5[1];
14413 vinfos[5].maxsolutions = _nj5;
14414 std::vector<int> vfree(0);
14427 IkReal j3array[1], cj3array[1], sj3array[1];
14428 bool j3valid[1]={
false};
14438 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1136.
value)))+(x1137.
value));
14439 sj3array[0]=
IKsin(j3array[0]);
14440 cj3array[0]=
IKcos(j3array[0]);
14441 if( j3array[0] >
IKPI )
14445 else if( j3array[0] < -
IKPI )
14446 { j3array[0]+=
IK2PI;
14449 for(
int ij3 = 0; ij3 < 1; ++ij3)
14451 if( !j3valid[ij3] )
14455 _ij3[0] = ij3; _ij3[1] = -1;
14456 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14460 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14463 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14465 IkReal evalcond[6];
14466 IkReal x1138=
IKsin(j3);
14467 IkReal x1139=
IKcos(j3);
14468 IkReal x1140=(gconst20*x1138);
14469 IkReal x1141=(gconst19*x1138);
14470 IkReal x1142=(gconst20*x1139);
14471 IkReal x1143=((1.0)*x1139);
14472 IkReal x1144=(gconst19*x1143);
14473 evalcond[0]=(x1140+(((-1.0)*x1144)));
14474 evalcond[1]=(gconst20+((new_r00*x1139))+((new_r10*x1138)));
14475 evalcond[2]=(x1142+x1141+new_r00);
14476 evalcond[3]=(gconst19+((new_r00*x1138))+(((-1.0)*new_r10*x1143)));
14477 evalcond[4]=((((-1.0)*x1141))+(((-1.0)*x1142)));
14478 evalcond[5]=(x1140+(((-1.0)*x1144))+new_r10);
14486 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14487 vinfos[0].jointtype = 1;
14488 vinfos[0].foffset = j0;
14489 vinfos[0].indices[0] = _ij0[0];
14490 vinfos[0].indices[1] = _ij0[1];
14491 vinfos[0].maxsolutions = _nj0;
14492 vinfos[1].jointtype = 1;
14493 vinfos[1].foffset = j1;
14494 vinfos[1].indices[0] = _ij1[0];
14495 vinfos[1].indices[1] = _ij1[1];
14496 vinfos[1].maxsolutions = _nj1;
14497 vinfos[2].jointtype = 1;
14498 vinfos[2].foffset = j2;
14499 vinfos[2].indices[0] = _ij2[0];
14500 vinfos[2].indices[1] = _ij2[1];
14501 vinfos[2].maxsolutions = _nj2;
14502 vinfos[3].jointtype = 1;
14503 vinfos[3].foffset = j3;
14504 vinfos[3].indices[0] = _ij3[0];
14505 vinfos[3].indices[1] = _ij3[1];
14506 vinfos[3].maxsolutions = _nj3;
14507 vinfos[4].jointtype = 1;
14508 vinfos[4].foffset = j4;
14509 vinfos[4].indices[0] = _ij4[0];
14510 vinfos[4].indices[1] = _ij4[1];
14511 vinfos[4].maxsolutions = _nj4;
14512 vinfos[5].jointtype = 1;
14513 vinfos[5].foffset = j5;
14514 vinfos[5].indices[0] = _ij5[0];
14515 vinfos[5].indices[1] = _ij5[1];
14516 vinfos[5].maxsolutions = _nj5;
14517 std::vector<int> vfree(0);
14529 if( bgotonextstatement )
14531 bool bgotonextstatement =
true;
14534 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
14535 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
14537 bgotonextstatement=
false;
14539 IkReal j3array[2], cj3array[2], sj3array[2];
14540 bool j3valid[2]={
false};
14546 sj3array[0]=(new_r11*(x1145.
value));
14549 j3valid[0] = j3valid[1] =
true;
14550 j3array[0] =
IKasin(sj3array[0]);
14551 cj3array[0] =
IKcos(j3array[0]);
14552 sj3array[1] = sj3array[0];
14553 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
14554 cj3array[1] = -cj3array[0];
14556 else if( isnan(sj3array[0]) )
14560 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14562 for(
int ij3 = 0; ij3 < 2; ++ij3)
14564 if( !j3valid[ij3] )
14568 _ij3[0] = ij3; _ij3[1] = -1;
14569 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
14573 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14576 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14578 IkReal evalcond[6];
14579 IkReal x1146=
IKcos(j3);
14580 IkReal x1147=
IKsin(j3);
14581 IkReal x1148=((-1.0)*x1146);
14582 evalcond[0]=(new_r00*x1146);
14583 evalcond[1]=(new_r11*x1148);
14584 evalcond[2]=(gconst19*x1148);
14585 evalcond[3]=(((new_r00*x1147))+gconst19);
14586 evalcond[4]=(((gconst19*x1147))+new_r00);
14587 evalcond[5]=((((-1.0)*gconst19))+((new_r11*x1147)));
14595 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14596 vinfos[0].jointtype = 1;
14597 vinfos[0].foffset = j0;
14598 vinfos[0].indices[0] = _ij0[0];
14599 vinfos[0].indices[1] = _ij0[1];
14600 vinfos[0].maxsolutions = _nj0;
14601 vinfos[1].jointtype = 1;
14602 vinfos[1].foffset = j1;
14603 vinfos[1].indices[0] = _ij1[0];
14604 vinfos[1].indices[1] = _ij1[1];
14605 vinfos[1].maxsolutions = _nj1;
14606 vinfos[2].jointtype = 1;
14607 vinfos[2].foffset = j2;
14608 vinfos[2].indices[0] = _ij2[0];
14609 vinfos[2].indices[1] = _ij2[1];
14610 vinfos[2].maxsolutions = _nj2;
14611 vinfos[3].jointtype = 1;
14612 vinfos[3].foffset = j3;
14613 vinfos[3].indices[0] = _ij3[0];
14614 vinfos[3].indices[1] = _ij3[1];
14615 vinfos[3].maxsolutions = _nj3;
14616 vinfos[4].jointtype = 1;
14617 vinfos[4].foffset = j4;
14618 vinfos[4].indices[0] = _ij4[0];
14619 vinfos[4].indices[1] = _ij4[1];
14620 vinfos[4].maxsolutions = _nj4;
14621 vinfos[5].jointtype = 1;
14622 vinfos[5].foffset = j5;
14623 vinfos[5].indices[0] = _ij5[0];
14624 vinfos[5].indices[1] = _ij5[1];
14625 vinfos[5].maxsolutions = _nj5;
14626 std::vector<int> vfree(0);
14634 if( bgotonextstatement )
14636 bool bgotonextstatement =
true;
14639 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
14640 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
14642 bgotonextstatement=
false;
14649 IkReal x1149=((-1.0)*(x1150.
value));
14652 j4=3.14159265358979;
14660 IkReal gconst18=x1149;
14661 IkReal x1151 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14662 if(
IKabs(x1151)==0){
14665 IkReal gconst19=(new_r00*(
pow(x1151,-0.5)));
14667 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
14668 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
14676 IkReal x1152=((-1.0)*(x1153.
value));
14679 j4=3.14159265358979;
14687 IkReal gconst18=x1152;
14688 IkReal x1154 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14689 if(
IKabs(x1154)==0){
14692 IkReal gconst19=(new_r00*(
pow(x1154,-0.5)));
14695 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
14703 IkReal x1155=((-1.0)*(x1156.
value));
14706 j4=3.14159265358979;
14714 IkReal gconst18=x1155;
14715 IkReal x1157 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14716 if(
IKabs(x1157)==0){
14719 IkReal gconst19=(new_r00*(
pow(x1157,-0.5)));
14723 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
14730 IkReal j3array[1], cj3array[1], sj3array[1];
14731 bool j3valid[1]={
false};
14744 sj3array[0]=
IKsin(j3array[0]);
14745 cj3array[0]=
IKcos(j3array[0]);
14746 if( j3array[0] >
IKPI )
14750 else if( j3array[0] < -
IKPI )
14751 { j3array[0]+=
IK2PI;
14754 for(
int ij3 = 0; ij3 < 1; ++ij3)
14756 if( !j3valid[ij3] )
14760 _ij3[0] = ij3; _ij3[1] = -1;
14761 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14765 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14768 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14770 IkReal evalcond[8];
14771 IkReal x1160=
IKsin(j3);
14772 IkReal x1161=
IKcos(j3);
14773 IkReal x1162=((1.0)*gconst19);
14774 IkReal x1163=(gconst19*x1160);
14775 evalcond[0]=(new_r01*x1160);
14776 evalcond[1]=(new_r00*x1161);
14777 evalcond[2]=((-1.0)*x1163);
14778 evalcond[3]=((-1.0)*gconst19*x1161);
14779 evalcond[4]=(gconst19+((new_r00*x1160)));
14780 evalcond[5]=(x1163+new_r00);
14781 evalcond[6]=(new_r01+(((-1.0)*x1161*x1162)));
14782 evalcond[7]=(((new_r01*x1161))+(((-1.0)*x1162)));
14790 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14791 vinfos[0].jointtype = 1;
14792 vinfos[0].foffset = j0;
14793 vinfos[0].indices[0] = _ij0[0];
14794 vinfos[0].indices[1] = _ij0[1];
14795 vinfos[0].maxsolutions = _nj0;
14796 vinfos[1].jointtype = 1;
14797 vinfos[1].foffset = j1;
14798 vinfos[1].indices[0] = _ij1[0];
14799 vinfos[1].indices[1] = _ij1[1];
14800 vinfos[1].maxsolutions = _nj1;
14801 vinfos[2].jointtype = 1;
14802 vinfos[2].foffset = j2;
14803 vinfos[2].indices[0] = _ij2[0];
14804 vinfos[2].indices[1] = _ij2[1];
14805 vinfos[2].maxsolutions = _nj2;
14806 vinfos[3].jointtype = 1;
14807 vinfos[3].foffset = j3;
14808 vinfos[3].indices[0] = _ij3[0];
14809 vinfos[3].indices[1] = _ij3[1];
14810 vinfos[3].maxsolutions = _nj3;
14811 vinfos[4].jointtype = 1;
14812 vinfos[4].foffset = j4;
14813 vinfos[4].indices[0] = _ij4[0];
14814 vinfos[4].indices[1] = _ij4[1];
14815 vinfos[4].maxsolutions = _nj4;
14816 vinfos[5].jointtype = 1;
14817 vinfos[5].foffset = j5;
14818 vinfos[5].indices[0] = _ij5[0];
14819 vinfos[5].indices[1] = _ij5[1];
14820 vinfos[5].maxsolutions = _nj5;
14821 std::vector<int> vfree(0);
14834 IkReal j3array[1], cj3array[1], sj3array[1];
14835 bool j3valid[1]={
false};
14848 sj3array[0]=
IKsin(j3array[0]);
14849 cj3array[0]=
IKcos(j3array[0]);
14850 if( j3array[0] >
IKPI )
14854 else if( j3array[0] < -
IKPI )
14855 { j3array[0]+=
IK2PI;
14858 for(
int ij3 = 0; ij3 < 1; ++ij3)
14860 if( !j3valid[ij3] )
14864 _ij3[0] = ij3; _ij3[1] = -1;
14865 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14869 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14872 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14874 IkReal evalcond[8];
14875 IkReal x1166=
IKsin(j3);
14876 IkReal x1167=
IKcos(j3);
14877 IkReal x1168=((1.0)*gconst19);
14878 IkReal x1169=(gconst19*x1166);
14879 evalcond[0]=(new_r01*x1166);
14880 evalcond[1]=(new_r00*x1167);
14881 evalcond[2]=((-1.0)*x1169);
14882 evalcond[3]=((-1.0)*gconst19*x1167);
14883 evalcond[4]=(gconst19+((new_r00*x1166)));
14884 evalcond[5]=(x1169+new_r00);
14885 evalcond[6]=((((-1.0)*x1167*x1168))+new_r01);
14886 evalcond[7]=(((new_r01*x1167))+(((-1.0)*x1168)));
14894 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14895 vinfos[0].jointtype = 1;
14896 vinfos[0].foffset = j0;
14897 vinfos[0].indices[0] = _ij0[0];
14898 vinfos[0].indices[1] = _ij0[1];
14899 vinfos[0].maxsolutions = _nj0;
14900 vinfos[1].jointtype = 1;
14901 vinfos[1].foffset = j1;
14902 vinfos[1].indices[0] = _ij1[0];
14903 vinfos[1].indices[1] = _ij1[1];
14904 vinfos[1].maxsolutions = _nj1;
14905 vinfos[2].jointtype = 1;
14906 vinfos[2].foffset = j2;
14907 vinfos[2].indices[0] = _ij2[0];
14908 vinfos[2].indices[1] = _ij2[1];
14909 vinfos[2].maxsolutions = _nj2;
14910 vinfos[3].jointtype = 1;
14911 vinfos[3].foffset = j3;
14912 vinfos[3].indices[0] = _ij3[0];
14913 vinfos[3].indices[1] = _ij3[1];
14914 vinfos[3].maxsolutions = _nj3;
14915 vinfos[4].jointtype = 1;
14916 vinfos[4].foffset = j4;
14917 vinfos[4].indices[0] = _ij4[0];
14918 vinfos[4].indices[1] = _ij4[1];
14919 vinfos[4].maxsolutions = _nj4;
14920 vinfos[5].jointtype = 1;
14921 vinfos[5].foffset = j5;
14922 vinfos[5].indices[0] = _ij5[0];
14923 vinfos[5].indices[1] = _ij5[1];
14924 vinfos[5].maxsolutions = _nj5;
14925 std::vector<int> vfree(0);
14938 IkReal j3array[1], cj3array[1], sj3array[1];
14939 bool j3valid[1]={
false};
14949 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1170.
value)))+(x1171.
value));
14950 sj3array[0]=
IKsin(j3array[0]);
14951 cj3array[0]=
IKcos(j3array[0]);
14952 if( j3array[0] >
IKPI )
14956 else if( j3array[0] < -
IKPI )
14957 { j3array[0]+=
IK2PI;
14960 for(
int ij3 = 0; ij3 < 1; ++ij3)
14962 if( !j3valid[ij3] )
14966 _ij3[0] = ij3; _ij3[1] = -1;
14967 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
14971 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
14974 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14976 IkReal evalcond[8];
14977 IkReal x1172=
IKsin(j3);
14978 IkReal x1173=
IKcos(j3);
14979 IkReal x1174=((1.0)*gconst19);
14980 IkReal x1175=(gconst19*x1172);
14981 evalcond[0]=(new_r01*x1172);
14982 evalcond[1]=(new_r00*x1173);
14983 evalcond[2]=((-1.0)*x1175);
14984 evalcond[3]=((-1.0)*gconst19*x1173);
14985 evalcond[4]=(gconst19+((new_r00*x1172)));
14986 evalcond[5]=(x1175+new_r00);
14987 evalcond[6]=((((-1.0)*x1173*x1174))+new_r01);
14988 evalcond[7]=((((-1.0)*x1174))+((new_r01*x1173)));
14996 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14997 vinfos[0].jointtype = 1;
14998 vinfos[0].foffset = j0;
14999 vinfos[0].indices[0] = _ij0[0];
15000 vinfos[0].indices[1] = _ij0[1];
15001 vinfos[0].maxsolutions = _nj0;
15002 vinfos[1].jointtype = 1;
15003 vinfos[1].foffset = j1;
15004 vinfos[1].indices[0] = _ij1[0];
15005 vinfos[1].indices[1] = _ij1[1];
15006 vinfos[1].maxsolutions = _nj1;
15007 vinfos[2].jointtype = 1;
15008 vinfos[2].foffset = j2;
15009 vinfos[2].indices[0] = _ij2[0];
15010 vinfos[2].indices[1] = _ij2[1];
15011 vinfos[2].maxsolutions = _nj2;
15012 vinfos[3].jointtype = 1;
15013 vinfos[3].foffset = j3;
15014 vinfos[3].indices[0] = _ij3[0];
15015 vinfos[3].indices[1] = _ij3[1];
15016 vinfos[3].maxsolutions = _nj3;
15017 vinfos[4].jointtype = 1;
15018 vinfos[4].foffset = j4;
15019 vinfos[4].indices[0] = _ij4[0];
15020 vinfos[4].indices[1] = _ij4[1];
15021 vinfos[4].maxsolutions = _nj4;
15022 vinfos[5].jointtype = 1;
15023 vinfos[5].foffset = j5;
15024 vinfos[5].indices[0] = _ij5[0];
15025 vinfos[5].indices[1] = _ij5[1];
15026 vinfos[5].maxsolutions = _nj5;
15027 std::vector<int> vfree(0);
15039 if( bgotonextstatement )
15041 bool bgotonextstatement =
true;
15044 evalcond[0]=
IKabs(new_r10);
15045 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
15047 bgotonextstatement=
false;
15054 IkReal x1176=((-1.0)*(x1177.
value));
15057 j4=3.14159265358979;
15062 IkReal gconst18=x1176;
15063 IkReal x1178 = new_r00*new_r00;
15064 if(
IKabs(x1178)==0){
15067 IkReal gconst19=(new_r00*(
pow(x1178,-0.5)));
15069 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
15070 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15078 IkReal x1179=((-1.0)*(x1180.
value));
15081 j4=3.14159265358979;
15086 IkReal gconst18=x1179;
15087 IkReal x1181 = new_r00*new_r00;
15088 if(
IKabs(x1181)==0){
15091 IkReal gconst19=(new_r00*(
pow(x1181,-0.5)));
15093 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
15094 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15102 IkReal x1182=((-1.0)*(x1183.
value));
15105 j4=3.14159265358979;
15110 IkReal gconst18=x1182;
15111 IkReal x1184 = new_r00*new_r00;
15112 if(
IKabs(x1184)==0){
15115 IkReal gconst19=(new_r00*(
pow(x1184,-0.5)));
15118 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15125 IkReal j3array[1], cj3array[1], sj3array[1];
15126 bool j3valid[1]={
false};
15139 sj3array[0]=
IKsin(j3array[0]);
15140 cj3array[0]=
IKcos(j3array[0]);
15141 if( j3array[0] >
IKPI )
15145 else if( j3array[0] < -
IKPI )
15146 { j3array[0]+=
IK2PI;
15149 for(
int ij3 = 0; ij3 < 1; ++ij3)
15151 if( !j3valid[ij3] )
15155 _ij3[0] = ij3; _ij3[1] = -1;
15156 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15160 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15163 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15165 IkReal evalcond[8];
15166 IkReal x1187=
IKcos(j3);
15167 IkReal x1188=
IKsin(j3);
15168 IkReal x1189=(gconst19*x1188);
15169 IkReal x1190=((1.0)*x1187);
15170 evalcond[0]=(new_r00*x1187);
15171 evalcond[1]=((-1.0)*gconst19*x1187);
15172 evalcond[2]=(gconst19+((new_r00*x1188)));
15173 evalcond[3]=(x1189+new_r00);
15174 evalcond[4]=((((-1.0)*gconst19*x1190))+new_r01);
15175 evalcond[5]=(new_r11+(((-1.0)*x1189)));
15176 evalcond[6]=((((-1.0)*new_r11*x1190))+((new_r01*x1188)));
15177 evalcond[7]=(((new_r01*x1187))+(((-1.0)*gconst19))+((new_r11*x1188)));
15185 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15186 vinfos[0].jointtype = 1;
15187 vinfos[0].foffset = j0;
15188 vinfos[0].indices[0] = _ij0[0];
15189 vinfos[0].indices[1] = _ij0[1];
15190 vinfos[0].maxsolutions = _nj0;
15191 vinfos[1].jointtype = 1;
15192 vinfos[1].foffset = j1;
15193 vinfos[1].indices[0] = _ij1[0];
15194 vinfos[1].indices[1] = _ij1[1];
15195 vinfos[1].maxsolutions = _nj1;
15196 vinfos[2].jointtype = 1;
15197 vinfos[2].foffset = j2;
15198 vinfos[2].indices[0] = _ij2[0];
15199 vinfos[2].indices[1] = _ij2[1];
15200 vinfos[2].maxsolutions = _nj2;
15201 vinfos[3].jointtype = 1;
15202 vinfos[3].foffset = j3;
15203 vinfos[3].indices[0] = _ij3[0];
15204 vinfos[3].indices[1] = _ij3[1];
15205 vinfos[3].maxsolutions = _nj3;
15206 vinfos[4].jointtype = 1;
15207 vinfos[4].foffset = j4;
15208 vinfos[4].indices[0] = _ij4[0];
15209 vinfos[4].indices[1] = _ij4[1];
15210 vinfos[4].maxsolutions = _nj4;
15211 vinfos[5].jointtype = 1;
15212 vinfos[5].foffset = j5;
15213 vinfos[5].indices[0] = _ij5[0];
15214 vinfos[5].indices[1] = _ij5[1];
15215 vinfos[5].maxsolutions = _nj5;
15216 std::vector<int> vfree(0);
15229 IkReal j3array[1], cj3array[1], sj3array[1];
15230 bool j3valid[1]={
false};
15240 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1191.
value)))+(x1192.
value));
15241 sj3array[0]=
IKsin(j3array[0]);
15242 cj3array[0]=
IKcos(j3array[0]);
15243 if( j3array[0] >
IKPI )
15247 else if( j3array[0] < -
IKPI )
15248 { j3array[0]+=
IK2PI;
15251 for(
int ij3 = 0; ij3 < 1; ++ij3)
15253 if( !j3valid[ij3] )
15257 _ij3[0] = ij3; _ij3[1] = -1;
15258 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15262 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15265 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15267 IkReal evalcond[8];
15268 IkReal x1193=
IKcos(j3);
15269 IkReal x1194=
IKsin(j3);
15270 IkReal x1195=(gconst19*x1194);
15271 IkReal x1196=((1.0)*x1193);
15272 evalcond[0]=(new_r00*x1193);
15273 evalcond[1]=((-1.0)*gconst19*x1193);
15274 evalcond[2]=(gconst19+((new_r00*x1194)));
15275 evalcond[3]=(x1195+new_r00);
15276 evalcond[4]=((((-1.0)*gconst19*x1196))+new_r01);
15277 evalcond[5]=(new_r11+(((-1.0)*x1195)));
15278 evalcond[6]=((((-1.0)*new_r11*x1196))+((new_r01*x1194)));
15279 evalcond[7]=(((new_r11*x1194))+(((-1.0)*gconst19))+((new_r01*x1193)));
15287 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15288 vinfos[0].jointtype = 1;
15289 vinfos[0].foffset = j0;
15290 vinfos[0].indices[0] = _ij0[0];
15291 vinfos[0].indices[1] = _ij0[1];
15292 vinfos[0].maxsolutions = _nj0;
15293 vinfos[1].jointtype = 1;
15294 vinfos[1].foffset = j1;
15295 vinfos[1].indices[0] = _ij1[0];
15296 vinfos[1].indices[1] = _ij1[1];
15297 vinfos[1].maxsolutions = _nj1;
15298 vinfos[2].jointtype = 1;
15299 vinfos[2].foffset = j2;
15300 vinfos[2].indices[0] = _ij2[0];
15301 vinfos[2].indices[1] = _ij2[1];
15302 vinfos[2].maxsolutions = _nj2;
15303 vinfos[3].jointtype = 1;
15304 vinfos[3].foffset = j3;
15305 vinfos[3].indices[0] = _ij3[0];
15306 vinfos[3].indices[1] = _ij3[1];
15307 vinfos[3].maxsolutions = _nj3;
15308 vinfos[4].jointtype = 1;
15309 vinfos[4].foffset = j4;
15310 vinfos[4].indices[0] = _ij4[0];
15311 vinfos[4].indices[1] = _ij4[1];
15312 vinfos[4].maxsolutions = _nj4;
15313 vinfos[5].jointtype = 1;
15314 vinfos[5].foffset = j5;
15315 vinfos[5].indices[0] = _ij5[0];
15316 vinfos[5].indices[1] = _ij5[1];
15317 vinfos[5].maxsolutions = _nj5;
15318 std::vector<int> vfree(0);
15331 IkReal j3array[1], cj3array[1], sj3array[1];
15332 bool j3valid[1]={
false};
15342 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1197.
value)))+(x1198.
value));
15343 sj3array[0]=
IKsin(j3array[0]);
15344 cj3array[0]=
IKcos(j3array[0]);
15345 if( j3array[0] >
IKPI )
15349 else if( j3array[0] < -
IKPI )
15350 { j3array[0]+=
IK2PI;
15353 for(
int ij3 = 0; ij3 < 1; ++ij3)
15355 if( !j3valid[ij3] )
15359 _ij3[0] = ij3; _ij3[1] = -1;
15360 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15364 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15367 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15369 IkReal evalcond[8];
15370 IkReal x1199=
IKcos(j3);
15371 IkReal x1200=
IKsin(j3);
15372 IkReal x1201=(gconst19*x1200);
15373 IkReal x1202=((1.0)*x1199);
15374 evalcond[0]=(new_r00*x1199);
15375 evalcond[1]=((-1.0)*gconst19*x1199);
15376 evalcond[2]=(((new_r00*x1200))+gconst19);
15377 evalcond[3]=(x1201+new_r00);
15378 evalcond[4]=((((-1.0)*gconst19*x1202))+new_r01);
15379 evalcond[5]=((((-1.0)*x1201))+new_r11);
15380 evalcond[6]=(((new_r01*x1200))+(((-1.0)*new_r11*x1202)));
15381 evalcond[7]=(((new_r11*x1200))+(((-1.0)*gconst19))+((new_r01*x1199)));
15389 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15390 vinfos[0].jointtype = 1;
15391 vinfos[0].foffset = j0;
15392 vinfos[0].indices[0] = _ij0[0];
15393 vinfos[0].indices[1] = _ij0[1];
15394 vinfos[0].maxsolutions = _nj0;
15395 vinfos[1].jointtype = 1;
15396 vinfos[1].foffset = j1;
15397 vinfos[1].indices[0] = _ij1[0];
15398 vinfos[1].indices[1] = _ij1[1];
15399 vinfos[1].maxsolutions = _nj1;
15400 vinfos[2].jointtype = 1;
15401 vinfos[2].foffset = j2;
15402 vinfos[2].indices[0] = _ij2[0];
15403 vinfos[2].indices[1] = _ij2[1];
15404 vinfos[2].maxsolutions = _nj2;
15405 vinfos[3].jointtype = 1;
15406 vinfos[3].foffset = j3;
15407 vinfos[3].indices[0] = _ij3[0];
15408 vinfos[3].indices[1] = _ij3[1];
15409 vinfos[3].maxsolutions = _nj3;
15410 vinfos[4].jointtype = 1;
15411 vinfos[4].foffset = j4;
15412 vinfos[4].indices[0] = _ij4[0];
15413 vinfos[4].indices[1] = _ij4[1];
15414 vinfos[4].maxsolutions = _nj4;
15415 vinfos[5].jointtype = 1;
15416 vinfos[5].foffset = j5;
15417 vinfos[5].indices[0] = _ij5[0];
15418 vinfos[5].indices[1] = _ij5[1];
15419 vinfos[5].maxsolutions = _nj5;
15420 std::vector<int> vfree(0);
15432 if( bgotonextstatement )
15434 bool bgotonextstatement =
true;
15439 bgotonextstatement=
false;
15444 if( bgotonextstatement )
15457 IkReal j3array[1], cj3array[1], sj3array[1];
15458 bool j3valid[1]={
false};
15460 IkReal x1203=((1.0)*gconst20);
15469 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1204.
value)))+(x1205.
value));
15470 sj3array[0]=
IKsin(j3array[0]);
15471 cj3array[0]=
IKcos(j3array[0]);
15472 if( j3array[0] >
IKPI )
15476 else if( j3array[0] < -
IKPI )
15477 { j3array[0]+=
IK2PI;
15480 for(
int ij3 = 0; ij3 < 1; ++ij3)
15482 if( !j3valid[ij3] )
15486 _ij3[0] = ij3; _ij3[1] = -1;
15487 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15491 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15494 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15496 IkReal evalcond[8];
15497 IkReal x1206=
IKcos(j3);
15498 IkReal x1207=
IKsin(j3);
15499 IkReal x1208=(gconst20*x1207);
15500 IkReal x1209=((1.0)*x1206);
15501 IkReal x1210=(gconst19*x1207);
15502 IkReal x1211=(gconst19*x1209);
15503 evalcond[0]=(((new_r00*x1206))+((new_r10*x1207))+gconst20);
15504 evalcond[1]=(((gconst20*x1206))+x1210+new_r00);
15505 evalcond[2]=(((new_r00*x1207))+gconst19+(((-1.0)*new_r10*x1209)));
15506 evalcond[3]=(((new_r01*x1207))+(((-1.0)*new_r11*x1209))+gconst20);
15507 evalcond[4]=(x1208+new_r01+(((-1.0)*x1211)));
15508 evalcond[5]=(x1208+new_r10+(((-1.0)*x1211)));
15509 evalcond[6]=(((new_r11*x1207))+((new_r01*x1206))+(((-1.0)*gconst19)));
15510 evalcond[7]=(new_r11+(((-1.0)*gconst20*x1209))+(((-1.0)*x1210)));
15518 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15519 vinfos[0].jointtype = 1;
15520 vinfos[0].foffset = j0;
15521 vinfos[0].indices[0] = _ij0[0];
15522 vinfos[0].indices[1] = _ij0[1];
15523 vinfos[0].maxsolutions = _nj0;
15524 vinfos[1].jointtype = 1;
15525 vinfos[1].foffset = j1;
15526 vinfos[1].indices[0] = _ij1[0];
15527 vinfos[1].indices[1] = _ij1[1];
15528 vinfos[1].maxsolutions = _nj1;
15529 vinfos[2].jointtype = 1;
15530 vinfos[2].foffset = j2;
15531 vinfos[2].indices[0] = _ij2[0];
15532 vinfos[2].indices[1] = _ij2[1];
15533 vinfos[2].maxsolutions = _nj2;
15534 vinfos[3].jointtype = 1;
15535 vinfos[3].foffset = j3;
15536 vinfos[3].indices[0] = _ij3[0];
15537 vinfos[3].indices[1] = _ij3[1];
15538 vinfos[3].maxsolutions = _nj3;
15539 vinfos[4].jointtype = 1;
15540 vinfos[4].foffset = j4;
15541 vinfos[4].indices[0] = _ij4[0];
15542 vinfos[4].indices[1] = _ij4[1];
15543 vinfos[4].maxsolutions = _nj4;
15544 vinfos[5].jointtype = 1;
15545 vinfos[5].foffset = j5;
15546 vinfos[5].indices[0] = _ij5[0];
15547 vinfos[5].indices[1] = _ij5[1];
15548 vinfos[5].maxsolutions = _nj5;
15549 std::vector<int> vfree(0);
15562 IkReal j3array[1], cj3array[1], sj3array[1];
15563 bool j3valid[1]={
false};
15565 IkReal x1212=((1.0)*gconst19);
15574 j3array[0]=((-1.5707963267949)+(x1213.
value)+(((1.5707963267949)*(x1214.
value))));
15575 sj3array[0]=
IKsin(j3array[0]);
15576 cj3array[0]=
IKcos(j3array[0]);
15577 if( j3array[0] >
IKPI )
15581 else if( j3array[0] < -
IKPI )
15582 { j3array[0]+=
IK2PI;
15585 for(
int ij3 = 0; ij3 < 1; ++ij3)
15587 if( !j3valid[ij3] )
15591 _ij3[0] = ij3; _ij3[1] = -1;
15592 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15596 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15599 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15601 IkReal evalcond[8];
15602 IkReal x1215=
IKcos(j3);
15603 IkReal x1216=
IKsin(j3);
15604 IkReal x1217=(gconst20*x1216);
15605 IkReal x1218=((1.0)*x1215);
15606 IkReal x1219=(gconst19*x1216);
15607 IkReal x1220=(gconst19*x1218);
15608 evalcond[0]=(((new_r10*x1216))+gconst20+((new_r00*x1215)));
15609 evalcond[1]=(x1219+((gconst20*x1215))+new_r00);
15610 evalcond[2]=(gconst19+((new_r00*x1216))+(((-1.0)*new_r10*x1218)));
15611 evalcond[3]=(gconst20+((new_r01*x1216))+(((-1.0)*new_r11*x1218)));
15612 evalcond[4]=(x1217+(((-1.0)*x1220))+new_r01);
15613 evalcond[5]=(x1217+(((-1.0)*x1220))+new_r10);
15614 evalcond[6]=(((new_r11*x1216))+((new_r01*x1215))+(((-1.0)*gconst19)));
15615 evalcond[7]=((((-1.0)*gconst20*x1218))+new_r11+(((-1.0)*x1219)));
15623 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15624 vinfos[0].jointtype = 1;
15625 vinfos[0].foffset = j0;
15626 vinfos[0].indices[0] = _ij0[0];
15627 vinfos[0].indices[1] = _ij0[1];
15628 vinfos[0].maxsolutions = _nj0;
15629 vinfos[1].jointtype = 1;
15630 vinfos[1].foffset = j1;
15631 vinfos[1].indices[0] = _ij1[0];
15632 vinfos[1].indices[1] = _ij1[1];
15633 vinfos[1].maxsolutions = _nj1;
15634 vinfos[2].jointtype = 1;
15635 vinfos[2].foffset = j2;
15636 vinfos[2].indices[0] = _ij2[0];
15637 vinfos[2].indices[1] = _ij2[1];
15638 vinfos[2].maxsolutions = _nj2;
15639 vinfos[3].jointtype = 1;
15640 vinfos[3].foffset = j3;
15641 vinfos[3].indices[0] = _ij3[0];
15642 vinfos[3].indices[1] = _ij3[1];
15643 vinfos[3].maxsolutions = _nj3;
15644 vinfos[4].jointtype = 1;
15645 vinfos[4].foffset = j4;
15646 vinfos[4].indices[0] = _ij4[0];
15647 vinfos[4].indices[1] = _ij4[1];
15648 vinfos[4].maxsolutions = _nj4;
15649 vinfos[5].jointtype = 1;
15650 vinfos[5].foffset = j5;
15651 vinfos[5].indices[0] = _ij5[0];
15652 vinfos[5].indices[1] = _ij5[1];
15653 vinfos[5].maxsolutions = _nj5;
15654 std::vector<int> vfree(0);
15667 IkReal j3array[1], cj3array[1], sj3array[1];
15668 bool j3valid[1]={
false};
15670 IkReal x1221=((1.0)*new_r10);
15679 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1222.
value)))+(x1223.
value));
15680 sj3array[0]=
IKsin(j3array[0]);
15681 cj3array[0]=
IKcos(j3array[0]);
15682 if( j3array[0] >
IKPI )
15686 else if( j3array[0] < -
IKPI )
15687 { j3array[0]+=
IK2PI;
15690 for(
int ij3 = 0; ij3 < 1; ++ij3)
15692 if( !j3valid[ij3] )
15696 _ij3[0] = ij3; _ij3[1] = -1;
15697 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
15701 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15706 IkReal evalcond[8];
15707 IkReal x1224=
IKcos(j3);
15708 IkReal x1225=
IKsin(j3);
15709 IkReal x1226=(gconst20*x1225);
15710 IkReal x1227=((1.0)*x1224);
15711 IkReal x1228=(gconst19*x1225);
15712 IkReal x1229=(gconst19*x1227);
15713 evalcond[0]=(gconst20+((new_r00*x1224))+((new_r10*x1225)));
15714 evalcond[1]=(x1228+((gconst20*x1224))+new_r00);
15715 evalcond[2]=(gconst19+((new_r00*x1225))+(((-1.0)*new_r10*x1227)));
15716 evalcond[3]=(gconst20+((new_r01*x1225))+(((-1.0)*new_r11*x1227)));
15717 evalcond[4]=(x1226+(((-1.0)*x1229))+new_r01);
15718 evalcond[5]=(x1226+(((-1.0)*x1229))+new_r10);
15719 evalcond[6]=(((new_r11*x1225))+((new_r01*x1224))+(((-1.0)*gconst19)));
15720 evalcond[7]=((((-1.0)*x1228))+(((-1.0)*gconst20*x1227))+new_r11);
15728 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15729 vinfos[0].jointtype = 1;
15730 vinfos[0].foffset = j0;
15731 vinfos[0].indices[0] = _ij0[0];
15732 vinfos[0].indices[1] = _ij0[1];
15733 vinfos[0].maxsolutions = _nj0;
15734 vinfos[1].jointtype = 1;
15735 vinfos[1].foffset = j1;
15736 vinfos[1].indices[0] = _ij1[0];
15737 vinfos[1].indices[1] = _ij1[1];
15738 vinfos[1].maxsolutions = _nj1;
15739 vinfos[2].jointtype = 1;
15740 vinfos[2].foffset = j2;
15741 vinfos[2].indices[0] = _ij2[0];
15742 vinfos[2].indices[1] = _ij2[1];
15743 vinfos[2].maxsolutions = _nj2;
15744 vinfos[3].jointtype = 1;
15745 vinfos[3].foffset = j3;
15746 vinfos[3].indices[0] = _ij3[0];
15747 vinfos[3].indices[1] = _ij3[1];
15748 vinfos[3].maxsolutions = _nj3;
15749 vinfos[4].jointtype = 1;
15750 vinfos[4].foffset = j4;
15751 vinfos[4].indices[0] = _ij4[0];
15752 vinfos[4].indices[1] = _ij4[1];
15753 vinfos[4].maxsolutions = _nj4;
15754 vinfos[5].jointtype = 1;
15755 vinfos[5].foffset = j5;
15756 vinfos[5].indices[0] = _ij5[0];
15757 vinfos[5].indices[1] = _ij5[1];
15758 vinfos[5].maxsolutions = _nj5;
15759 std::vector<int> vfree(0);
15771 if( bgotonextstatement )
15773 bool bgotonextstatement =
true;
15776 IkReal x1230=((-1.0)*new_r00);
15777 IkReal x1232 = ((new_r10*new_r10)+(new_r00*new_r00));
15778 if(
IKabs(x1232)==0){
15781 IkReal x1231=
pow(x1232,-0.5);
15786 IkReal gconst21=((3.14159265358979)+(((-1.0)*(x1233.
value))));
15787 IkReal gconst22=(x1230*x1231);
15788 IkReal gconst23=((1.0)*new_r10*x1231);
15793 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+(x1234.
value)+j5)))), 6.28318530717959)));
15794 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
15796 bgotonextstatement=
false;
15799 IkReal x1235=((-1.0)*new_r00);
15804 IkReal x1236=((1.0)*(x1238.
value));
15805 IkReal x1237=x1231;
15808 j4=3.14159265358979;
15811 j5=((3.14159265)+(((-1.0)*x1236)));
15812 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1236)));
15813 IkReal gconst22=(x1235*x1237);
15814 IkReal gconst23=((1.0)*new_r10*x1237);
15815 IkReal x1239=new_r10*new_r10;
15816 IkReal x1240=(new_r10*new_r11);
15817 IkReal x1241=((((-1.0)*x1240))+(((-1.0)*new_r00*new_r01)));
15818 IkReal x1242=x1231;
15819 IkReal x1243=(new_r10*x1242);
15821 j3eval[1]=((
IKabs((((x1240*x1242))+((new_r00*x1243)))))+(
IKabs(((((-1.0)*x1239*x1242))+((new_r01*x1243))))));
15822 j3eval[2]=
IKsign(x1241);
15823 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
15827 IkReal x1244=((-1.0)*new_r00);
15832 IkReal x1245=((1.0)*(x1247.
value));
15833 IkReal x1246=x1231;
15836 j4=3.14159265358979;
15839 j5=((3.14159265)+(((-1.0)*x1245)));
15840 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1245)));
15841 IkReal gconst22=(x1244*x1246);
15842 IkReal gconst23=((1.0)*new_r10*x1246);
15843 IkReal x1248=new_r10*new_r10;
15844 IkReal x1249=new_r00*new_r00*new_r00;
15849 IkReal x1250=x1253.
value;
15850 IkReal x1251=(x1248*x1250);
15851 IkReal x1252=(x1249*x1250);
15852 j3eval[0]=((
IKabs((x1251+((new_r11*x1252))+((new_r00*new_r11*x1251)))))+(
IKabs((((new_r00*new_r10*x1250))+((new_r01*x1252))+((new_r00*new_r01*x1251))))));
15853 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15857 IkReal x1254=((-1.0)*new_r00);
15862 IkReal x1255=((1.0)*(x1257.
value));
15863 IkReal x1256=x1231;
15866 j4=3.14159265358979;
15869 j5=((3.14159265)+(((-1.0)*x1255)));
15870 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1255)));
15871 IkReal gconst22=(x1254*x1256);
15872 IkReal gconst23=((1.0)*new_r10*x1256);
15873 IkReal x1258=new_r10*new_r10;
15874 IkReal x1259=new_r00*new_r00;
15879 IkReal x1260=x1263.
value;
15880 IkReal x1261=(new_r10*x1260);
15881 IkReal x1262=(x1258*x1260);
15882 j3eval[0]=((
IKabs((x1262+(((-1.0)*x1260*(x1259*x1259)))+(((-1.0)*x1259*x1262)))))+(
IKabs((((new_r00*x1261))+((x1261*(new_r00*new_r00*new_r00)))+((new_r00*x1261*(new_r10*new_r10)))))));
15883 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15886 IkReal evalcond[3];
15887 bool bgotonextstatement =
true;
15890 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
15891 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
15893 bgotonextstatement=
false;
15900 IkReal x1264=((1.0)*(x1265.
value));
15903 j4=3.14159265358979;
15906 j5=((3.14159265)+(((-1.0)*x1264)));
15909 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1264)));
15911 IkReal x1266 = new_r10*new_r10;
15912 if(
IKabs(x1266)==0){
15915 IkReal gconst23=((1.0)*new_r10*(
pow(x1266,-0.5)));
15917 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
15920 IkReal j3array[2], cj3array[2], sj3array[2];
15921 bool j3valid[2]={
false};
15927 sj3array[0]=((-1.0)*new_r01*(x1267.
value));
15930 j3valid[0] = j3valid[1] =
true;
15931 j3array[0] =
IKasin(sj3array[0]);
15932 cj3array[0] =
IKcos(j3array[0]);
15933 sj3array[1] = sj3array[0];
15934 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
15935 cj3array[1] = -cj3array[0];
15937 else if( isnan(sj3array[0]) )
15941 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
15943 for(
int ij3 = 0; ij3 < 2; ++ij3)
15945 if( !j3valid[ij3] )
15949 _ij3[0] = ij3; _ij3[1] = -1;
15950 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
15954 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
15957 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15959 IkReal evalcond[6];
15960 IkReal x1268=
IKcos(j3);
15961 IkReal x1269=
IKsin(j3);
15962 evalcond[0]=(new_r01*x1268);
15963 evalcond[1]=(gconst23*x1268);
15964 evalcond[2]=((-1.0)*new_r10*x1268);
15965 evalcond[3]=(((new_r01*x1269))+gconst23);
15966 evalcond[4]=(gconst23+((new_r10*x1269)));
15967 evalcond[5]=(((gconst23*x1269))+new_r10);
15975 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15976 vinfos[0].jointtype = 1;
15977 vinfos[0].foffset = j0;
15978 vinfos[0].indices[0] = _ij0[0];
15979 vinfos[0].indices[1] = _ij0[1];
15980 vinfos[0].maxsolutions = _nj0;
15981 vinfos[1].jointtype = 1;
15982 vinfos[1].foffset = j1;
15983 vinfos[1].indices[0] = _ij1[0];
15984 vinfos[1].indices[1] = _ij1[1];
15985 vinfos[1].maxsolutions = _nj1;
15986 vinfos[2].jointtype = 1;
15987 vinfos[2].foffset = j2;
15988 vinfos[2].indices[0] = _ij2[0];
15989 vinfos[2].indices[1] = _ij2[1];
15990 vinfos[2].maxsolutions = _nj2;
15991 vinfos[3].jointtype = 1;
15992 vinfos[3].foffset = j3;
15993 vinfos[3].indices[0] = _ij3[0];
15994 vinfos[3].indices[1] = _ij3[1];
15995 vinfos[3].maxsolutions = _nj3;
15996 vinfos[4].jointtype = 1;
15997 vinfos[4].foffset = j4;
15998 vinfos[4].indices[0] = _ij4[0];
15999 vinfos[4].indices[1] = _ij4[1];
16000 vinfos[4].maxsolutions = _nj4;
16001 vinfos[5].jointtype = 1;
16002 vinfos[5].foffset = j5;
16003 vinfos[5].indices[0] = _ij5[0];
16004 vinfos[5].indices[1] = _ij5[1];
16005 vinfos[5].maxsolutions = _nj5;
16006 std::vector<int> vfree(0);
16015 IkReal j3array[2], cj3array[2], sj3array[2];
16016 bool j3valid[2]={
false};
16022 sj3array[0]=((-1.0)*gconst23*(x1270.
value));
16025 j3valid[0] = j3valid[1] =
true;
16026 j3array[0] =
IKasin(sj3array[0]);
16027 cj3array[0] =
IKcos(j3array[0]);
16028 sj3array[1] = sj3array[0];
16029 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
16030 cj3array[1] = -cj3array[0];
16032 else if( isnan(sj3array[0]) )
16036 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16038 for(
int ij3 = 0; ij3 < 2; ++ij3)
16040 if( !j3valid[ij3] )
16044 _ij3[0] = ij3; _ij3[1] = -1;
16045 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
16049 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16052 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16054 IkReal evalcond[6];
16055 IkReal x1271=
IKcos(j3);
16056 IkReal x1272=
IKsin(j3);
16057 IkReal x1273=(gconst23*x1272);
16058 evalcond[0]=(new_r01*x1271);
16059 evalcond[1]=(gconst23*x1271);
16060 evalcond[2]=((-1.0)*new_r10*x1271);
16061 evalcond[3]=(x1273+new_r01);
16062 evalcond[4]=(gconst23+((new_r10*x1272)));
16063 evalcond[5]=(x1273+new_r10);
16071 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16072 vinfos[0].jointtype = 1;
16073 vinfos[0].foffset = j0;
16074 vinfos[0].indices[0] = _ij0[0];
16075 vinfos[0].indices[1] = _ij0[1];
16076 vinfos[0].maxsolutions = _nj0;
16077 vinfos[1].jointtype = 1;
16078 vinfos[1].foffset = j1;
16079 vinfos[1].indices[0] = _ij1[0];
16080 vinfos[1].indices[1] = _ij1[1];
16081 vinfos[1].maxsolutions = _nj1;
16082 vinfos[2].jointtype = 1;
16083 vinfos[2].foffset = j2;
16084 vinfos[2].indices[0] = _ij2[0];
16085 vinfos[2].indices[1] = _ij2[1];
16086 vinfos[2].maxsolutions = _nj2;
16087 vinfos[3].jointtype = 1;
16088 vinfos[3].foffset = j3;
16089 vinfos[3].indices[0] = _ij3[0];
16090 vinfos[3].indices[1] = _ij3[1];
16091 vinfos[3].maxsolutions = _nj3;
16092 vinfos[4].jointtype = 1;
16093 vinfos[4].foffset = j4;
16094 vinfos[4].indices[0] = _ij4[0];
16095 vinfos[4].indices[1] = _ij4[1];
16096 vinfos[4].maxsolutions = _nj4;
16097 vinfos[5].jointtype = 1;
16098 vinfos[5].foffset = j5;
16099 vinfos[5].indices[0] = _ij5[0];
16100 vinfos[5].indices[1] = _ij5[1];
16101 vinfos[5].maxsolutions = _nj5;
16102 std::vector<int> vfree(0);
16114 if( bgotonextstatement )
16116 bool bgotonextstatement =
true;
16119 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
16120 evalcond[1]=gconst23;
16121 evalcond[2]=gconst22;
16122 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
16124 bgotonextstatement=
false;
16127 IkReal x1274=((-1.0)*new_r00);
16132 IkReal x1275=((1.0)*(x1276.
value));
16135 j4=3.14159265358979;
16138 j5=((3.14159265)+(((-1.0)*x1275)));
16143 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1275)));
16144 IkReal gconst22=x1274;
16145 IkReal gconst23=((1.0)*new_r10);
16148 j3eval[2]=((
IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(
IKabs(((1.0)*new_r00*new_r10))));
16149 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
16153 IkReal x1277=((-1.0)*new_r00);
16158 IkReal x1278=((1.0)*(x1279.
value));
16161 j4=3.14159265358979;
16164 j5=((3.14159265)+(((-1.0)*x1278)));
16169 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1278)));
16170 IkReal gconst22=x1277;
16171 IkReal gconst23=((1.0)*new_r10);
16173 j3eval[1]=((
IKabs(((1.0)*new_r00*new_r10)))+(
IKabs(((-1.0)+(new_r10*new_r10)))));
16175 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
16179 IkReal x1280=((-1.0)*new_r00);
16184 IkReal x1281=((1.0)*(x1282.
value));
16187 j4=3.14159265358979;
16190 j5=((3.14159265)+(((-1.0)*x1281)));
16195 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1281)));
16196 IkReal gconst22=x1280;
16197 IkReal gconst23=((1.0)*new_r10);
16199 j3eval[1]=((
IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(
IKabs(((2.0)*new_r00*new_r10))));
16201 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
16208 IkReal j3array[1], cj3array[1], sj3array[1];
16209 bool j3valid[1]={
false};
16211 IkReal x1283=((1.0)*gconst23);
16220 j3array[0]=((-1.5707963267949)+(x1284.
value)+(((1.5707963267949)*(x1285.
value))));
16221 sj3array[0]=
IKsin(j3array[0]);
16222 cj3array[0]=
IKcos(j3array[0]);
16223 if( j3array[0] >
IKPI )
16227 else if( j3array[0] < -
IKPI )
16228 { j3array[0]+=
IK2PI;
16231 for(
int ij3 = 0; ij3 < 1; ++ij3)
16233 if( !j3valid[ij3] )
16237 _ij3[0] = ij3; _ij3[1] = -1;
16238 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16242 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16245 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16247 IkReal evalcond[6];
16248 IkReal x1286=
IKsin(j3);
16249 IkReal x1287=
IKcos(j3);
16250 IkReal x1288=(gconst23*x1286);
16251 IkReal x1289=(gconst22*x1286);
16252 IkReal x1290=(gconst23*x1287);
16253 IkReal x1291=((1.0)*x1287);
16254 IkReal x1292=(gconst22*x1291);
16255 evalcond[0]=(x1288+(((-1.0)*x1292)));
16256 evalcond[1]=(((new_r10*x1286))+gconst23+((new_r00*x1287)));
16257 evalcond[2]=(x1289+x1290+new_r00);
16258 evalcond[3]=((((-1.0)*new_r10*x1291))+gconst22+((new_r00*x1286)));
16259 evalcond[4]=((((-1.0)*x1289))+(((-1.0)*x1290)));
16260 evalcond[5]=(x1288+(((-1.0)*x1292))+new_r10);
16268 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16269 vinfos[0].jointtype = 1;
16270 vinfos[0].foffset = j0;
16271 vinfos[0].indices[0] = _ij0[0];
16272 vinfos[0].indices[1] = _ij0[1];
16273 vinfos[0].maxsolutions = _nj0;
16274 vinfos[1].jointtype = 1;
16275 vinfos[1].foffset = j1;
16276 vinfos[1].indices[0] = _ij1[0];
16277 vinfos[1].indices[1] = _ij1[1];
16278 vinfos[1].maxsolutions = _nj1;
16279 vinfos[2].jointtype = 1;
16280 vinfos[2].foffset = j2;
16281 vinfos[2].indices[0] = _ij2[0];
16282 vinfos[2].indices[1] = _ij2[1];
16283 vinfos[2].maxsolutions = _nj2;
16284 vinfos[3].jointtype = 1;
16285 vinfos[3].foffset = j3;
16286 vinfos[3].indices[0] = _ij3[0];
16287 vinfos[3].indices[1] = _ij3[1];
16288 vinfos[3].maxsolutions = _nj3;
16289 vinfos[4].jointtype = 1;
16290 vinfos[4].foffset = j4;
16291 vinfos[4].indices[0] = _ij4[0];
16292 vinfos[4].indices[1] = _ij4[1];
16293 vinfos[4].maxsolutions = _nj4;
16294 vinfos[5].jointtype = 1;
16295 vinfos[5].foffset = j5;
16296 vinfos[5].indices[0] = _ij5[0];
16297 vinfos[5].indices[1] = _ij5[1];
16298 vinfos[5].maxsolutions = _nj5;
16299 std::vector<int> vfree(0);
16312 IkReal j3array[1], cj3array[1], sj3array[1];
16313 bool j3valid[1]={
false};
16323 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1293.
value)))+(x1294.
value));
16324 sj3array[0]=
IKsin(j3array[0]);
16325 cj3array[0]=
IKcos(j3array[0]);
16326 if( j3array[0] >
IKPI )
16330 else if( j3array[0] < -
IKPI )
16331 { j3array[0]+=
IK2PI;
16334 for(
int ij3 = 0; ij3 < 1; ++ij3)
16336 if( !j3valid[ij3] )
16340 _ij3[0] = ij3; _ij3[1] = -1;
16341 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16345 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16348 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16350 IkReal evalcond[6];
16351 IkReal x1295=
IKsin(j3);
16352 IkReal x1296=
IKcos(j3);
16353 IkReal x1297=(gconst23*x1295);
16354 IkReal x1298=(gconst22*x1295);
16355 IkReal x1299=(gconst23*x1296);
16356 IkReal x1300=((1.0)*x1296);
16357 IkReal x1301=(gconst22*x1300);
16358 evalcond[0]=(x1297+(((-1.0)*x1301)));
16359 evalcond[1]=(gconst23+((new_r10*x1295))+((new_r00*x1296)));
16360 evalcond[2]=(x1298+x1299+new_r00);
16361 evalcond[3]=((((-1.0)*new_r10*x1300))+gconst22+((new_r00*x1295)));
16362 evalcond[4]=((((-1.0)*x1298))+(((-1.0)*x1299)));
16363 evalcond[5]=(x1297+(((-1.0)*x1301))+new_r10);
16371 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16372 vinfos[0].jointtype = 1;
16373 vinfos[0].foffset = j0;
16374 vinfos[0].indices[0] = _ij0[0];
16375 vinfos[0].indices[1] = _ij0[1];
16376 vinfos[0].maxsolutions = _nj0;
16377 vinfos[1].jointtype = 1;
16378 vinfos[1].foffset = j1;
16379 vinfos[1].indices[0] = _ij1[0];
16380 vinfos[1].indices[1] = _ij1[1];
16381 vinfos[1].maxsolutions = _nj1;
16382 vinfos[2].jointtype = 1;
16383 vinfos[2].foffset = j2;
16384 vinfos[2].indices[0] = _ij2[0];
16385 vinfos[2].indices[1] = _ij2[1];
16386 vinfos[2].maxsolutions = _nj2;
16387 vinfos[3].jointtype = 1;
16388 vinfos[3].foffset = j3;
16389 vinfos[3].indices[0] = _ij3[0];
16390 vinfos[3].indices[1] = _ij3[1];
16391 vinfos[3].maxsolutions = _nj3;
16392 vinfos[4].jointtype = 1;
16393 vinfos[4].foffset = j4;
16394 vinfos[4].indices[0] = _ij4[0];
16395 vinfos[4].indices[1] = _ij4[1];
16396 vinfos[4].maxsolutions = _nj4;
16397 vinfos[5].jointtype = 1;
16398 vinfos[5].foffset = j5;
16399 vinfos[5].indices[0] = _ij5[0];
16400 vinfos[5].indices[1] = _ij5[1];
16401 vinfos[5].maxsolutions = _nj5;
16402 std::vector<int> vfree(0);
16415 IkReal j3array[1], cj3array[1], sj3array[1];
16416 bool j3valid[1]={
false};
16426 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1302.
value)))+(x1303.
value));
16427 sj3array[0]=
IKsin(j3array[0]);
16428 cj3array[0]=
IKcos(j3array[0]);
16429 if( j3array[0] >
IKPI )
16433 else if( j3array[0] < -
IKPI )
16434 { j3array[0]+=
IK2PI;
16437 for(
int ij3 = 0; ij3 < 1; ++ij3)
16439 if( !j3valid[ij3] )
16443 _ij3[0] = ij3; _ij3[1] = -1;
16444 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16448 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16451 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16453 IkReal evalcond[6];
16454 IkReal x1304=
IKsin(j3);
16455 IkReal x1305=
IKcos(j3);
16456 IkReal x1306=(gconst23*x1304);
16457 IkReal x1307=(gconst22*x1304);
16458 IkReal x1308=(gconst23*x1305);
16459 IkReal x1309=((1.0)*x1305);
16460 IkReal x1310=(gconst22*x1309);
16461 evalcond[0]=(x1306+(((-1.0)*x1310)));
16462 evalcond[1]=(gconst23+((new_r10*x1304))+((new_r00*x1305)));
16463 evalcond[2]=(x1307+x1308+new_r00);
16464 evalcond[3]=((((-1.0)*new_r10*x1309))+gconst22+((new_r00*x1304)));
16465 evalcond[4]=((((-1.0)*x1308))+(((-1.0)*x1307)));
16466 evalcond[5]=(x1306+(((-1.0)*x1310))+new_r10);
16474 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16475 vinfos[0].jointtype = 1;
16476 vinfos[0].foffset = j0;
16477 vinfos[0].indices[0] = _ij0[0];
16478 vinfos[0].indices[1] = _ij0[1];
16479 vinfos[0].maxsolutions = _nj0;
16480 vinfos[1].jointtype = 1;
16481 vinfos[1].foffset = j1;
16482 vinfos[1].indices[0] = _ij1[0];
16483 vinfos[1].indices[1] = _ij1[1];
16484 vinfos[1].maxsolutions = _nj1;
16485 vinfos[2].jointtype = 1;
16486 vinfos[2].foffset = j2;
16487 vinfos[2].indices[0] = _ij2[0];
16488 vinfos[2].indices[1] = _ij2[1];
16489 vinfos[2].maxsolutions = _nj2;
16490 vinfos[3].jointtype = 1;
16491 vinfos[3].foffset = j3;
16492 vinfos[3].indices[0] = _ij3[0];
16493 vinfos[3].indices[1] = _ij3[1];
16494 vinfos[3].maxsolutions = _nj3;
16495 vinfos[4].jointtype = 1;
16496 vinfos[4].foffset = j4;
16497 vinfos[4].indices[0] = _ij4[0];
16498 vinfos[4].indices[1] = _ij4[1];
16499 vinfos[4].maxsolutions = _nj4;
16500 vinfos[5].jointtype = 1;
16501 vinfos[5].foffset = j5;
16502 vinfos[5].indices[0] = _ij5[0];
16503 vinfos[5].indices[1] = _ij5[1];
16504 vinfos[5].maxsolutions = _nj5;
16505 std::vector<int> vfree(0);
16517 if( bgotonextstatement )
16519 bool bgotonextstatement =
true;
16522 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
16523 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
16525 bgotonextstatement=
false;
16527 IkReal j3array[2], cj3array[2], sj3array[2];
16528 bool j3valid[2]={
false};
16534 sj3array[0]=(new_r11*(x1311.
value));
16537 j3valid[0] = j3valid[1] =
true;
16538 j3array[0] =
IKasin(sj3array[0]);
16539 cj3array[0] =
IKcos(j3array[0]);
16540 sj3array[1] = sj3array[0];
16541 j3array[1] = j3array[0] > 0 ? (
IKPI-j3array[0]) : (-
IKPI-j3array[0]);
16542 cj3array[1] = -cj3array[0];
16544 else if( isnan(sj3array[0]) )
16548 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16550 for(
int ij3 = 0; ij3 < 2; ++ij3)
16552 if( !j3valid[ij3] )
16556 _ij3[0] = ij3; _ij3[1] = -1;
16557 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
16561 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16564 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16566 IkReal evalcond[6];
16567 IkReal x1312=
IKcos(j3);
16568 IkReal x1313=
IKsin(j3);
16569 IkReal x1314=((-1.0)*x1312);
16570 evalcond[0]=(new_r00*x1312);
16571 evalcond[1]=(new_r11*x1314);
16572 evalcond[2]=(gconst22*x1314);
16573 evalcond[3]=(((new_r00*x1313))+gconst22);
16574 evalcond[4]=(((gconst22*x1313))+new_r00);
16575 evalcond[5]=(((new_r11*x1313))+(((-1.0)*gconst22)));
16583 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16584 vinfos[0].jointtype = 1;
16585 vinfos[0].foffset = j0;
16586 vinfos[0].indices[0] = _ij0[0];
16587 vinfos[0].indices[1] = _ij0[1];
16588 vinfos[0].maxsolutions = _nj0;
16589 vinfos[1].jointtype = 1;
16590 vinfos[1].foffset = j1;
16591 vinfos[1].indices[0] = _ij1[0];
16592 vinfos[1].indices[1] = _ij1[1];
16593 vinfos[1].maxsolutions = _nj1;
16594 vinfos[2].jointtype = 1;
16595 vinfos[2].foffset = j2;
16596 vinfos[2].indices[0] = _ij2[0];
16597 vinfos[2].indices[1] = _ij2[1];
16598 vinfos[2].maxsolutions = _nj2;
16599 vinfos[3].jointtype = 1;
16600 vinfos[3].foffset = j3;
16601 vinfos[3].indices[0] = _ij3[0];
16602 vinfos[3].indices[1] = _ij3[1];
16603 vinfos[3].maxsolutions = _nj3;
16604 vinfos[4].jointtype = 1;
16605 vinfos[4].foffset = j4;
16606 vinfos[4].indices[0] = _ij4[0];
16607 vinfos[4].indices[1] = _ij4[1];
16608 vinfos[4].maxsolutions = _nj4;
16609 vinfos[5].jointtype = 1;
16610 vinfos[5].foffset = j5;
16611 vinfos[5].indices[0] = _ij5[0];
16612 vinfos[5].indices[1] = _ij5[1];
16613 vinfos[5].maxsolutions = _nj5;
16614 std::vector<int> vfree(0);
16622 if( bgotonextstatement )
16624 bool bgotonextstatement =
true;
16627 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
16628 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
16630 bgotonextstatement=
false;
16633 IkReal x1315=((-1.0)*new_r00);
16638 IkReal x1316=((1.0)*(x1317.
value));
16641 j4=3.14159265358979;
16644 j5=((3.14159265)+(((-1.0)*x1316)));
16649 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1316)));
16650 IkReal x1318 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16651 if(
IKabs(x1318)==0){
16654 IkReal gconst22=(x1315*(
pow(x1318,-0.5)));
16656 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
16657 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
16661 IkReal x1319=((-1.0)*new_r00);
16666 IkReal x1320=((1.0)*(x1321.
value));
16669 j4=3.14159265358979;
16672 j5=((3.14159265)+(((-1.0)*x1320)));
16677 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1320)));
16678 IkReal x1322 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16679 if(
IKabs(x1322)==0){
16682 IkReal gconst22=(x1319*(
pow(x1322,-0.5)));
16685 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
16689 IkReal x1323=((-1.0)*new_r00);
16694 IkReal x1324=((1.0)*(x1325.
value));
16697 j4=3.14159265358979;
16700 j5=((3.14159265)+(((-1.0)*x1324)));
16705 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1324)));
16706 IkReal x1326 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16707 if(
IKabs(x1326)==0){
16710 IkReal gconst22=(x1323*(
pow(x1326,-0.5)));
16714 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
16721 IkReal j3array[1], cj3array[1], sj3array[1];
16722 bool j3valid[1]={
false};
16735 sj3array[0]=
IKsin(j3array[0]);
16736 cj3array[0]=
IKcos(j3array[0]);
16737 if( j3array[0] >
IKPI )
16741 else if( j3array[0] < -
IKPI )
16742 { j3array[0]+=
IK2PI;
16745 for(
int ij3 = 0; ij3 < 1; ++ij3)
16747 if( !j3valid[ij3] )
16751 _ij3[0] = ij3; _ij3[1] = -1;
16752 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16756 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16759 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16761 IkReal evalcond[8];
16762 IkReal x1329=
IKsin(j3);
16763 IkReal x1330=
IKcos(j3);
16764 IkReal x1331=(gconst22*x1330);
16765 IkReal x1332=(gconst22*x1329);
16766 evalcond[0]=(new_r01*x1329);
16767 evalcond[1]=(new_r00*x1330);
16768 evalcond[2]=((-1.0)*x1332);
16769 evalcond[3]=((-1.0)*x1331);
16770 evalcond[4]=(((new_r00*x1329))+gconst22);
16771 evalcond[5]=(x1332+new_r00);
16772 evalcond[6]=((((-1.0)*x1331))+new_r01);
16773 evalcond[7]=(((new_r01*x1330))+(((-1.0)*gconst22)));
16781 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16782 vinfos[0].jointtype = 1;
16783 vinfos[0].foffset = j0;
16784 vinfos[0].indices[0] = _ij0[0];
16785 vinfos[0].indices[1] = _ij0[1];
16786 vinfos[0].maxsolutions = _nj0;
16787 vinfos[1].jointtype = 1;
16788 vinfos[1].foffset = j1;
16789 vinfos[1].indices[0] = _ij1[0];
16790 vinfos[1].indices[1] = _ij1[1];
16791 vinfos[1].maxsolutions = _nj1;
16792 vinfos[2].jointtype = 1;
16793 vinfos[2].foffset = j2;
16794 vinfos[2].indices[0] = _ij2[0];
16795 vinfos[2].indices[1] = _ij2[1];
16796 vinfos[2].maxsolutions = _nj2;
16797 vinfos[3].jointtype = 1;
16798 vinfos[3].foffset = j3;
16799 vinfos[3].indices[0] = _ij3[0];
16800 vinfos[3].indices[1] = _ij3[1];
16801 vinfos[3].maxsolutions = _nj3;
16802 vinfos[4].jointtype = 1;
16803 vinfos[4].foffset = j4;
16804 vinfos[4].indices[0] = _ij4[0];
16805 vinfos[4].indices[1] = _ij4[1];
16806 vinfos[4].maxsolutions = _nj4;
16807 vinfos[5].jointtype = 1;
16808 vinfos[5].foffset = j5;
16809 vinfos[5].indices[0] = _ij5[0];
16810 vinfos[5].indices[1] = _ij5[1];
16811 vinfos[5].maxsolutions = _nj5;
16812 std::vector<int> vfree(0);
16825 IkReal j3array[1], cj3array[1], sj3array[1];
16826 bool j3valid[1]={
false};
16839 sj3array[0]=
IKsin(j3array[0]);
16840 cj3array[0]=
IKcos(j3array[0]);
16841 if( j3array[0] >
IKPI )
16845 else if( j3array[0] < -
IKPI )
16846 { j3array[0]+=
IK2PI;
16849 for(
int ij3 = 0; ij3 < 1; ++ij3)
16851 if( !j3valid[ij3] )
16855 _ij3[0] = ij3; _ij3[1] = -1;
16856 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16860 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16863 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16865 IkReal evalcond[8];
16866 IkReal x1335=
IKsin(j3);
16867 IkReal x1336=
IKcos(j3);
16868 IkReal x1337=(gconst22*x1336);
16869 IkReal x1338=(gconst22*x1335);
16870 evalcond[0]=(new_r01*x1335);
16871 evalcond[1]=(new_r00*x1336);
16872 evalcond[2]=((-1.0)*x1338);
16873 evalcond[3]=((-1.0)*x1337);
16874 evalcond[4]=(gconst22+((new_r00*x1335)));
16875 evalcond[5]=(x1338+new_r00);
16876 evalcond[6]=((((-1.0)*x1337))+new_r01);
16877 evalcond[7]=(((new_r01*x1336))+(((-1.0)*gconst22)));
16885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16886 vinfos[0].jointtype = 1;
16887 vinfos[0].foffset = j0;
16888 vinfos[0].indices[0] = _ij0[0];
16889 vinfos[0].indices[1] = _ij0[1];
16890 vinfos[0].maxsolutions = _nj0;
16891 vinfos[1].jointtype = 1;
16892 vinfos[1].foffset = j1;
16893 vinfos[1].indices[0] = _ij1[0];
16894 vinfos[1].indices[1] = _ij1[1];
16895 vinfos[1].maxsolutions = _nj1;
16896 vinfos[2].jointtype = 1;
16897 vinfos[2].foffset = j2;
16898 vinfos[2].indices[0] = _ij2[0];
16899 vinfos[2].indices[1] = _ij2[1];
16900 vinfos[2].maxsolutions = _nj2;
16901 vinfos[3].jointtype = 1;
16902 vinfos[3].foffset = j3;
16903 vinfos[3].indices[0] = _ij3[0];
16904 vinfos[3].indices[1] = _ij3[1];
16905 vinfos[3].maxsolutions = _nj3;
16906 vinfos[4].jointtype = 1;
16907 vinfos[4].foffset = j4;
16908 vinfos[4].indices[0] = _ij4[0];
16909 vinfos[4].indices[1] = _ij4[1];
16910 vinfos[4].maxsolutions = _nj4;
16911 vinfos[5].jointtype = 1;
16912 vinfos[5].foffset = j5;
16913 vinfos[5].indices[0] = _ij5[0];
16914 vinfos[5].indices[1] = _ij5[1];
16915 vinfos[5].maxsolutions = _nj5;
16916 std::vector<int> vfree(0);
16929 IkReal j3array[1], cj3array[1], sj3array[1];
16930 bool j3valid[1]={
false};
16940 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1339.
value)))+(x1340.
value));
16941 sj3array[0]=
IKsin(j3array[0]);
16942 cj3array[0]=
IKcos(j3array[0]);
16943 if( j3array[0] >
IKPI )
16947 else if( j3array[0] < -
IKPI )
16948 { j3array[0]+=
IK2PI;
16951 for(
int ij3 = 0; ij3 < 1; ++ij3)
16953 if( !j3valid[ij3] )
16957 _ij3[0] = ij3; _ij3[1] = -1;
16958 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
16962 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
16965 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16967 IkReal evalcond[8];
16968 IkReal x1341=
IKsin(j3);
16969 IkReal x1342=
IKcos(j3);
16970 IkReal x1343=(gconst22*x1342);
16971 IkReal x1344=(gconst22*x1341);
16972 evalcond[0]=(new_r01*x1341);
16973 evalcond[1]=(new_r00*x1342);
16974 evalcond[2]=((-1.0)*x1344);
16975 evalcond[3]=((-1.0)*x1343);
16976 evalcond[4]=(gconst22+((new_r00*x1341)));
16977 evalcond[5]=(x1344+new_r00);
16978 evalcond[6]=(new_r01+(((-1.0)*x1343)));
16979 evalcond[7]=(((new_r01*x1342))+(((-1.0)*gconst22)));
16987 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16988 vinfos[0].jointtype = 1;
16989 vinfos[0].foffset = j0;
16990 vinfos[0].indices[0] = _ij0[0];
16991 vinfos[0].indices[1] = _ij0[1];
16992 vinfos[0].maxsolutions = _nj0;
16993 vinfos[1].jointtype = 1;
16994 vinfos[1].foffset = j1;
16995 vinfos[1].indices[0] = _ij1[0];
16996 vinfos[1].indices[1] = _ij1[1];
16997 vinfos[1].maxsolutions = _nj1;
16998 vinfos[2].jointtype = 1;
16999 vinfos[2].foffset = j2;
17000 vinfos[2].indices[0] = _ij2[0];
17001 vinfos[2].indices[1] = _ij2[1];
17002 vinfos[2].maxsolutions = _nj2;
17003 vinfos[3].jointtype = 1;
17004 vinfos[3].foffset = j3;
17005 vinfos[3].indices[0] = _ij3[0];
17006 vinfos[3].indices[1] = _ij3[1];
17007 vinfos[3].maxsolutions = _nj3;
17008 vinfos[4].jointtype = 1;
17009 vinfos[4].foffset = j4;
17010 vinfos[4].indices[0] = _ij4[0];
17011 vinfos[4].indices[1] = _ij4[1];
17012 vinfos[4].maxsolutions = _nj4;
17013 vinfos[5].jointtype = 1;
17014 vinfos[5].foffset = j5;
17015 vinfos[5].indices[0] = _ij5[0];
17016 vinfos[5].indices[1] = _ij5[1];
17017 vinfos[5].maxsolutions = _nj5;
17018 std::vector<int> vfree(0);
17030 if( bgotonextstatement )
17032 bool bgotonextstatement =
true;
17035 evalcond[0]=
IKabs(new_r10);
17036 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
17038 bgotonextstatement=
false;
17041 IkReal x1345=((-1.0)*new_r00);
17046 IkReal x1346=((1.0)*(x1347.
value));
17049 j4=3.14159265358979;
17052 j5=((3.14159265)+(((-1.0)*x1346)));
17054 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1346)));
17055 IkReal x1348 = new_r00*new_r00;
17056 if(
IKabs(x1348)==0){
17059 IkReal gconst22=(x1345*(
pow(x1348,-0.5)));
17061 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
17062 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
17066 IkReal x1349=((-1.0)*new_r00);
17071 IkReal x1350=((1.0)*(x1351.
value));
17074 j4=3.14159265358979;
17077 j5=((3.14159265)+(((-1.0)*x1350)));
17079 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1350)));
17080 IkReal x1352 = new_r00*new_r00;
17081 if(
IKabs(x1352)==0){
17084 IkReal gconst22=(x1349*(
pow(x1352,-0.5)));
17086 j3eval[0]=((
IKabs(new_r00))+(
IKabs(new_r01)));
17087 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
17091 IkReal x1353=((-1.0)*new_r00);
17096 IkReal x1354=((1.0)*(x1355.
value));
17099 j4=3.14159265358979;
17102 j5=((3.14159265)+(((-1.0)*x1354)));
17104 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1354)));
17105 IkReal x1356 = new_r00*new_r00;
17106 if(
IKabs(x1356)==0){
17109 IkReal gconst22=(x1353*(
pow(x1356,-0.5)));
17112 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
17119 IkReal j3array[1], cj3array[1], sj3array[1];
17120 bool j3valid[1]={
false};
17133 sj3array[0]=
IKsin(j3array[0]);
17134 cj3array[0]=
IKcos(j3array[0]);
17135 if( j3array[0] >
IKPI )
17139 else if( j3array[0] < -
IKPI )
17140 { j3array[0]+=
IK2PI;
17143 for(
int ij3 = 0; ij3 < 1; ++ij3)
17145 if( !j3valid[ij3] )
17149 _ij3[0] = ij3; _ij3[1] = -1;
17150 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17154 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17157 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17159 IkReal evalcond[8];
17160 IkReal x1359=
IKcos(j3);
17161 IkReal x1360=
IKsin(j3);
17162 IkReal x1361=(gconst22*x1360);
17163 IkReal x1362=((1.0)*x1359);
17164 evalcond[0]=(new_r00*x1359);
17165 evalcond[1]=((-1.0)*gconst22*x1359);
17166 evalcond[2]=(gconst22+((new_r00*x1360)));
17167 evalcond[3]=(x1361+new_r00);
17168 evalcond[4]=((((-1.0)*gconst22*x1362))+new_r01);
17169 evalcond[5]=((((-1.0)*x1361))+new_r11);
17170 evalcond[6]=((((-1.0)*new_r11*x1362))+((new_r01*x1360)));
17171 evalcond[7]=(((new_r01*x1359))+(((-1.0)*gconst22))+((new_r11*x1360)));
17179 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17180 vinfos[0].jointtype = 1;
17181 vinfos[0].foffset = j0;
17182 vinfos[0].indices[0] = _ij0[0];
17183 vinfos[0].indices[1] = _ij0[1];
17184 vinfos[0].maxsolutions = _nj0;
17185 vinfos[1].jointtype = 1;
17186 vinfos[1].foffset = j1;
17187 vinfos[1].indices[0] = _ij1[0];
17188 vinfos[1].indices[1] = _ij1[1];
17189 vinfos[1].maxsolutions = _nj1;
17190 vinfos[2].jointtype = 1;
17191 vinfos[2].foffset = j2;
17192 vinfos[2].indices[0] = _ij2[0];
17193 vinfos[2].indices[1] = _ij2[1];
17194 vinfos[2].maxsolutions = _nj2;
17195 vinfos[3].jointtype = 1;
17196 vinfos[3].foffset = j3;
17197 vinfos[3].indices[0] = _ij3[0];
17198 vinfos[3].indices[1] = _ij3[1];
17199 vinfos[3].maxsolutions = _nj3;
17200 vinfos[4].jointtype = 1;
17201 vinfos[4].foffset = j4;
17202 vinfos[4].indices[0] = _ij4[0];
17203 vinfos[4].indices[1] = _ij4[1];
17204 vinfos[4].maxsolutions = _nj4;
17205 vinfos[5].jointtype = 1;
17206 vinfos[5].foffset = j5;
17207 vinfos[5].indices[0] = _ij5[0];
17208 vinfos[5].indices[1] = _ij5[1];
17209 vinfos[5].maxsolutions = _nj5;
17210 std::vector<int> vfree(0);
17223 IkReal j3array[1], cj3array[1], sj3array[1];
17224 bool j3valid[1]={
false};
17234 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1363.
value)))+(x1364.
value));
17235 sj3array[0]=
IKsin(j3array[0]);
17236 cj3array[0]=
IKcos(j3array[0]);
17237 if( j3array[0] >
IKPI )
17241 else if( j3array[0] < -
IKPI )
17242 { j3array[0]+=
IK2PI;
17245 for(
int ij3 = 0; ij3 < 1; ++ij3)
17247 if( !j3valid[ij3] )
17251 _ij3[0] = ij3; _ij3[1] = -1;
17252 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17256 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17259 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17261 IkReal evalcond[8];
17262 IkReal x1365=
IKcos(j3);
17263 IkReal x1366=
IKsin(j3);
17264 IkReal x1367=(gconst22*x1366);
17265 IkReal x1368=((1.0)*x1365);
17266 evalcond[0]=(new_r00*x1365);
17267 evalcond[1]=((-1.0)*gconst22*x1365);
17268 evalcond[2]=(gconst22+((new_r00*x1366)));
17269 evalcond[3]=(x1367+new_r00);
17270 evalcond[4]=((((-1.0)*gconst22*x1368))+new_r01);
17271 evalcond[5]=((((-1.0)*x1367))+new_r11);
17272 evalcond[6]=((((-1.0)*new_r11*x1368))+((new_r01*x1366)));
17273 evalcond[7]=((((-1.0)*gconst22))+((new_r01*x1365))+((new_r11*x1366)));
17281 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17282 vinfos[0].jointtype = 1;
17283 vinfos[0].foffset = j0;
17284 vinfos[0].indices[0] = _ij0[0];
17285 vinfos[0].indices[1] = _ij0[1];
17286 vinfos[0].maxsolutions = _nj0;
17287 vinfos[1].jointtype = 1;
17288 vinfos[1].foffset = j1;
17289 vinfos[1].indices[0] = _ij1[0];
17290 vinfos[1].indices[1] = _ij1[1];
17291 vinfos[1].maxsolutions = _nj1;
17292 vinfos[2].jointtype = 1;
17293 vinfos[2].foffset = j2;
17294 vinfos[2].indices[0] = _ij2[0];
17295 vinfos[2].indices[1] = _ij2[1];
17296 vinfos[2].maxsolutions = _nj2;
17297 vinfos[3].jointtype = 1;
17298 vinfos[3].foffset = j3;
17299 vinfos[3].indices[0] = _ij3[0];
17300 vinfos[3].indices[1] = _ij3[1];
17301 vinfos[3].maxsolutions = _nj3;
17302 vinfos[4].jointtype = 1;
17303 vinfos[4].foffset = j4;
17304 vinfos[4].indices[0] = _ij4[0];
17305 vinfos[4].indices[1] = _ij4[1];
17306 vinfos[4].maxsolutions = _nj4;
17307 vinfos[5].jointtype = 1;
17308 vinfos[5].foffset = j5;
17309 vinfos[5].indices[0] = _ij5[0];
17310 vinfos[5].indices[1] = _ij5[1];
17311 vinfos[5].maxsolutions = _nj5;
17312 std::vector<int> vfree(0);
17325 IkReal j3array[1], cj3array[1], sj3array[1];
17326 bool j3valid[1]={
false};
17336 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1369.
value)))+(x1370.
value));
17337 sj3array[0]=
IKsin(j3array[0]);
17338 cj3array[0]=
IKcos(j3array[0]);
17339 if( j3array[0] >
IKPI )
17343 else if( j3array[0] < -
IKPI )
17344 { j3array[0]+=
IK2PI;
17347 for(
int ij3 = 0; ij3 < 1; ++ij3)
17349 if( !j3valid[ij3] )
17353 _ij3[0] = ij3; _ij3[1] = -1;
17354 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17358 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17361 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17363 IkReal evalcond[8];
17364 IkReal x1371=
IKcos(j3);
17365 IkReal x1372=
IKsin(j3);
17366 IkReal x1373=(gconst22*x1372);
17367 IkReal x1374=((1.0)*x1371);
17368 evalcond[0]=(new_r00*x1371);
17369 evalcond[1]=((-1.0)*gconst22*x1371);
17370 evalcond[2]=(gconst22+((new_r00*x1372)));
17371 evalcond[3]=(x1373+new_r00);
17372 evalcond[4]=(new_r01+(((-1.0)*gconst22*x1374)));
17373 evalcond[5]=((((-1.0)*x1373))+new_r11);
17374 evalcond[6]=((((-1.0)*new_r11*x1374))+((new_r01*x1372)));
17375 evalcond[7]=(((new_r11*x1372))+(((-1.0)*gconst22))+((new_r01*x1371)));
17383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17384 vinfos[0].jointtype = 1;
17385 vinfos[0].foffset = j0;
17386 vinfos[0].indices[0] = _ij0[0];
17387 vinfos[0].indices[1] = _ij0[1];
17388 vinfos[0].maxsolutions = _nj0;
17389 vinfos[1].jointtype = 1;
17390 vinfos[1].foffset = j1;
17391 vinfos[1].indices[0] = _ij1[0];
17392 vinfos[1].indices[1] = _ij1[1];
17393 vinfos[1].maxsolutions = _nj1;
17394 vinfos[2].jointtype = 1;
17395 vinfos[2].foffset = j2;
17396 vinfos[2].indices[0] = _ij2[0];
17397 vinfos[2].indices[1] = _ij2[1];
17398 vinfos[2].maxsolutions = _nj2;
17399 vinfos[3].jointtype = 1;
17400 vinfos[3].foffset = j3;
17401 vinfos[3].indices[0] = _ij3[0];
17402 vinfos[3].indices[1] = _ij3[1];
17403 vinfos[3].maxsolutions = _nj3;
17404 vinfos[4].jointtype = 1;
17405 vinfos[4].foffset = j4;
17406 vinfos[4].indices[0] = _ij4[0];
17407 vinfos[4].indices[1] = _ij4[1];
17408 vinfos[4].maxsolutions = _nj4;
17409 vinfos[5].jointtype = 1;
17410 vinfos[5].foffset = j5;
17411 vinfos[5].indices[0] = _ij5[0];
17412 vinfos[5].indices[1] = _ij5[1];
17413 vinfos[5].maxsolutions = _nj5;
17414 std::vector<int> vfree(0);
17426 if( bgotonextstatement )
17428 bool bgotonextstatement =
true;
17433 bgotonextstatement=
false;
17438 if( bgotonextstatement )
17451 IkReal j3array[1], cj3array[1], sj3array[1];
17452 bool j3valid[1]={
false};
17454 IkReal x1375=((1.0)*gconst23);
17463 j3array[0]=((-1.5707963267949)+(x1376.
value)+(((1.5707963267949)*(x1377.
value))));
17464 sj3array[0]=
IKsin(j3array[0]);
17465 cj3array[0]=
IKcos(j3array[0]);
17466 if( j3array[0] >
IKPI )
17470 else if( j3array[0] < -
IKPI )
17471 { j3array[0]+=
IK2PI;
17474 for(
int ij3 = 0; ij3 < 1; ++ij3)
17476 if( !j3valid[ij3] )
17480 _ij3[0] = ij3; _ij3[1] = -1;
17481 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17485 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17488 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17490 IkReal evalcond[8];
17491 IkReal x1378=
IKsin(j3);
17492 IkReal x1379=
IKcos(j3);
17493 IkReal x1380=((1.0)*gconst22);
17494 IkReal x1381=(gconst23*x1378);
17495 IkReal x1382=((1.0)*x1379);
17496 IkReal x1383=(x1379*x1380);
17497 evalcond[0]=(gconst23+((new_r10*x1378))+((new_r00*x1379)));
17498 evalcond[1]=(((gconst23*x1379))+((gconst22*x1378))+new_r00);
17499 evalcond[2]=(gconst22+(((-1.0)*new_r10*x1382))+((new_r00*x1378)));
17500 evalcond[3]=(gconst23+(((-1.0)*new_r11*x1382))+((new_r01*x1378)));
17501 evalcond[4]=((((-1.0)*x1383))+x1381+new_r01);
17502 evalcond[5]=((((-1.0)*x1383))+x1381+new_r10);
17503 evalcond[6]=((((-1.0)*x1380))+((new_r11*x1378))+((new_r01*x1379)));
17504 evalcond[7]=((((-1.0)*gconst23*x1382))+new_r11+(((-1.0)*x1378*x1380)));
17512 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17513 vinfos[0].jointtype = 1;
17514 vinfos[0].foffset = j0;
17515 vinfos[0].indices[0] = _ij0[0];
17516 vinfos[0].indices[1] = _ij0[1];
17517 vinfos[0].maxsolutions = _nj0;
17518 vinfos[1].jointtype = 1;
17519 vinfos[1].foffset = j1;
17520 vinfos[1].indices[0] = _ij1[0];
17521 vinfos[1].indices[1] = _ij1[1];
17522 vinfos[1].maxsolutions = _nj1;
17523 vinfos[2].jointtype = 1;
17524 vinfos[2].foffset = j2;
17525 vinfos[2].indices[0] = _ij2[0];
17526 vinfos[2].indices[1] = _ij2[1];
17527 vinfos[2].maxsolutions = _nj2;
17528 vinfos[3].jointtype = 1;
17529 vinfos[3].foffset = j3;
17530 vinfos[3].indices[0] = _ij3[0];
17531 vinfos[3].indices[1] = _ij3[1];
17532 vinfos[3].maxsolutions = _nj3;
17533 vinfos[4].jointtype = 1;
17534 vinfos[4].foffset = j4;
17535 vinfos[4].indices[0] = _ij4[0];
17536 vinfos[4].indices[1] = _ij4[1];
17537 vinfos[4].maxsolutions = _nj4;
17538 vinfos[5].jointtype = 1;
17539 vinfos[5].foffset = j5;
17540 vinfos[5].indices[0] = _ij5[0];
17541 vinfos[5].indices[1] = _ij5[1];
17542 vinfos[5].maxsolutions = _nj5;
17543 std::vector<int> vfree(0);
17556 IkReal j3array[1], cj3array[1], sj3array[1];
17557 bool j3valid[1]={
false};
17559 IkReal x1384=((1.0)*gconst22);
17568 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.
value)))+(x1386.
value));
17569 sj3array[0]=
IKsin(j3array[0]);
17570 cj3array[0]=
IKcos(j3array[0]);
17571 if( j3array[0] >
IKPI )
17575 else if( j3array[0] < -
IKPI )
17576 { j3array[0]+=
IK2PI;
17579 for(
int ij3 = 0; ij3 < 1; ++ij3)
17581 if( !j3valid[ij3] )
17585 _ij3[0] = ij3; _ij3[1] = -1;
17586 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17590 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17593 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17595 IkReal evalcond[8];
17596 IkReal x1387=
IKsin(j3);
17597 IkReal x1388=
IKcos(j3);
17598 IkReal x1389=((1.0)*gconst22);
17599 IkReal x1390=(gconst23*x1387);
17600 IkReal x1391=((1.0)*x1388);
17601 IkReal x1392=(x1388*x1389);
17602 evalcond[0]=(((new_r10*x1387))+gconst23+((new_r00*x1388)));
17603 evalcond[1]=(((gconst22*x1387))+((gconst23*x1388))+new_r00);
17604 evalcond[2]=(gconst22+((new_r00*x1387))+(((-1.0)*new_r10*x1391)));
17605 evalcond[3]=(gconst23+((new_r01*x1387))+(((-1.0)*new_r11*x1391)));
17606 evalcond[4]=(x1390+new_r01+(((-1.0)*x1392)));
17607 evalcond[5]=(x1390+new_r10+(((-1.0)*x1392)));
17608 evalcond[6]=((((-1.0)*x1389))+((new_r11*x1387))+((new_r01*x1388)));
17609 evalcond[7]=((((-1.0)*gconst23*x1391))+new_r11+(((-1.0)*x1387*x1389)));
17617 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17618 vinfos[0].jointtype = 1;
17619 vinfos[0].foffset = j0;
17620 vinfos[0].indices[0] = _ij0[0];
17621 vinfos[0].indices[1] = _ij0[1];
17622 vinfos[0].maxsolutions = _nj0;
17623 vinfos[1].jointtype = 1;
17624 vinfos[1].foffset = j1;
17625 vinfos[1].indices[0] = _ij1[0];
17626 vinfos[1].indices[1] = _ij1[1];
17627 vinfos[1].maxsolutions = _nj1;
17628 vinfos[2].jointtype = 1;
17629 vinfos[2].foffset = j2;
17630 vinfos[2].indices[0] = _ij2[0];
17631 vinfos[2].indices[1] = _ij2[1];
17632 vinfos[2].maxsolutions = _nj2;
17633 vinfos[3].jointtype = 1;
17634 vinfos[3].foffset = j3;
17635 vinfos[3].indices[0] = _ij3[0];
17636 vinfos[3].indices[1] = _ij3[1];
17637 vinfos[3].maxsolutions = _nj3;
17638 vinfos[4].jointtype = 1;
17639 vinfos[4].foffset = j4;
17640 vinfos[4].indices[0] = _ij4[0];
17641 vinfos[4].indices[1] = _ij4[1];
17642 vinfos[4].maxsolutions = _nj4;
17643 vinfos[5].jointtype = 1;
17644 vinfos[5].foffset = j5;
17645 vinfos[5].indices[0] = _ij5[0];
17646 vinfos[5].indices[1] = _ij5[1];
17647 vinfos[5].maxsolutions = _nj5;
17648 std::vector<int> vfree(0);
17661 IkReal j3array[1], cj3array[1], sj3array[1];
17662 bool j3valid[1]={
false};
17664 IkReal x1393=((1.0)*new_r10);
17673 j3array[0]=((-1.5707963267949)+(x1394.
value)+(((1.5707963267949)*(x1395.
value))));
17674 sj3array[0]=
IKsin(j3array[0]);
17675 cj3array[0]=
IKcos(j3array[0]);
17676 if( j3array[0] >
IKPI )
17680 else if( j3array[0] < -
IKPI )
17681 { j3array[0]+=
IK2PI;
17684 for(
int ij3 = 0; ij3 < 1; ++ij3)
17686 if( !j3valid[ij3] )
17690 _ij3[0] = ij3; _ij3[1] = -1;
17691 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17695 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17698 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17700 IkReal evalcond[8];
17701 IkReal x1396=
IKsin(j3);
17702 IkReal x1397=
IKcos(j3);
17703 IkReal x1398=((1.0)*gconst22);
17704 IkReal x1399=(gconst23*x1396);
17705 IkReal x1400=((1.0)*x1397);
17706 IkReal x1401=(x1397*x1398);
17707 evalcond[0]=(((new_r10*x1396))+gconst23+((new_r00*x1397)));
17708 evalcond[1]=(((gconst22*x1396))+((gconst23*x1397))+new_r00);
17709 evalcond[2]=(gconst22+((new_r00*x1396))+(((-1.0)*new_r10*x1400)));
17710 evalcond[3]=(gconst23+((new_r01*x1396))+(((-1.0)*new_r11*x1400)));
17711 evalcond[4]=(x1399+(((-1.0)*x1401))+new_r01);
17712 evalcond[5]=(x1399+(((-1.0)*x1401))+new_r10);
17713 evalcond[6]=(((new_r11*x1396))+((new_r01*x1397))+(((-1.0)*x1398)));
17714 evalcond[7]=((((-1.0)*x1396*x1398))+(((-1.0)*gconst23*x1400))+new_r11);
17722 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17723 vinfos[0].jointtype = 1;
17724 vinfos[0].foffset = j0;
17725 vinfos[0].indices[0] = _ij0[0];
17726 vinfos[0].indices[1] = _ij0[1];
17727 vinfos[0].maxsolutions = _nj0;
17728 vinfos[1].jointtype = 1;
17729 vinfos[1].foffset = j1;
17730 vinfos[1].indices[0] = _ij1[0];
17731 vinfos[1].indices[1] = _ij1[1];
17732 vinfos[1].maxsolutions = _nj1;
17733 vinfos[2].jointtype = 1;
17734 vinfos[2].foffset = j2;
17735 vinfos[2].indices[0] = _ij2[0];
17736 vinfos[2].indices[1] = _ij2[1];
17737 vinfos[2].maxsolutions = _nj2;
17738 vinfos[3].jointtype = 1;
17739 vinfos[3].foffset = j3;
17740 vinfos[3].indices[0] = _ij3[0];
17741 vinfos[3].indices[1] = _ij3[1];
17742 vinfos[3].maxsolutions = _nj3;
17743 vinfos[4].jointtype = 1;
17744 vinfos[4].foffset = j4;
17745 vinfos[4].indices[0] = _ij4[0];
17746 vinfos[4].indices[1] = _ij4[1];
17747 vinfos[4].maxsolutions = _nj4;
17748 vinfos[5].jointtype = 1;
17749 vinfos[5].foffset = j5;
17750 vinfos[5].indices[0] = _ij5[0];
17751 vinfos[5].indices[1] = _ij5[1];
17752 vinfos[5].maxsolutions = _nj5;
17753 std::vector<int> vfree(0);
17765 if( bgotonextstatement )
17767 bool bgotonextstatement =
true;
17770 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17771 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
17773 bgotonextstatement=
false;
17775 IkReal j3array[1], cj3array[1], sj3array[1];
17776 bool j3valid[1]={
false};
17780 j3array[0]=
IKatan2(((-1.0)*new_r00), new_r01);
17781 sj3array[0]=
IKsin(j3array[0]);
17782 cj3array[0]=
IKcos(j3array[0]);
17783 if( j3array[0] >
IKPI )
17787 else if( j3array[0] < -
IKPI )
17788 { j3array[0]+=
IK2PI;
17791 for(
int ij3 = 0; ij3 < 1; ++ij3)
17793 if( !j3valid[ij3] )
17797 _ij3[0] = ij3; _ij3[1] = -1;
17798 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17802 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17805 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17807 IkReal evalcond[8];
17808 IkReal x1402=
IKsin(j3);
17809 IkReal x1403=
IKcos(j3);
17810 IkReal x1404=((1.0)*x1403);
17811 evalcond[0]=(x1402+new_r00);
17812 evalcond[1]=((((-1.0)*x1404))+new_r01);
17813 evalcond[2]=((((-1.0)*x1402))+new_r11);
17814 evalcond[3]=((((-1.0)*x1404))+new_r10);
17815 evalcond[4]=(((new_r10*x1402))+((new_r00*x1403)));
17816 evalcond[5]=(((new_r01*x1402))+(((-1.0)*new_r11*x1404)));
17817 evalcond[6]=((-1.0)+((new_r01*x1403))+((new_r11*x1402)));
17818 evalcond[7]=((1.0)+((new_r00*x1402))+(((-1.0)*new_r10*x1404)));
17826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17827 vinfos[0].jointtype = 1;
17828 vinfos[0].foffset = j0;
17829 vinfos[0].indices[0] = _ij0[0];
17830 vinfos[0].indices[1] = _ij0[1];
17831 vinfos[0].maxsolutions = _nj0;
17832 vinfos[1].jointtype = 1;
17833 vinfos[1].foffset = j1;
17834 vinfos[1].indices[0] = _ij1[0];
17835 vinfos[1].indices[1] = _ij1[1];
17836 vinfos[1].maxsolutions = _nj1;
17837 vinfos[2].jointtype = 1;
17838 vinfos[2].foffset = j2;
17839 vinfos[2].indices[0] = _ij2[0];
17840 vinfos[2].indices[1] = _ij2[1];
17841 vinfos[2].maxsolutions = _nj2;
17842 vinfos[3].jointtype = 1;
17843 vinfos[3].foffset = j3;
17844 vinfos[3].indices[0] = _ij3[0];
17845 vinfos[3].indices[1] = _ij3[1];
17846 vinfos[3].maxsolutions = _nj3;
17847 vinfos[4].jointtype = 1;
17848 vinfos[4].foffset = j4;
17849 vinfos[4].indices[0] = _ij4[0];
17850 vinfos[4].indices[1] = _ij4[1];
17851 vinfos[4].maxsolutions = _nj4;
17852 vinfos[5].jointtype = 1;
17853 vinfos[5].foffset = j5;
17854 vinfos[5].indices[0] = _ij5[0];
17855 vinfos[5].indices[1] = _ij5[1];
17856 vinfos[5].maxsolutions = _nj5;
17857 std::vector<int> vfree(0);
17865 if( bgotonextstatement )
17867 bool bgotonextstatement =
true;
17870 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17871 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
17873 bgotonextstatement=
false;
17875 IkReal j3array[1], cj3array[1], sj3array[1];
17876 bool j3valid[1]={
false};
17880 j3array[0]=
IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17881 sj3array[0]=
IKsin(j3array[0]);
17882 cj3array[0]=
IKcos(j3array[0]);
17883 if( j3array[0] >
IKPI )
17887 else if( j3array[0] < -
IKPI )
17888 { j3array[0]+=
IK2PI;
17891 for(
int ij3 = 0; ij3 < 1; ++ij3)
17893 if( !j3valid[ij3] )
17897 _ij3[0] = ij3; _ij3[1] = -1;
17898 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
17902 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
17905 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17907 IkReal evalcond[8];
17908 IkReal x1405=
IKcos(j3);
17909 IkReal x1406=
IKsin(j3);
17910 IkReal x1407=((1.0)*x1405);
17911 evalcond[0]=(x1405+new_r01);
17912 evalcond[1]=(x1406+new_r11);
17913 evalcond[2]=(x1405+new_r10);
17914 evalcond[3]=((((-1.0)*x1406))+new_r00);
17915 evalcond[4]=(((new_r10*x1406))+((new_r00*x1405)));
17916 evalcond[5]=(((new_r01*x1406))+(((-1.0)*new_r11*x1407)));
17917 evalcond[6]=((1.0)+((new_r01*x1405))+((new_r11*x1406)));
17918 evalcond[7]=((-1.0)+((new_r00*x1406))+(((-1.0)*new_r10*x1407)));
17926 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17927 vinfos[0].jointtype = 1;
17928 vinfos[0].foffset = j0;
17929 vinfos[0].indices[0] = _ij0[0];
17930 vinfos[0].indices[1] = _ij0[1];
17931 vinfos[0].maxsolutions = _nj0;
17932 vinfos[1].jointtype = 1;
17933 vinfos[1].foffset = j1;
17934 vinfos[1].indices[0] = _ij1[0];
17935 vinfos[1].indices[1] = _ij1[1];
17936 vinfos[1].maxsolutions = _nj1;
17937 vinfos[2].jointtype = 1;
17938 vinfos[2].foffset = j2;
17939 vinfos[2].indices[0] = _ij2[0];
17940 vinfos[2].indices[1] = _ij2[1];
17941 vinfos[2].maxsolutions = _nj2;
17942 vinfos[3].jointtype = 1;
17943 vinfos[3].foffset = j3;
17944 vinfos[3].indices[0] = _ij3[0];
17945 vinfos[3].indices[1] = _ij3[1];
17946 vinfos[3].maxsolutions = _nj3;
17947 vinfos[4].jointtype = 1;
17948 vinfos[4].foffset = j4;
17949 vinfos[4].indices[0] = _ij4[0];
17950 vinfos[4].indices[1] = _ij4[1];
17951 vinfos[4].maxsolutions = _nj4;
17952 vinfos[5].jointtype = 1;
17953 vinfos[5].foffset = j5;
17954 vinfos[5].indices[0] = _ij5[0];
17955 vinfos[5].indices[1] = _ij5[1];
17956 vinfos[5].maxsolutions = _nj5;
17957 std::vector<int> vfree(0);
17965 if( bgotonextstatement )
17967 bool bgotonextstatement =
true;
17970 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r00)));
17971 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
17973 bgotonextstatement=
false;
17978 j4=3.14159265358979;
17982 j3eval[1]=
IKsign(new_r10);
17984 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
17990 j4=3.14159265358979;
17994 j3eval[1]=
IKsign(new_r01);
17996 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
18002 j4=3.14159265358979;
18007 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
18014 IkReal j3array[1], cj3array[1], sj3array[1];
18015 bool j3valid[1]={
false};
18028 sj3array[0]=
IKsin(j3array[0]);
18029 cj3array[0]=
IKcos(j3array[0]);
18030 if( j3array[0] >
IKPI )
18034 else if( j3array[0] < -
IKPI )
18035 { j3array[0]+=
IK2PI;
18038 for(
int ij3 = 0; ij3 < 1; ++ij3)
18040 if( !j3valid[ij3] )
18044 _ij3[0] = ij3; _ij3[1] = -1;
18045 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18049 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18052 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18054 IkReal evalcond[7];
18055 IkReal x1410=
IKsin(j3);
18056 IkReal x1411=
IKcos(j3);
18057 IkReal x1412=((1.0)*sj5);
18058 IkReal x1413=(cj5*x1410);
18059 IkReal x1414=(x1411*x1412);
18060 evalcond[0]=(cj5+((new_r01*x1410)));
18061 evalcond[1]=(cj5+((new_r10*x1410)));
18062 evalcond[2]=(sj5+(((-1.0)*new_r10*x1411)));
18063 evalcond[3]=(((new_r01*x1411))+(((-1.0)*x1412)));
18064 evalcond[4]=(((sj5*x1410))+((cj5*x1411)));
18065 evalcond[5]=((((-1.0)*x1414))+x1413+new_r01);
18066 evalcond[6]=((((-1.0)*x1414))+x1413+new_r10);
18074 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18075 vinfos[0].jointtype = 1;
18076 vinfos[0].foffset = j0;
18077 vinfos[0].indices[0] = _ij0[0];
18078 vinfos[0].indices[1] = _ij0[1];
18079 vinfos[0].maxsolutions = _nj0;
18080 vinfos[1].jointtype = 1;
18081 vinfos[1].foffset = j1;
18082 vinfos[1].indices[0] = _ij1[0];
18083 vinfos[1].indices[1] = _ij1[1];
18084 vinfos[1].maxsolutions = _nj1;
18085 vinfos[2].jointtype = 1;
18086 vinfos[2].foffset = j2;
18087 vinfos[2].indices[0] = _ij2[0];
18088 vinfos[2].indices[1] = _ij2[1];
18089 vinfos[2].maxsolutions = _nj2;
18090 vinfos[3].jointtype = 1;
18091 vinfos[3].foffset = j3;
18092 vinfos[3].indices[0] = _ij3[0];
18093 vinfos[3].indices[1] = _ij3[1];
18094 vinfos[3].maxsolutions = _nj3;
18095 vinfos[4].jointtype = 1;
18096 vinfos[4].foffset = j4;
18097 vinfos[4].indices[0] = _ij4[0];
18098 vinfos[4].indices[1] = _ij4[1];
18099 vinfos[4].maxsolutions = _nj4;
18100 vinfos[5].jointtype = 1;
18101 vinfos[5].foffset = j5;
18102 vinfos[5].indices[0] = _ij5[0];
18103 vinfos[5].indices[1] = _ij5[1];
18104 vinfos[5].maxsolutions = _nj5;
18105 std::vector<int> vfree(0);
18118 IkReal j3array[1], cj3array[1], sj3array[1];
18119 bool j3valid[1]={
false};
18129 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1415.
value)))+(x1416.
value));
18130 sj3array[0]=
IKsin(j3array[0]);
18131 cj3array[0]=
IKcos(j3array[0]);
18132 if( j3array[0] >
IKPI )
18136 else if( j3array[0] < -
IKPI )
18137 { j3array[0]+=
IK2PI;
18140 for(
int ij3 = 0; ij3 < 1; ++ij3)
18142 if( !j3valid[ij3] )
18146 _ij3[0] = ij3; _ij3[1] = -1;
18147 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18151 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18154 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18156 IkReal evalcond[7];
18157 IkReal x1417=
IKsin(j3);
18158 IkReal x1418=
IKcos(j3);
18159 IkReal x1419=((1.0)*sj5);
18160 IkReal x1420=(cj5*x1417);
18161 IkReal x1421=(x1418*x1419);
18162 evalcond[0]=(cj5+((new_r01*x1417)));
18163 evalcond[1]=(cj5+((new_r10*x1417)));
18164 evalcond[2]=(sj5+(((-1.0)*new_r10*x1418)));
18165 evalcond[3]=(((new_r01*x1418))+(((-1.0)*x1419)));
18166 evalcond[4]=(((sj5*x1417))+((cj5*x1418)));
18167 evalcond[5]=(x1420+new_r01+(((-1.0)*x1421)));
18168 evalcond[6]=(x1420+new_r10+(((-1.0)*x1421)));
18176 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18177 vinfos[0].jointtype = 1;
18178 vinfos[0].foffset = j0;
18179 vinfos[0].indices[0] = _ij0[0];
18180 vinfos[0].indices[1] = _ij0[1];
18181 vinfos[0].maxsolutions = _nj0;
18182 vinfos[1].jointtype = 1;
18183 vinfos[1].foffset = j1;
18184 vinfos[1].indices[0] = _ij1[0];
18185 vinfos[1].indices[1] = _ij1[1];
18186 vinfos[1].maxsolutions = _nj1;
18187 vinfos[2].jointtype = 1;
18188 vinfos[2].foffset = j2;
18189 vinfos[2].indices[0] = _ij2[0];
18190 vinfos[2].indices[1] = _ij2[1];
18191 vinfos[2].maxsolutions = _nj2;
18192 vinfos[3].jointtype = 1;
18193 vinfos[3].foffset = j3;
18194 vinfos[3].indices[0] = _ij3[0];
18195 vinfos[3].indices[1] = _ij3[1];
18196 vinfos[3].maxsolutions = _nj3;
18197 vinfos[4].jointtype = 1;
18198 vinfos[4].foffset = j4;
18199 vinfos[4].indices[0] = _ij4[0];
18200 vinfos[4].indices[1] = _ij4[1];
18201 vinfos[4].maxsolutions = _nj4;
18202 vinfos[5].jointtype = 1;
18203 vinfos[5].foffset = j5;
18204 vinfos[5].indices[0] = _ij5[0];
18205 vinfos[5].indices[1] = _ij5[1];
18206 vinfos[5].maxsolutions = _nj5;
18207 std::vector<int> vfree(0);
18220 IkReal j3array[1], cj3array[1], sj3array[1];
18221 bool j3valid[1]={
false};
18231 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1422.
value)))+(x1423.
value));
18232 sj3array[0]=
IKsin(j3array[0]);
18233 cj3array[0]=
IKcos(j3array[0]);
18234 if( j3array[0] >
IKPI )
18238 else if( j3array[0] < -
IKPI )
18239 { j3array[0]+=
IK2PI;
18242 for(
int ij3 = 0; ij3 < 1; ++ij3)
18244 if( !j3valid[ij3] )
18248 _ij3[0] = ij3; _ij3[1] = -1;
18249 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18253 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18258 IkReal evalcond[7];
18259 IkReal x1424=
IKsin(j3);
18260 IkReal x1425=
IKcos(j3);
18261 IkReal x1426=((1.0)*sj5);
18262 IkReal x1427=(cj5*x1424);
18263 IkReal x1428=(x1425*x1426);
18264 evalcond[0]=(cj5+((new_r01*x1424)));
18265 evalcond[1]=(cj5+((new_r10*x1424)));
18266 evalcond[2]=(sj5+(((-1.0)*new_r10*x1425)));
18267 evalcond[3]=(((new_r01*x1425))+(((-1.0)*x1426)));
18268 evalcond[4]=(((sj5*x1424))+((cj5*x1425)));
18269 evalcond[5]=(x1427+new_r01+(((-1.0)*x1428)));
18270 evalcond[6]=(x1427+new_r10+(((-1.0)*x1428)));
18278 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18279 vinfos[0].jointtype = 1;
18280 vinfos[0].foffset = j0;
18281 vinfos[0].indices[0] = _ij0[0];
18282 vinfos[0].indices[1] = _ij0[1];
18283 vinfos[0].maxsolutions = _nj0;
18284 vinfos[1].jointtype = 1;
18285 vinfos[1].foffset = j1;
18286 vinfos[1].indices[0] = _ij1[0];
18287 vinfos[1].indices[1] = _ij1[1];
18288 vinfos[1].maxsolutions = _nj1;
18289 vinfos[2].jointtype = 1;
18290 vinfos[2].foffset = j2;
18291 vinfos[2].indices[0] = _ij2[0];
18292 vinfos[2].indices[1] = _ij2[1];
18293 vinfos[2].maxsolutions = _nj2;
18294 vinfos[3].jointtype = 1;
18295 vinfos[3].foffset = j3;
18296 vinfos[3].indices[0] = _ij3[0];
18297 vinfos[3].indices[1] = _ij3[1];
18298 vinfos[3].maxsolutions = _nj3;
18299 vinfos[4].jointtype = 1;
18300 vinfos[4].foffset = j4;
18301 vinfos[4].indices[0] = _ij4[0];
18302 vinfos[4].indices[1] = _ij4[1];
18303 vinfos[4].maxsolutions = _nj4;
18304 vinfos[5].jointtype = 1;
18305 vinfos[5].foffset = j5;
18306 vinfos[5].indices[0] = _ij5[0];
18307 vinfos[5].indices[1] = _ij5[1];
18308 vinfos[5].maxsolutions = _nj5;
18309 std::vector<int> vfree(0);
18321 if( bgotonextstatement )
18323 bool bgotonextstatement =
true;
18326 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
18327 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
18329 bgotonextstatement=
false;
18334 j4=3.14159265358979;
18339 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
18340 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
18347 IkReal j3array[2], cj3array[2], sj3array[2];
18348 bool j3valid[2]={
false};
18354 IkReal x1429=x1430.
value;
18355 j3array[0]=((-1.0)*x1429);
18356 sj3array[0]=
IKsin(j3array[0]);
18357 cj3array[0]=
IKcos(j3array[0]);
18358 j3array[1]=((3.14159265358979)+(((-1.0)*x1429)));
18359 sj3array[1]=
IKsin(j3array[1]);
18360 cj3array[1]=
IKcos(j3array[1]);
18361 if( j3array[0] >
IKPI )
18365 else if( j3array[0] < -
IKPI )
18366 { j3array[0]+=
IK2PI;
18369 if( j3array[1] >
IKPI )
18373 else if( j3array[1] < -
IKPI )
18374 { j3array[1]+=
IK2PI;
18377 for(
int ij3 = 0; ij3 < 2; ++ij3)
18379 if( !j3valid[ij3] )
18383 _ij3[0] = ij3; _ij3[1] = -1;
18384 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
18388 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18391 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18393 IkReal evalcond[1];
18394 evalcond[0]=(((new_r00*(
IKsin(j3))))+(((-1.0)*new_r10*(
IKcos(j3)))));
18402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18403 vinfos[0].jointtype = 1;
18404 vinfos[0].foffset = j0;
18405 vinfos[0].indices[0] = _ij0[0];
18406 vinfos[0].indices[1] = _ij0[1];
18407 vinfos[0].maxsolutions = _nj0;
18408 vinfos[1].jointtype = 1;
18409 vinfos[1].foffset = j1;
18410 vinfos[1].indices[0] = _ij1[0];
18411 vinfos[1].indices[1] = _ij1[1];
18412 vinfos[1].maxsolutions = _nj1;
18413 vinfos[2].jointtype = 1;
18414 vinfos[2].foffset = j2;
18415 vinfos[2].indices[0] = _ij2[0];
18416 vinfos[2].indices[1] = _ij2[1];
18417 vinfos[2].maxsolutions = _nj2;
18418 vinfos[3].jointtype = 1;
18419 vinfos[3].foffset = j3;
18420 vinfos[3].indices[0] = _ij3[0];
18421 vinfos[3].indices[1] = _ij3[1];
18422 vinfos[3].maxsolutions = _nj3;
18423 vinfos[4].jointtype = 1;
18424 vinfos[4].foffset = j4;
18425 vinfos[4].indices[0] = _ij4[0];
18426 vinfos[4].indices[1] = _ij4[1];
18427 vinfos[4].maxsolutions = _nj4;
18428 vinfos[5].jointtype = 1;
18429 vinfos[5].foffset = j5;
18430 vinfos[5].indices[0] = _ij5[0];
18431 vinfos[5].indices[1] = _ij5[1];
18432 vinfos[5].maxsolutions = _nj5;
18433 std::vector<int> vfree(0);
18445 if( bgotonextstatement )
18447 bool bgotonextstatement =
true;
18450 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
18451 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
18453 bgotonextstatement=
false;
18458 j4=3.14159265358979;
18463 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
18464 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
18471 IkReal j3array[2], cj3array[2], sj3array[2];
18472 bool j3valid[2]={
false};
18478 IkReal x1431=x1432.
value;
18479 j3array[0]=((-1.0)*x1431);
18480 sj3array[0]=
IKsin(j3array[0]);
18481 cj3array[0]=
IKcos(j3array[0]);
18482 j3array[1]=((3.14159265358979)+(((-1.0)*x1431)));
18483 sj3array[1]=
IKsin(j3array[1]);
18484 cj3array[1]=
IKcos(j3array[1]);
18485 if( j3array[0] >
IKPI )
18489 else if( j3array[0] < -
IKPI )
18490 { j3array[0]+=
IK2PI;
18493 if( j3array[1] >
IKPI )
18497 else if( j3array[1] < -
IKPI )
18498 { j3array[1]+=
IK2PI;
18501 for(
int ij3 = 0; ij3 < 2; ++ij3)
18503 if( !j3valid[ij3] )
18507 _ij3[0] = ij3; _ij3[1] = -1;
18508 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
18512 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18515 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18517 IkReal evalcond[1];
18518 evalcond[0]=(((new_r01*(
IKsin(j3))))+(((-1.0)*new_r11*(
IKcos(j3)))));
18526 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18527 vinfos[0].jointtype = 1;
18528 vinfos[0].foffset = j0;
18529 vinfos[0].indices[0] = _ij0[0];
18530 vinfos[0].indices[1] = _ij0[1];
18531 vinfos[0].maxsolutions = _nj0;
18532 vinfos[1].jointtype = 1;
18533 vinfos[1].foffset = j1;
18534 vinfos[1].indices[0] = _ij1[0];
18535 vinfos[1].indices[1] = _ij1[1];
18536 vinfos[1].maxsolutions = _nj1;
18537 vinfos[2].jointtype = 1;
18538 vinfos[2].foffset = j2;
18539 vinfos[2].indices[0] = _ij2[0];
18540 vinfos[2].indices[1] = _ij2[1];
18541 vinfos[2].maxsolutions = _nj2;
18542 vinfos[3].jointtype = 1;
18543 vinfos[3].foffset = j3;
18544 vinfos[3].indices[0] = _ij3[0];
18545 vinfos[3].indices[1] = _ij3[1];
18546 vinfos[3].maxsolutions = _nj3;
18547 vinfos[4].jointtype = 1;
18548 vinfos[4].foffset = j4;
18549 vinfos[4].indices[0] = _ij4[0];
18550 vinfos[4].indices[1] = _ij4[1];
18551 vinfos[4].maxsolutions = _nj4;
18552 vinfos[5].jointtype = 1;
18553 vinfos[5].foffset = j5;
18554 vinfos[5].indices[0] = _ij5[0];
18555 vinfos[5].indices[1] = _ij5[1];
18556 vinfos[5].maxsolutions = _nj5;
18557 std::vector<int> vfree(0);
18569 if( bgotonextstatement )
18571 bool bgotonextstatement =
true;
18574 evalcond[0]=((
IKabs(new_r10))+(
IKabs(new_r01)));
18575 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
18577 bgotonextstatement=
false;
18582 j4=3.14159265358979;
18586 j3eval[1]=
IKsign(new_r00);
18588 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 ||
IKabs(j3eval[2]) < 0.0000010000000000 )
18594 j4=3.14159265358979;
18599 if(
IKabs(j3eval[0]) < 0.0000010000000000 ||
IKabs(j3eval[1]) < 0.0000010000000000 )
18606 IkReal j3array[1], cj3array[1], sj3array[1];
18607 bool j3valid[1]={
false};
18620 sj3array[0]=
IKsin(j3array[0]);
18621 cj3array[0]=
IKcos(j3array[0]);
18622 if( j3array[0] >
IKPI )
18626 else if( j3array[0] < -
IKPI )
18627 { j3array[0]+=
IK2PI;
18630 for(
int ij3 = 0; ij3 < 1; ++ij3)
18632 if( !j3valid[ij3] )
18636 _ij3[0] = ij3; _ij3[1] = -1;
18637 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18641 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18644 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18646 IkReal evalcond[7];
18647 IkReal x1435=
IKcos(j3);
18648 IkReal x1436=
IKsin(j3);
18649 IkReal x1437=((1.0)*sj5);
18650 IkReal x1438=(cj5*x1435);
18651 IkReal x1439=((1.0)*x1435);
18652 evalcond[0]=(sj5+((new_r00*x1436)));
18653 evalcond[1]=(cj5+((new_r00*x1435)));
18654 evalcond[2]=(cj5+(((-1.0)*new_r11*x1439)));
18655 evalcond[3]=(((new_r11*x1436))+(((-1.0)*x1437)));
18656 evalcond[4]=((((-1.0)*x1435*x1437))+((cj5*x1436)));
18657 evalcond[5]=(((sj5*x1436))+x1438+new_r00);
18658 evalcond[6]=((((-1.0)*x1436*x1437))+(((-1.0)*x1438))+new_r11);
18666 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18667 vinfos[0].jointtype = 1;
18668 vinfos[0].foffset = j0;
18669 vinfos[0].indices[0] = _ij0[0];
18670 vinfos[0].indices[1] = _ij0[1];
18671 vinfos[0].maxsolutions = _nj0;
18672 vinfos[1].jointtype = 1;
18673 vinfos[1].foffset = j1;
18674 vinfos[1].indices[0] = _ij1[0];
18675 vinfos[1].indices[1] = _ij1[1];
18676 vinfos[1].maxsolutions = _nj1;
18677 vinfos[2].jointtype = 1;
18678 vinfos[2].foffset = j2;
18679 vinfos[2].indices[0] = _ij2[0];
18680 vinfos[2].indices[1] = _ij2[1];
18681 vinfos[2].maxsolutions = _nj2;
18682 vinfos[3].jointtype = 1;
18683 vinfos[3].foffset = j3;
18684 vinfos[3].indices[0] = _ij3[0];
18685 vinfos[3].indices[1] = _ij3[1];
18686 vinfos[3].maxsolutions = _nj3;
18687 vinfos[4].jointtype = 1;
18688 vinfos[4].foffset = j4;
18689 vinfos[4].indices[0] = _ij4[0];
18690 vinfos[4].indices[1] = _ij4[1];
18691 vinfos[4].maxsolutions = _nj4;
18692 vinfos[5].jointtype = 1;
18693 vinfos[5].foffset = j5;
18694 vinfos[5].indices[0] = _ij5[0];
18695 vinfos[5].indices[1] = _ij5[1];
18696 vinfos[5].maxsolutions = _nj5;
18697 std::vector<int> vfree(0);
18710 IkReal j3array[1], cj3array[1], sj3array[1];
18711 bool j3valid[1]={
false};
18721 j3array[0]=((-1.5707963267949)+(x1440.
value)+(((1.5707963267949)*(x1441.
value))));
18722 sj3array[0]=
IKsin(j3array[0]);
18723 cj3array[0]=
IKcos(j3array[0]);
18724 if( j3array[0] >
IKPI )
18728 else if( j3array[0] < -
IKPI )
18729 { j3array[0]+=
IK2PI;
18732 for(
int ij3 = 0; ij3 < 1; ++ij3)
18734 if( !j3valid[ij3] )
18738 _ij3[0] = ij3; _ij3[1] = -1;
18739 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18743 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18746 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18748 IkReal evalcond[7];
18749 IkReal x1442=
IKcos(j3);
18750 IkReal x1443=
IKsin(j3);
18751 IkReal x1444=((1.0)*sj5);
18752 IkReal x1445=(cj5*x1442);
18753 IkReal x1446=((1.0)*x1442);
18754 evalcond[0]=(sj5+((new_r00*x1443)));
18755 evalcond[1]=(cj5+((new_r00*x1442)));
18756 evalcond[2]=(cj5+(((-1.0)*new_r11*x1446)));
18757 evalcond[3]=(((new_r11*x1443))+(((-1.0)*x1444)));
18758 evalcond[4]=((((-1.0)*x1442*x1444))+((cj5*x1443)));
18759 evalcond[5]=(((sj5*x1443))+x1445+new_r00);
18760 evalcond[6]=((((-1.0)*x1445))+new_r11+(((-1.0)*x1443*x1444)));
18768 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18769 vinfos[0].jointtype = 1;
18770 vinfos[0].foffset = j0;
18771 vinfos[0].indices[0] = _ij0[0];
18772 vinfos[0].indices[1] = _ij0[1];
18773 vinfos[0].maxsolutions = _nj0;
18774 vinfos[1].jointtype = 1;
18775 vinfos[1].foffset = j1;
18776 vinfos[1].indices[0] = _ij1[0];
18777 vinfos[1].indices[1] = _ij1[1];
18778 vinfos[1].maxsolutions = _nj1;
18779 vinfos[2].jointtype = 1;
18780 vinfos[2].foffset = j2;
18781 vinfos[2].indices[0] = _ij2[0];
18782 vinfos[2].indices[1] = _ij2[1];
18783 vinfos[2].maxsolutions = _nj2;
18784 vinfos[3].jointtype = 1;
18785 vinfos[3].foffset = j3;
18786 vinfos[3].indices[0] = _ij3[0];
18787 vinfos[3].indices[1] = _ij3[1];
18788 vinfos[3].maxsolutions = _nj3;
18789 vinfos[4].jointtype = 1;
18790 vinfos[4].foffset = j4;
18791 vinfos[4].indices[0] = _ij4[0];
18792 vinfos[4].indices[1] = _ij4[1];
18793 vinfos[4].maxsolutions = _nj4;
18794 vinfos[5].jointtype = 1;
18795 vinfos[5].foffset = j5;
18796 vinfos[5].indices[0] = _ij5[0];
18797 vinfos[5].indices[1] = _ij5[1];
18798 vinfos[5].maxsolutions = _nj5;
18799 std::vector<int> vfree(0);
18811 if( bgotonextstatement )
18813 bool bgotonextstatement =
true;
18818 bgotonextstatement=
false;
18823 if( bgotonextstatement )
18841 IkReal j3array[1], cj3array[1], sj3array[1];
18842 bool j3valid[1]={
false};
18844 IkReal x1447=((1.0)*new_r00);
18853 j3array[0]=((-1.5707963267949)+(x1448.
value)+(((1.5707963267949)*(x1449.
value))));
18854 sj3array[0]=
IKsin(j3array[0]);
18855 cj3array[0]=
IKcos(j3array[0]);
18856 if( j3array[0] >
IKPI )
18860 else if( j3array[0] < -
IKPI )
18861 { j3array[0]+=
IK2PI;
18864 for(
int ij3 = 0; ij3 < 1; ++ij3)
18866 if( !j3valid[ij3] )
18870 _ij3[0] = ij3; _ij3[1] = -1;
18871 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18875 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18878 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18880 IkReal evalcond[8];
18881 IkReal x1450=
IKsin(j3);
18882 IkReal x1451=
IKcos(j3);
18883 IkReal x1452=((1.0)*sj5);
18884 IkReal x1453=(cj5*x1450);
18885 IkReal x1454=((1.0)*x1451);
18886 IkReal x1455=(x1451*x1452);
18887 evalcond[0]=(cj5+((new_r00*x1451))+((new_r10*x1450)));
18888 evalcond[1]=(((cj5*x1451))+((sj5*x1450))+new_r00);
18889 evalcond[2]=((((-1.0)*new_r10*x1454))+sj5+((new_r00*x1450)));
18890 evalcond[3]=(cj5+((new_r01*x1450))+(((-1.0)*new_r11*x1454)));
18891 evalcond[4]=((((-1.0)*x1455))+x1453+new_r01);
18892 evalcond[5]=((((-1.0)*x1455))+x1453+new_r10);
18893 evalcond[6]=((((-1.0)*x1452))+((new_r01*x1451))+((new_r11*x1450)));
18894 evalcond[7]=((((-1.0)*cj5*x1454))+new_r11+(((-1.0)*x1450*x1452)));
18902 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18903 vinfos[0].jointtype = 1;
18904 vinfos[0].foffset = j0;
18905 vinfos[0].indices[0] = _ij0[0];
18906 vinfos[0].indices[1] = _ij0[1];
18907 vinfos[0].maxsolutions = _nj0;
18908 vinfos[1].jointtype = 1;
18909 vinfos[1].foffset = j1;
18910 vinfos[1].indices[0] = _ij1[0];
18911 vinfos[1].indices[1] = _ij1[1];
18912 vinfos[1].maxsolutions = _nj1;
18913 vinfos[2].jointtype = 1;
18914 vinfos[2].foffset = j2;
18915 vinfos[2].indices[0] = _ij2[0];
18916 vinfos[2].indices[1] = _ij2[1];
18917 vinfos[2].maxsolutions = _nj2;
18918 vinfos[3].jointtype = 1;
18919 vinfos[3].foffset = j3;
18920 vinfos[3].indices[0] = _ij3[0];
18921 vinfos[3].indices[1] = _ij3[1];
18922 vinfos[3].maxsolutions = _nj3;
18923 vinfos[4].jointtype = 1;
18924 vinfos[4].foffset = j4;
18925 vinfos[4].indices[0] = _ij4[0];
18926 vinfos[4].indices[1] = _ij4[1];
18927 vinfos[4].maxsolutions = _nj4;
18928 vinfos[5].jointtype = 1;
18929 vinfos[5].foffset = j5;
18930 vinfos[5].indices[0] = _ij5[0];
18931 vinfos[5].indices[1] = _ij5[1];
18932 vinfos[5].maxsolutions = _nj5;
18933 std::vector<int> vfree(0);
18946 IkReal j3array[1], cj3array[1], sj3array[1];
18947 bool j3valid[1]={
false};
18949 IkReal x1456=((1.0)*new_r10);
18958 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1457.
value)))+(x1458.
value));
18959 sj3array[0]=
IKsin(j3array[0]);
18960 cj3array[0]=
IKcos(j3array[0]);
18961 if( j3array[0] >
IKPI )
18965 else if( j3array[0] < -
IKPI )
18966 { j3array[0]+=
IK2PI;
18969 for(
int ij3 = 0; ij3 < 1; ++ij3)
18971 if( !j3valid[ij3] )
18975 _ij3[0] = ij3; _ij3[1] = -1;
18976 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
18980 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
18983 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18985 IkReal evalcond[8];
18986 IkReal x1459=
IKsin(j3);
18987 IkReal x1460=
IKcos(j3);
18988 IkReal x1461=((1.0)*sj5);
18989 IkReal x1462=(cj5*x1459);
18990 IkReal x1463=((1.0)*x1460);
18991 IkReal x1464=(x1460*x1461);
18992 evalcond[0]=(((new_r00*x1460))+cj5+((new_r10*x1459)));
18993 evalcond[1]=(((sj5*x1459))+((cj5*x1460))+new_r00);
18994 evalcond[2]=((((-1.0)*new_r10*x1463))+sj5+((new_r00*x1459)));
18995 evalcond[3]=(cj5+((new_r01*x1459))+(((-1.0)*new_r11*x1463)));
18996 evalcond[4]=((((-1.0)*x1464))+x1462+new_r01);
18997 evalcond[5]=((((-1.0)*x1464))+x1462+new_r10);
18998 evalcond[6]=(((new_r01*x1460))+(((-1.0)*x1461))+((new_r11*x1459)));
18999 evalcond[7]=((((-1.0)*cj5*x1463))+(((-1.0)*x1459*x1461))+new_r11);
19007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19008 vinfos[0].jointtype = 1;
19009 vinfos[0].foffset = j0;
19010 vinfos[0].indices[0] = _ij0[0];
19011 vinfos[0].indices[1] = _ij0[1];
19012 vinfos[0].maxsolutions = _nj0;
19013 vinfos[1].jointtype = 1;
19014 vinfos[1].foffset = j1;
19015 vinfos[1].indices[0] = _ij1[0];
19016 vinfos[1].indices[1] = _ij1[1];
19017 vinfos[1].maxsolutions = _nj1;
19018 vinfos[2].jointtype = 1;
19019 vinfos[2].foffset = j2;
19020 vinfos[2].indices[0] = _ij2[0];
19021 vinfos[2].indices[1] = _ij2[1];
19022 vinfos[2].maxsolutions = _nj2;
19023 vinfos[3].jointtype = 1;
19024 vinfos[3].foffset = j3;
19025 vinfos[3].indices[0] = _ij3[0];
19026 vinfos[3].indices[1] = _ij3[1];
19027 vinfos[3].maxsolutions = _nj3;
19028 vinfos[4].jointtype = 1;
19029 vinfos[4].foffset = j4;
19030 vinfos[4].indices[0] = _ij4[0];
19031 vinfos[4].indices[1] = _ij4[1];
19032 vinfos[4].maxsolutions = _nj4;
19033 vinfos[5].jointtype = 1;
19034 vinfos[5].foffset = j5;
19035 vinfos[5].indices[0] = _ij5[0];
19036 vinfos[5].indices[1] = _ij5[1];
19037 vinfos[5].maxsolutions = _nj5;
19038 std::vector<int> vfree(0);
19051 IkReal j3array[1], cj3array[1], sj3array[1];
19052 bool j3valid[1]={
false};
19054 IkReal x1465=((1.0)*new_r10);
19063 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1466.
value)))+(x1467.
value));
19064 sj3array[0]=
IKsin(j3array[0]);
19065 cj3array[0]=
IKcos(j3array[0]);
19066 if( j3array[0] >
IKPI )
19070 else if( j3array[0] < -
IKPI )
19071 { j3array[0]+=
IK2PI;
19074 for(
int ij3 = 0; ij3 < 1; ++ij3)
19076 if( !j3valid[ij3] )
19080 _ij3[0] = ij3; _ij3[1] = -1;
19081 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
19085 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19088 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19090 IkReal evalcond[8];
19091 IkReal x1468=
IKsin(j3);
19092 IkReal x1469=
IKcos(j3);
19093 IkReal x1470=((1.0)*sj5);
19094 IkReal x1471=(cj5*x1468);
19095 IkReal x1472=((1.0)*x1469);
19096 IkReal x1473=(x1469*x1470);
19097 evalcond[0]=(((new_r00*x1469))+cj5+((new_r10*x1468)));
19098 evalcond[1]=(((cj5*x1469))+new_r00+((sj5*x1468)));
19099 evalcond[2]=(((new_r00*x1468))+sj5+(((-1.0)*new_r10*x1472)));
19100 evalcond[3]=(((new_r01*x1468))+cj5+(((-1.0)*new_r11*x1472)));
19101 evalcond[4]=(x1471+(((-1.0)*x1473))+new_r01);
19102 evalcond[5]=(x1471+(((-1.0)*x1473))+new_r10);
19103 evalcond[6]=(((new_r01*x1469))+((new_r11*x1468))+(((-1.0)*x1470)));
19104 evalcond[7]=((((-1.0)*cj5*x1472))+(((-1.0)*x1468*x1470))+new_r11);
19112 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19113 vinfos[0].jointtype = 1;
19114 vinfos[0].foffset = j0;
19115 vinfos[0].indices[0] = _ij0[0];
19116 vinfos[0].indices[1] = _ij0[1];
19117 vinfos[0].maxsolutions = _nj0;
19118 vinfos[1].jointtype = 1;
19119 vinfos[1].foffset = j1;
19120 vinfos[1].indices[0] = _ij1[0];
19121 vinfos[1].indices[1] = _ij1[1];
19122 vinfos[1].maxsolutions = _nj1;
19123 vinfos[2].jointtype = 1;
19124 vinfos[2].foffset = j2;
19125 vinfos[2].indices[0] = _ij2[0];
19126 vinfos[2].indices[1] = _ij2[1];
19127 vinfos[2].maxsolutions = _nj2;
19128 vinfos[3].jointtype = 1;
19129 vinfos[3].foffset = j3;
19130 vinfos[3].indices[0] = _ij3[0];
19131 vinfos[3].indices[1] = _ij3[1];
19132 vinfos[3].maxsolutions = _nj3;
19133 vinfos[4].jointtype = 1;
19134 vinfos[4].foffset = j4;
19135 vinfos[4].indices[0] = _ij4[0];
19136 vinfos[4].indices[1] = _ij4[1];
19137 vinfos[4].maxsolutions = _nj4;
19138 vinfos[5].jointtype = 1;
19139 vinfos[5].foffset = j5;
19140 vinfos[5].indices[0] = _ij5[0];
19141 vinfos[5].indices[1] = _ij5[1];
19142 vinfos[5].maxsolutions = _nj5;
19143 std::vector<int> vfree(0);
19155 if( bgotonextstatement )
19157 bool bgotonextstatement =
true;
19160 evalcond[0]=((
IKabs(new_r12))+(
IKabs(new_r02)));
19161 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
19163 bgotonextstatement=
false;
19170 j3eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
19171 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
19179 j3eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
19180 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
19188 j3eval[0]=((
IKabs((new_r11*new_r22)))+(
IKabs((new_r01*new_r22))));
19189 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
19196 IkReal j3array[2], cj3array[2], sj3array[2];
19197 bool j3valid[2]={
false};
19203 IkReal x1474=x1475.
value;
19204 j3array[0]=((-1.0)*x1474);
19205 sj3array[0]=
IKsin(j3array[0]);
19206 cj3array[0]=
IKcos(j3array[0]);
19207 j3array[1]=((3.14159265358979)+(((-1.0)*x1474)));
19208 sj3array[1]=
IKsin(j3array[1]);
19209 cj3array[1]=
IKcos(j3array[1]);
19210 if( j3array[0] >
IKPI )
19214 else if( j3array[0] < -
IKPI )
19215 { j3array[0]+=
IK2PI;
19218 if( j3array[1] >
IKPI )
19222 else if( j3array[1] < -
IKPI )
19223 { j3array[1]+=
IK2PI;
19226 for(
int ij3 = 0; ij3 < 2; ++ij3)
19228 if( !j3valid[ij3] )
19232 _ij3[0] = ij3; _ij3[1] = -1;
19233 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
19237 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19240 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19242 IkReal evalcond[5];
19243 IkReal x1476=
IKsin(j3);
19244 IkReal x1477=
IKcos(j3);
19245 IkReal x1478=(new_r00*x1477);
19246 IkReal x1479=((1.0)*x1477);
19247 IkReal x1480=(new_r10*x1476);
19248 evalcond[0]=(((new_r11*x1476))+((new_r01*x1477)));
19249 evalcond[1]=(x1478+x1480);
19250 evalcond[2]=(((new_r00*x1476))+(((-1.0)*new_r10*x1479)));
19251 evalcond[3]=((((-1.0)*new_r11*x1479))+((new_r01*x1476)));
19252 evalcond[4]=(((new_r22*x1480))+((new_r22*x1478)));
19260 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19261 vinfos[0].jointtype = 1;
19262 vinfos[0].foffset = j0;
19263 vinfos[0].indices[0] = _ij0[0];
19264 vinfos[0].indices[1] = _ij0[1];
19265 vinfos[0].maxsolutions = _nj0;
19266 vinfos[1].jointtype = 1;
19267 vinfos[1].foffset = j1;
19268 vinfos[1].indices[0] = _ij1[0];
19269 vinfos[1].indices[1] = _ij1[1];
19270 vinfos[1].maxsolutions = _nj1;
19271 vinfos[2].jointtype = 1;
19272 vinfos[2].foffset = j2;
19273 vinfos[2].indices[0] = _ij2[0];
19274 vinfos[2].indices[1] = _ij2[1];
19275 vinfos[2].maxsolutions = _nj2;
19276 vinfos[3].jointtype = 1;
19277 vinfos[3].foffset = j3;
19278 vinfos[3].indices[0] = _ij3[0];
19279 vinfos[3].indices[1] = _ij3[1];
19280 vinfos[3].maxsolutions = _nj3;
19281 vinfos[4].jointtype = 1;
19282 vinfos[4].foffset = j4;
19283 vinfos[4].indices[0] = _ij4[0];
19284 vinfos[4].indices[1] = _ij4[1];
19285 vinfos[4].maxsolutions = _nj4;
19286 vinfos[5].jointtype = 1;
19287 vinfos[5].foffset = j5;
19288 vinfos[5].indices[0] = _ij5[0];
19289 vinfos[5].indices[1] = _ij5[1];
19290 vinfos[5].maxsolutions = _nj5;
19291 std::vector<int> vfree(0);
19304 IkReal j3array[2], cj3array[2], sj3array[2];
19305 bool j3valid[2]={
false};
19311 IkReal x1481=x1482.
value;
19312 j3array[0]=((-1.0)*x1481);
19313 sj3array[0]=
IKsin(j3array[0]);
19314 cj3array[0]=
IKcos(j3array[0]);
19315 j3array[1]=((3.14159265358979)+(((-1.0)*x1481)));
19316 sj3array[1]=
IKsin(j3array[1]);
19317 cj3array[1]=
IKcos(j3array[1]);
19318 if( j3array[0] >
IKPI )
19322 else if( j3array[0] < -
IKPI )
19323 { j3array[0]+=
IK2PI;
19326 if( j3array[1] >
IKPI )
19330 else if( j3array[1] < -
IKPI )
19331 { j3array[1]+=
IK2PI;
19334 for(
int ij3 = 0; ij3 < 2; ++ij3)
19336 if( !j3valid[ij3] )
19340 _ij3[0] = ij3; _ij3[1] = -1;
19341 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
19345 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19348 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19350 IkReal evalcond[5];
19351 IkReal x1483=
IKsin(j3);
19352 IkReal x1484=
IKcos(j3);
19353 IkReal x1485=(new_r22*x1484);
19354 IkReal x1486=(new_r22*x1483);
19355 IkReal x1487=((1.0)*x1484);
19356 evalcond[0]=(((new_r11*x1483))+((new_r01*x1484)));
19357 evalcond[1]=((((-1.0)*new_r10*x1487))+((new_r00*x1483)));
19358 evalcond[2]=((((-1.0)*new_r11*x1487))+((new_r01*x1483)));
19359 evalcond[3]=(((new_r11*x1486))+((new_r01*x1485)));
19360 evalcond[4]=(((new_r00*x1485))+((new_r10*x1486)));
19368 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19369 vinfos[0].jointtype = 1;
19370 vinfos[0].foffset = j0;
19371 vinfos[0].indices[0] = _ij0[0];
19372 vinfos[0].indices[1] = _ij0[1];
19373 vinfos[0].maxsolutions = _nj0;
19374 vinfos[1].jointtype = 1;
19375 vinfos[1].foffset = j1;
19376 vinfos[1].indices[0] = _ij1[0];
19377 vinfos[1].indices[1] = _ij1[1];
19378 vinfos[1].maxsolutions = _nj1;
19379 vinfos[2].jointtype = 1;
19380 vinfos[2].foffset = j2;
19381 vinfos[2].indices[0] = _ij2[0];
19382 vinfos[2].indices[1] = _ij2[1];
19383 vinfos[2].maxsolutions = _nj2;
19384 vinfos[3].jointtype = 1;
19385 vinfos[3].foffset = j3;
19386 vinfos[3].indices[0] = _ij3[0];
19387 vinfos[3].indices[1] = _ij3[1];
19388 vinfos[3].maxsolutions = _nj3;
19389 vinfos[4].jointtype = 1;
19390 vinfos[4].foffset = j4;
19391 vinfos[4].indices[0] = _ij4[0];
19392 vinfos[4].indices[1] = _ij4[1];
19393 vinfos[4].maxsolutions = _nj4;
19394 vinfos[5].jointtype = 1;
19395 vinfos[5].foffset = j5;
19396 vinfos[5].indices[0] = _ij5[0];
19397 vinfos[5].indices[1] = _ij5[1];
19398 vinfos[5].maxsolutions = _nj5;
19399 std::vector<int> vfree(0);
19412 IkReal j3array[2], cj3array[2], sj3array[2];
19413 bool j3valid[2]={
false};
19419 IkReal x1488=x1489.
value;
19420 j3array[0]=((-1.0)*x1488);
19421 sj3array[0]=
IKsin(j3array[0]);
19422 cj3array[0]=
IKcos(j3array[0]);
19423 j3array[1]=((3.14159265358979)+(((-1.0)*x1488)));
19424 sj3array[1]=
IKsin(j3array[1]);
19425 cj3array[1]=
IKcos(j3array[1]);
19426 if( j3array[0] >
IKPI )
19430 else if( j3array[0] < -
IKPI )
19431 { j3array[0]+=
IK2PI;
19434 if( j3array[1] >
IKPI )
19438 else if( j3array[1] < -
IKPI )
19439 { j3array[1]+=
IK2PI;
19442 for(
int ij3 = 0; ij3 < 2; ++ij3)
19444 if( !j3valid[ij3] )
19448 _ij3[0] = ij3; _ij3[1] = -1;
19449 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
19453 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19456 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19458 IkReal evalcond[5];
19459 IkReal x1490=
IKsin(j3);
19460 IkReal x1491=
IKcos(j3);
19461 IkReal x1492=(new_r22*x1491);
19462 IkReal x1493=(new_r22*x1490);
19463 IkReal x1494=((1.0)*x1491);
19464 evalcond[0]=(((new_r00*x1491))+((new_r10*x1490)));
19465 evalcond[1]=((((-1.0)*new_r10*x1494))+((new_r00*x1490)));
19466 evalcond[2]=(((new_r01*x1490))+(((-1.0)*new_r11*x1494)));
19467 evalcond[3]=(((new_r01*x1492))+((new_r11*x1493)));
19468 evalcond[4]=(((new_r00*x1492))+((new_r10*x1493)));
19476 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19477 vinfos[0].jointtype = 1;
19478 vinfos[0].foffset = j0;
19479 vinfos[0].indices[0] = _ij0[0];
19480 vinfos[0].indices[1] = _ij0[1];
19481 vinfos[0].maxsolutions = _nj0;
19482 vinfos[1].jointtype = 1;
19483 vinfos[1].foffset = j1;
19484 vinfos[1].indices[0] = _ij1[0];
19485 vinfos[1].indices[1] = _ij1[1];
19486 vinfos[1].maxsolutions = _nj1;
19487 vinfos[2].jointtype = 1;
19488 vinfos[2].foffset = j2;
19489 vinfos[2].indices[0] = _ij2[0];
19490 vinfos[2].indices[1] = _ij2[1];
19491 vinfos[2].maxsolutions = _nj2;
19492 vinfos[3].jointtype = 1;
19493 vinfos[3].foffset = j3;
19494 vinfos[3].indices[0] = _ij3[0];
19495 vinfos[3].indices[1] = _ij3[1];
19496 vinfos[3].maxsolutions = _nj3;
19497 vinfos[4].jointtype = 1;
19498 vinfos[4].foffset = j4;
19499 vinfos[4].indices[0] = _ij4[0];
19500 vinfos[4].indices[1] = _ij4[1];
19501 vinfos[4].maxsolutions = _nj4;
19502 vinfos[5].jointtype = 1;
19503 vinfos[5].foffset = j5;
19504 vinfos[5].indices[0] = _ij5[0];
19505 vinfos[5].indices[1] = _ij5[1];
19506 vinfos[5].maxsolutions = _nj5;
19507 std::vector<int> vfree(0);
19519 if( bgotonextstatement )
19521 bool bgotonextstatement =
true;
19526 bgotonextstatement=
false;
19531 if( bgotonextstatement )
19542 IkReal j3array[1], cj3array[1], sj3array[1];
19543 bool j3valid[1]={
false};
19549 IkReal x1495=x1496.
value;
19556 j3array[0]=
IKatan2((x1495*(x1497.
value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1495));
19557 sj3array[0]=
IKsin(j3array[0]);
19558 cj3array[0]=
IKcos(j3array[0]);
19559 if( j3array[0] >
IKPI )
19563 else if( j3array[0] < -
IKPI )
19564 { j3array[0]+=
IK2PI;
19567 for(
int ij3 = 0; ij3 < 1; ++ij3)
19569 if( !j3valid[ij3] )
19573 _ij3[0] = ij3; _ij3[1] = -1;
19574 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
19578 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19581 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19583 IkReal evalcond[18];
19584 IkReal x1498=
IKcos(j3);
19585 IkReal x1499=
IKsin(j3);
19586 IkReal x1500=((1.0)*cj4);
19587 IkReal x1501=((1.0)*cj5);
19588 IkReal x1502=(cj4*x1498);
19589 IkReal x1503=(sj4*x1499);
19590 IkReal x1504=((1.0)*x1498);
19591 IkReal x1505=(sj4*x1498);
19592 IkReal x1506=(cj4*x1499);
19593 IkReal x1507=(cj5*x1499);
19594 evalcond[0]=(x1505+new_r02);
19595 evalcond[1]=(x1503+new_r12);
19596 evalcond[2]=((((-1.0)*new_r02*x1499))+((new_r12*x1498)));
19597 evalcond[3]=(sj4+((new_r02*x1498))+((new_r12*x1499)));
19598 evalcond[4]=(sj5+(((-1.0)*new_r10*x1504))+((new_r00*x1499)));
19599 evalcond[5]=((((-1.0)*new_r11*x1504))+cj5+((new_r01*x1499)));
19600 evalcond[6]=(((sj5*x1502))+x1507+new_r01);
19601 evalcond[7]=(((new_r01*x1498))+((cj4*sj5))+((new_r11*x1499)));
19602 evalcond[8]=(((sj5*x1499))+(((-1.0)*cj5*x1498*x1500))+new_r00);
19603 evalcond[9]=((((-1.0)*x1498*x1501))+((sj5*x1506))+new_r11);
19604 evalcond[10]=((((-1.0)*cj5*x1500))+((new_r00*x1498))+((new_r10*x1499)));
19605 evalcond[11]=((((-1.0)*sj5*x1504))+(((-1.0)*x1500*x1507))+new_r10);
19606 evalcond[12]=(((new_r02*x1502))+((new_r22*sj4))+((new_r12*x1506)));
19607 evalcond[13]=(((new_r10*x1503))+(((-1.0)*new_r20*x1500))+((new_r00*x1505)));
19608 evalcond[14]=((((-1.0)*new_r21*x1500))+((new_r01*x1505))+((new_r11*x1503)));
19609 evalcond[15]=(sj5+((new_r01*x1502))+((new_r21*sj4))+((new_r11*x1506)));
19610 evalcond[16]=((1.0)+(((-1.0)*new_r22*x1500))+((new_r02*x1505))+((new_r12*x1503)));
19611 evalcond[17]=(((new_r10*x1506))+(((-1.0)*x1501))+((new_r20*sj4))+((new_r00*x1502)));
19619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19620 vinfos[0].jointtype = 1;
19621 vinfos[0].foffset = j0;
19622 vinfos[0].indices[0] = _ij0[0];
19623 vinfos[0].indices[1] = _ij0[1];
19624 vinfos[0].maxsolutions = _nj0;
19625 vinfos[1].jointtype = 1;
19626 vinfos[1].foffset = j1;
19627 vinfos[1].indices[0] = _ij1[0];
19628 vinfos[1].indices[1] = _ij1[1];
19629 vinfos[1].maxsolutions = _nj1;
19630 vinfos[2].jointtype = 1;
19631 vinfos[2].foffset = j2;
19632 vinfos[2].indices[0] = _ij2[0];
19633 vinfos[2].indices[1] = _ij2[1];
19634 vinfos[2].maxsolutions = _nj2;
19635 vinfos[3].jointtype = 1;
19636 vinfos[3].foffset = j3;
19637 vinfos[3].indices[0] = _ij3[0];
19638 vinfos[3].indices[1] = _ij3[1];
19639 vinfos[3].maxsolutions = _nj3;
19640 vinfos[4].jointtype = 1;
19641 vinfos[4].foffset = j4;
19642 vinfos[4].indices[0] = _ij4[0];
19643 vinfos[4].indices[1] = _ij4[1];
19644 vinfos[4].maxsolutions = _nj4;
19645 vinfos[5].jointtype = 1;
19646 vinfos[5].foffset = j5;
19647 vinfos[5].indices[0] = _ij5[0];
19648 vinfos[5].indices[1] = _ij5[1];
19649 vinfos[5].maxsolutions = _nj5;
19650 std::vector<int> vfree(0);
19663 IkReal j3array[1], cj3array[1], sj3array[1];
19664 bool j3valid[1]={
false};
19674 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1508.
value)))+(x1509.
value));
19675 sj3array[0]=
IKsin(j3array[0]);
19676 cj3array[0]=
IKcos(j3array[0]);
19677 if( j3array[0] >
IKPI )
19681 else if( j3array[0] < -
IKPI )
19682 { j3array[0]+=
IK2PI;
19685 for(
int ij3 = 0; ij3 < 1; ++ij3)
19687 if( !j3valid[ij3] )
19691 _ij3[0] = ij3; _ij3[1] = -1;
19692 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
19696 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
19699 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19701 IkReal evalcond[18];
19702 IkReal x1510=
IKcos(j3);
19703 IkReal x1511=
IKsin(j3);
19704 IkReal x1512=((1.0)*cj4);
19705 IkReal x1513=((1.0)*cj5);
19706 IkReal x1514=(cj4*x1510);
19707 IkReal x1515=(sj4*x1511);
19708 IkReal x1516=((1.0)*x1510);
19709 IkReal x1517=(sj4*x1510);
19710 IkReal x1518=(cj4*x1511);
19711 IkReal x1519=(cj5*x1511);
19712 evalcond[0]=(x1517+new_r02);
19713 evalcond[1]=(x1515+new_r12);
19714 evalcond[2]=((((-1.0)*new_r02*x1511))+((new_r12*x1510)));
19715 evalcond[3]=(sj4+((new_r02*x1510))+((new_r12*x1511)));
19716 evalcond[4]=(sj5+(((-1.0)*new_r10*x1516))+((new_r00*x1511)));
19717 evalcond[5]=((((-1.0)*new_r11*x1516))+cj5+((new_r01*x1511)));
19718 evalcond[6]=(((sj5*x1514))+x1519+new_r01);
19719 evalcond[7]=(((cj4*sj5))+((new_r01*x1510))+((new_r11*x1511)));
19720 evalcond[8]=((((-1.0)*cj5*x1510*x1512))+((sj5*x1511))+new_r00);
19721 evalcond[9]=(((sj5*x1518))+(((-1.0)*x1510*x1513))+new_r11);
19722 evalcond[10]=(((new_r10*x1511))+(((-1.0)*cj5*x1512))+((new_r00*x1510)));
19723 evalcond[11]=((((-1.0)*sj5*x1516))+(((-1.0)*x1512*x1519))+new_r10);
19724 evalcond[12]=(((new_r02*x1514))+((new_r22*sj4))+((new_r12*x1518)));
19725 evalcond[13]=(((new_r10*x1515))+(((-1.0)*new_r20*x1512))+((new_r00*x1517)));
19726 evalcond[14]=((((-1.0)*new_r21*x1512))+((new_r01*x1517))+((new_r11*x1515)));
19727 evalcond[15]=(sj5+((new_r01*x1514))+((new_r21*sj4))+((new_r11*x1518)));
19728 evalcond[16]=((1.0)+(((-1.0)*new_r22*x1512))+((new_r02*x1517))+((new_r12*x1515)));
19729 evalcond[17]=(((new_r10*x1518))+(((-1.0)*x1513))+((new_r20*sj4))+((new_r00*x1514)));
19737 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19738 vinfos[0].jointtype = 1;
19739 vinfos[0].foffset = j0;
19740 vinfos[0].indices[0] = _ij0[0];
19741 vinfos[0].indices[1] = _ij0[1];
19742 vinfos[0].maxsolutions = _nj0;
19743 vinfos[1].jointtype = 1;
19744 vinfos[1].foffset = j1;
19745 vinfos[1].indices[0] = _ij1[0];
19746 vinfos[1].indices[1] = _ij1[1];
19747 vinfos[1].maxsolutions = _nj1;
19748 vinfos[2].jointtype = 1;
19749 vinfos[2].foffset = j2;
19750 vinfos[2].indices[0] = _ij2[0];
19751 vinfos[2].indices[1] = _ij2[1];
19752 vinfos[2].maxsolutions = _nj2;
19753 vinfos[3].jointtype = 1;
19754 vinfos[3].foffset = j3;
19755 vinfos[3].indices[0] = _ij3[0];
19756 vinfos[3].indices[1] = _ij3[1];
19757 vinfos[3].maxsolutions = _nj3;
19758 vinfos[4].jointtype = 1;
19759 vinfos[4].foffset = j4;
19760 vinfos[4].indices[0] = _ij4[0];
19761 vinfos[4].indices[1] = _ij4[1];
19762 vinfos[4].maxsolutions = _nj4;
19763 vinfos[5].jointtype = 1;
19764 vinfos[5].foffset = j5;
19765 vinfos[5].indices[0] = _ij5[0];
19766 vinfos[5].indices[1] = _ij5[1];
19767 vinfos[5].maxsolutions = _nj5;
19768 std::vector<int> vfree(0);
19786 }
static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3],
int& numroots)
19788 using std::complex;
19789 if( rawcoeffs[0] == 0 ) {
19791 polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
19795 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19796 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
19797 complex<IkReal> coeffs[3];
19798 const int maxsteps = 110;
19799 for(
int i = 0; i < 3; ++i) {
19800 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19802 complex<IkReal> roots[3];
19804 roots[0] = complex<IkReal>(1,0);
19805 roots[1] = complex<IkReal>(0.4,0.9);
19808 for(
int i = 2; i < 3; ++i) {
19809 roots[i] = roots[i-1]*roots[1];
19813 bool changed =
false;
19814 for(
int i = 0; i < 3; ++i) {
19815 if ( err[i] >= tol ) {
19818 complex<IkReal>
x = roots[i] + coeffs[0];
19819 for(
int j = 1; j < 3; ++j) {
19820 x = roots[i] * x + coeffs[j];
19822 for(
int j = 0; j < 3; ++j) {
19824 if( roots[i] != roots[j] ) {
19825 x /= (roots[i] - roots[j]);
19839 bool visited[3] = {
false};
19840 for(
int i = 0; i < 3; ++i) {
19841 if( !visited[i] ) {
19844 complex<IkReal> newroot=roots[i];
19846 for(
int j = i+1; j < 3; ++j) {
19848 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19849 newroot += roots[j];
19858 if(
IKabs(imag(newroot)) < tolsqrt ) {
19859 rawroots[numroots++] = real(newroot);
19864 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2],
int& numroots) {
19865 IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19869 else if( det == 0 ) {
19870 rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19875 rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19876 rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);
19880 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4],
int& numroots)
19882 using std::complex;
19883 if( rawcoeffs[0] == 0 ) {
19885 polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19889 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19890 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
19891 complex<IkReal> coeffs[4];
19892 const int maxsteps = 110;
19893 for(
int i = 0; i < 4; ++i) {
19894 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19896 complex<IkReal> roots[4];
19898 roots[0] = complex<IkReal>(1,0);
19899 roots[1] = complex<IkReal>(0.4,0.9);
19902 for(
int i = 2; i < 4; ++i) {
19903 roots[i] = roots[i-1]*roots[1];
19907 bool changed =
false;
19908 for(
int i = 0; i < 4; ++i) {
19909 if ( err[i] >= tol ) {
19912 complex<IkReal>
x = roots[i] + coeffs[0];
19913 for(
int j = 1; j < 4; ++j) {
19914 x = roots[i] * x + coeffs[j];
19916 for(
int j = 0; j < 4; ++j) {
19918 if( roots[i] != roots[j] ) {
19919 x /= (roots[i] - roots[j]);
19933 bool visited[4] = {
false};
19934 for(
int i = 0; i < 4; ++i) {
19935 if( !visited[i] ) {
19938 complex<IkReal> newroot=roots[i];
19940 for(
int j = i+1; j < 4; ++j) {
19942 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19943 newroot += roots[j];
19952 if(
IKabs(imag(newroot)) < tolsqrt ) {
19953 rawroots[numroots++] = real(newroot);
19965 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
19970 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
19973 IKFAST_API
const char*
GetKinematicsHash() {
return "<robot:GenericRobot - fanuc_lrmate200id7l (4198bf8a131c845c474e8ef89919d50f)>"; }
19977 #ifdef IKFAST_NAMESPACE 19981 #ifndef IKFAST_NO_MAIN 19983 #include <stdlib.h> 19984 #ifdef IKFAST_NAMESPACE 19985 using namespace IKFAST_NAMESPACE;
19990 printf(
"\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" 19991 "Returns the ik solutions given the transformation of the end effector specified by\n" 19992 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" 19999 IkReal eerot[9],eetrans[3];
20000 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
20001 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
20002 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20003 for(std::size_t i = 0; i < vfree.size(); ++i)
20004 vfree[i] = atof(argv[13+i]);
20005 bool bSuccess =
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
20008 fprintf(stderr,
"Failed to get ik solution\n");
20012 printf(
"Found %d ik solutions:\n", (
int)solutions.
GetNumSolutions());
20016 printf(
"sol%d (free=%d): ", (
int)i, (
int)sol.
GetFree().size());
20017 std::vector<IkReal> vsolfree(sol.
GetFree().size());
20018 sol.
GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
20019 for( std::size_t j = 0; j < solvalues.size(); ++j)
20020 printf(
"%.15f, ", solvalues[j]);
IKFAST_API int * GetFreeParameters()
IKFAST_API const char * GetIkFastVersion()
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
IKFAST_API const char * GetKinematicsHash()
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
virtual size_t GetNumSolutions() const
returns the number of solutions stored
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
int main(int argc, char **argv)
The discrete solutions are returned in this structure.
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
#define IKFAST_ATAN2_MAGTHRESH
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
#define IKFAST_EVALCOND_THRESH
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
float IKfmod(float x, float y)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
IKFAST_API int GetIkType()
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
#define IKFAST_SOLUTION_THRESH
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKFAST_API int GetNumFreeParameters()
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
IKFAST_API int GetNumJoints()
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
float IKatan2(float fy, float fx)
Default implementation of IkSolutionListBase.
#define IKFAST_COMPILE_ASSERT(x)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
float IKatan2Simple(float fy, float fx)
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
#define IKFAST_SINCOS_THRESH
manages all the solutions
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
IKFAST_API int GetIkRealSize()