1 #define IKFAST_HAS_LIBRARY 26 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] 41 #ifndef __PRETTY_FUNCTION__ 42 #define __PRETTY_FUNCTION__ __FUNCDNAME__ 46 #ifndef __PRETTY_FUNCTION__ 47 #define __PRETTY_FUNCTION__ __func__ 50 #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()); } } 55 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x 57 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) 60 #define IK2PI ((IkReal)6.28318530717959) 61 #define IKPI ((IkReal)3.14159265358979) 62 #define IKPI_2 ((IkReal)1.57079632679490) 78 void dgetrf_ (
const int* m,
const int* n,
double* a,
const int* lda,
int* ipiv,
int* info);
79 void zgetrf_ (
const int* m,
const int* n, std::complex<double>* a,
const int* lda,
int* ipiv,
int* info);
80 void dgetri_(
const int* n,
const double* a,
const int* lda,
int* ipiv,
double* work,
const int* lwork,
int* info);
81 void dgesv_ (
const int* n,
const int* nrhs,
double* a,
const int* lda,
int* ipiv,
double* b,
const int* ldb,
int* info);
82 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);
83 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);
88 #ifdef IKFAST_NAMESPACE 89 namespace IKFAST_NAMESPACE {
92 inline float IKabs(
float f) {
return fabsf(f); }
93 inline double IKabs(
double f) {
return fabs(f); }
95 inline float IKsqr(
float f) {
return f*f; }
96 inline double IKsqr(
double f) {
return f*f; }
98 inline float IKlog(
float f) {
return logf(f); }
99 inline double IKlog(
double f) {
return log(f); }
102 #ifndef IKFAST_SINCOS_THRESH 103 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7) 107 #ifndef IKFAST_ATAN2_MAGTHRESH 108 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7) 112 #ifndef IKFAST_SOLUTION_THRESH 113 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) 117 #ifndef IKFAST_EVALCOND_THRESH 118 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) 125 if( f <= -1 )
return float(-
IKPI_2);
126 else if( f >= 1 )
return float(
IKPI_2);
132 if( f <= -1 )
return -
IKPI_2;
133 else if( f >= 1 )
return IKPI_2;
158 if( f <= -1 )
return float(
IKPI);
159 else if( f >= 1 )
return float(0);
165 if( f <= -1 )
return IKPI;
166 else if( f >= 1 )
return 0;
169 inline float IKsin(
float f) {
return sinf(f); }
171 inline float IKcos(
float f) {
return cosf(f); }
173 inline float IKtan(
float f) {
return tanf(f); }
175 inline float IKsqrt(
float f) {
if( f <= 0.0f )
return 0.0f;
return sqrtf(f); }
176 inline double IKsqrt(
double f) {
if( f <= 0.0 )
return 0.0;
return sqrt(f); }
178 return atan2f(fy,fx);
185 else if( isnan(fx) ) {
188 return atan2f(fy,fx);
198 else if( isnan(fx) ) {
204 template <
typename T>
211 template <
typename T>
217 if( !isnan(fy) && !isnan(fx) ) {
246 template <
typename T>
265 ret.
value = (T)1.0e30;
269 ret.
value = T(1.0)/f;
274 int num = n > 0 ? n : -n;
278 else if( num == 3 ) {
300 IKFAST_API
void ComputeFk(
const IkReal* j, IkReal* eetrans, IkReal* eerot) {
301 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47;
334 x32=((((-1.0)*x26))+x21);
335 x33=((((-1.0)*x25))+(((-1.0)*x28)));
337 x35=(((x23*x6))+x29);
339 x37=((((-1.0)*x0*x21))+x30);
340 x38=(x31+(((-1.0)*x20*x24)));
341 x39=(x0*(((((1.0)*x25))+x28)));
348 x46=(((x6*x7))+((x39*x5)));
349 IkReal x48=((1.0)*x20);
350 x47=(((x0*x7))+((x5*(((((-1.0)*x23*x48))+(((-1.0)*x25*x48)))))));
351 eerot[0]=(((x11*((((x10*x45))+x42))))+((x46*x8)));
352 eerot[1]=(((x11*x46))+((x8*(((((-1.0)*x12*x37))+(((-1.0)*x13*x45)))))));
353 eerot[2]=(((x45*x9))+((x10*(((((-1.0)*x30))+((x0*x21)))))));
354 IkReal x49=((1.0)*x24);
355 eetrans[0]=((-3.153)+((x18*x21))+((x0*x19))+(((-1.0)*x18*x49))+(((0.322)*x0))+((x10*((((x14*x21))+(((-1.0)*x14*x49))))))+((x17*x23))+((x17*x25))+((x9*(((((0.208)*x43))+((x15*x5))))))+j[0]+(((0.011)*x6)));
356 eerot[3]=(((x47*x8))+((x11*((((x10*x44))+((x38*x9)))))));
357 eerot[4]=(((x8*(((((-1.0)*x12*x38))+(((-1.0)*x13*x44))))))+((x11*x47)));
358 eerot[5]=(((x44*x9))+((x10*(((((-1.0)*x31))+((x24*x6)))))));
359 IkReal x50=((1.0)*x21);
360 eetrans[1]=((-0.212)+(((-1.0)*x16*x50))+(((-1.0)*x19*x6))+((x10*(((((-1.0)*x15*x50))+((x15*x24))))))+((x9*(((((0.208)*x41))+((x14*x5))))))+((x16*x24))+(((-0.322)*x6))+(((-0.2)*x29))+(((-0.2)*x23*x6))+(((0.011)*x0)));
361 eerot[6]=(((x11*(((((-1.0)*x12*x33))+(((-1.0)*x13*x32*x7))))))+((x34*x8)));
362 eerot[7]=(((x8*((((x33*x9))+x36))))+((x11*x34)));
363 eerot[8]=(((x10*x33))+((x22*(((((-1.0)*x21))+x26)))));
364 eetrans[2]=((1.718)+((x10*(((((-0.208)*x23))+(((-0.208)*x25))))))+(((0.2)*x21))+(((-1.395)*x25))+(((-1.395)*x23))+((x22*(((((-0.208)*x21))+(((0.208)*x24))))))+(((1.07)*x3))+(((-0.2)*x24)));
377 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,j6,cj6,sj6,htj6,j6mul,j2,cj2,sj2,htj2,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;
378 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij2[2], _nj2;
380 IkReal j100, cj100, sj100;
381 unsigned char _ij100[2], _nj100;
383 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; 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; j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; _ij2[0] = -1; _ij2[1] = -1; _nj2 = 0;
384 for(
int dummyiter = 0; dummyiter < 1; ++dummyiter) {
386 j2=pfree[0]; cj2=
cos(pfree[0]); sj2=
sin(pfree[0]), htj2=
tan(pfree[0]*0.5);
396 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
398 new_r00=((-1.0)*r20);
399 new_r01=((-1.0)*r21);
400 new_r02=((-1.0)*r22);
401 new_px=((1.167)+(((-1.0)*pz))+(((0.208)*r22)));
405 new_py=((0.212)+(((-0.208)*r12))+py);
409 new_pz=((3.153)+(((-0.208)*r02))+px);
410 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;
411 IkReal x51=((1.0)*px);
412 IkReal x52=((1.0)*pz);
413 IkReal x53=((1.0)*py);
414 pp=((px*px)+(py*py)+(pz*pz));
415 npx=(((px*r00))+((py*r10))+((pz*r20)));
416 npy=(((px*r01))+((py*r11))+((pz*r21)));
417 npz=(((px*r02))+((py*r12))+((pz*r22)));
418 rxp0_0=((((-1.0)*r20*x53))+((pz*r10)));
419 rxp0_1=(((px*r20))+(((-1.0)*r00*x52)));
420 rxp0_2=((((-1.0)*r10*x51))+((py*r00)));
421 rxp1_0=((((-1.0)*r21*x53))+((pz*r11)));
422 rxp1_1=(((px*r21))+(((-1.0)*r01*x52)));
423 rxp1_2=((((-1.0)*r11*x51))+((py*r01)));
424 rxp2_0=(((pz*r12))+(((-1.0)*r22*x53)));
425 rxp2_1=(((px*r22))+(((-1.0)*r02*x52)));
426 rxp2_2=((((-1.0)*r12*x51))+((py*r02)));
429 j3eval[0]=((sj2*sj2)+(cj2*cj2));
430 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
437 IkReal j3array[2], cj3array[2], sj3array[2];
438 bool j3valid[2]={
false};
440 IkReal x54=((((-1.395)*sj2))+(((0.2)*cj2)));
441 IkReal x55=((((-1.395)*cj2))+(((-0.2)*sj2)));
446 IkReal x56=((1.0)*(x58.
value));
447 if((((x54*x54)+(x55*x55))) < -0.00001)
455 IkReal x57=
IKasin(((x59.
value)*(((0.551)+px+(((1.07)*cj2))))));
456 j3array[0]=((((-1.0)*x56))+(((-1.0)*x57)));
457 sj3array[0]=
IKsin(j3array[0]);
458 cj3array[0]=
IKcos(j3array[0]);
459 j3array[1]=((3.14159265358979)+x57+(((-1.0)*x56)));
460 sj3array[1]=
IKsin(j3array[1]);
461 cj3array[1]=
IKcos(j3array[1]);
462 if( j3array[0] >
IKPI )
466 else if( j3array[0] < -
IKPI )
470 if( j3array[1] >
IKPI )
474 else if( j3array[1] < -
IKPI )
478 for(
int ij3 = 0; ij3 < 2; ++ij3)
484 _ij3[0] = ij3; _ij3[1] = -1;
485 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
489 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
492 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
495 IkReal j0array[2], cj0array[2], sj0array[2];
496 bool j0valid[2]={
false};
498 if((((3.538331)+(((0.428)*cj3))+(((-2.9853)*sj3))+(((1.11878)*cj2*cj3))+(((0.68908)*sj2))+(((1.17914)*cj2))+(((-1.0)*pp))+(pz*pz)+(((-1.40849)*cj2*sj3))+(((-1.11878)*sj2*sj3))+(((-1.40849)*cj3*sj2)))) < -0.00001)
500 IkReal x60=
IKsqrt(((3.538331)+(((0.428)*cj3))+(((-2.9853)*sj3))+(((1.11878)*cj2*cj3))+(((0.68908)*sj2))+(((1.17914)*cj2))+(((-1.0)*pp))+(pz*pz)+(((-1.40849)*cj2*sj3))+(((-1.11878)*sj2*sj3))+(((-1.40849)*cj3*sj2))));
502 sj0array[0]=
IKsin(j0array[0]);
503 cj0array[0]=
IKcos(j0array[0]);
504 j0array[1]=(pz+(((-1.0)*x60)));
505 sj0array[1]=
IKsin(j0array[1]);
506 cj0array[1]=
IKcos(j0array[1]);
509 for(
int ij0 = 0; ij0 < 2; ++ij0)
515 _ij0[0] = ij0; _ij0[1] = -1;
516 for(
int iij0 = ij0+1; iij0 < 2; ++iij0)
520 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
523 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
529 IkReal x63=((1000.0)*cj2);
530 IkReal x64=(cj2*j0*pz);
531 j1eval[0]=((((-2.0)*x64))+((cj2*pp))+(((-1.0)*cj2*x62))+((cj2*x61)));
532 j1eval[1]=
IKsign((((pp*x63))+((x61*x63))+(((-2000.0)*x64))+(((-1.0)*x62*x63))));
533 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
540 IkReal x68=(j0*pz*sj2);
541 IkReal x69=(sj2*x65);
542 IkReal x70=(sj2*x66);
543 j1eval[0]=((((-2.0)*x68))+x67+x69+(((-1.0)*x70)));
544 j1eval[1]=
IKsign(((((1000.0)*x69))+(((1000.0)*x67))+(((-1000.0)*x70))+(((-2000.0)*x68))));
545 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
549 IkReal x71=((18.1818181818182)*py);
551 j1eval[0]=((((-1.0)*cj3*sj2*x71))+(((-97.2727272727273)*x72))+(((-1.0)*cj2*sj3*x71))+(((126.818181818182)*sj3*x72))+(((-1.0)*pz))+(((-29.2727272727273)*py))+(((-126.818181818182)*cj2*cj3*py))+j0);
552 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
556 bool bgotonextstatement =
true;
559 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j2))), 6.28318530717959)));
560 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
562 bgotonextstatement=
false;
570 IkReal x75=((200.0)*sj3);
571 IkReal x76=((1395.0)*cj3);
573 j1eval[0]=((((-2.0)*x77))+pp+x73+(((-1.0)*x74)));
574 j1eval[1]=((
IKabs(((((-322.0)*py))+(((-11.0)*j0))+(((-1.0)*py*x76))+(((-1.0)*py*x75))+(((11.0)*pz)))))+(
IKabs((((pz*x75))+((pz*x76))+(((322.0)*pz))+(((11.0)*py))+(((-1.0)*j0*x75))+(((-1.0)*j0*x76))+(((-322.0)*j0))))));
575 j1eval[2]=
IKsign(((((1000.0)*x73))+(((-2000.0)*x77))+(((-1000.0)*x74))+(((1000.0)*pp))));
576 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
583 IkReal x78=((1395.0)*cj3);
584 IkReal x79=((18.1818181818182)*sj3);
585 IkReal x80=((1000.0)*py);
586 IkReal x81=((200.0)*sj3);
587 IkReal x82=((126.818181818182)*cj3);
588 j1eval[0]=(((j0*x82))+(((29.2727272727273)*j0))+((j0*x79))+(((-29.2727272727273)*pz))+py+(((-1.0)*pz*x79))+(((-1.0)*pz*x82)));
589 j1eval[1]=
IKsign(((((-322.0)*pz))+(((322.0)*j0))+((j0*x81))+((j0*x78))+(((11.0)*py))+(((-1.0)*pz*x78))+(((-1.0)*pz*x81))));
590 j1eval[2]=((
IKabs(((-3.542)+(((-15.345)*cj3))+((pz*x80))+(((-2.2)*sj3))+(((-1.0)*j0*x80)))))+(
IKabs(((-143.684)+(((-128.8)*sj3))+(((-898.38)*cj3))+(((-558.0)*cj3*sj3))+((py*x80))+(((-1906.025)*(cj3*cj3)))))));
591 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
598 j1eval[0]=((((-1.0)*pz))+(((-29.2727272727273)*py))+(((-18.1818181818182)*py*sj3))+(((-126.818181818182)*cj3*py))+j0);
599 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
603 bool bgotonextstatement =
true;
606 IkReal x83=
IKcos(pz);
607 IkReal x84=
IKsin(pz);
608 if((((-1.0)*(py*py))) < -0.00001)
610 IkReal x85=
IKsqrt(((-1.0)*(py*py)));
611 IkReal x86=
IKcos(x85);
612 IkReal x87=
IKsin(x85);
613 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
615 IkReal gconst0=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
616 IkReal gconst1=(((x84*x86))+((x83*x87)));
617 IkReal gconst2=((((-1.0)*x84*x87))+((x83*x86)));
618 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
620 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
621 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
623 bgotonextstatement=
false;
626 IkReal x88=
IKcos(pz);
627 IkReal x89=
IKsin(pz);
629 IkReal x91=
IKcos(x90);
630 IkReal x92=
IKsin(x90);
631 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
633 IkReal x93=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
641 IkReal gconst1=(((x89*x91))+((x88*x92)));
642 IkReal gconst2=((((-1.0)*x89*x92))+((x88*x91)));
644 IkReal x95=(cj3*sj3);
645 j1eval[0]=((1.11649844720497)+(((14.7983307453416)*x94))+sj3+(((6.975)*cj3))+(((4.33229813664596)*x95)));
646 j1eval[1]=
IKsign(((143805.0)+(((1906025.0)*x94))+(((898380.0)*cj3))+(((558000.0)*x95))+(((128800.0)*sj3))));
647 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
651 IkReal x96=
IKcos(pz);
652 IkReal x97=
IKsin(pz);
654 IkReal x99=
IKcos(x98);
655 IkReal x100=
IKsin(x98);
656 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
658 IkReal x101=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
666 IkReal gconst1=(((x100*x96))+((x97*x99)));
667 IkReal gconst2=(((x96*x99))+(((-1.0)*x100*x97)));
669 IkReal x103=((px*px)+(((-1.0)*pp))+(pz*pz));
670 if((x103) < -0.00001)
674 j1eval[0]=((((29.2727272727273)*x105))+py+(((126.818181818182)*cj3*x105))+(((18.1818181818182)*sj3*x105)));
675 j1eval[1]=
IKsign(((((11.0)*py))+(((322.0)*x105))+(((1395.0)*cj3*x105))+(((200.0)*sj3*x105))));
676 if((((-1.0)*x102)) < -0.00001)
678 j1eval[2]=((
IKabs(((-3.542)+(((-15.345)*cj3))+(((-1000.0)*py*(
IKsqrt(((-1.0)*x102)))))+(((-2.2)*sj3)))))+(
IKabs(((-143.684)+(((-128.8)*sj3))+(((1000.0)*x102))+(((-898.38)*cj3))+(((-558.0)*cj3*sj3))+(((-1906.025)*(cj3*cj3)))))));
679 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
683 IkReal x106=
IKcos(pz);
684 IkReal x107=
IKsin(pz);
686 IkReal x109=
IKcos(x108);
687 IkReal x110=
IKsin(x108);
688 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
690 IkReal x111=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
698 IkReal gconst1=(((x107*x109))+((x106*x110)));
699 IkReal gconst2=(((x106*x109))+(((-1.0)*x107*x110)));
701 IkReal x113=((px*px)+(((-1.0)*pp))+(pz*pz));
702 if((x113) < -0.00001)
706 j1eval[0]=((((-1.0)*py))+(((-126.818181818182)*cj3*x115))+(((-29.2727272727273)*x115))+(((-18.1818181818182)*sj3*x115)));
707 j1eval[1]=
IKsign(((((-51842.0)*x115))+(((-32200.0)*sj3*x115))+(((-224595.0)*cj3*x115))+(((-1771.0)*py))));
708 if((((-1.0)*x112)) < -0.00001)
710 j1eval[2]=((
IKabs(((23133.124)+(((89838.0)*cj3*sj3))+(((306870.025)*(cj3*cj3)))+(((-161000.0)*x112))+(((20736.8)*sj3))+(((144639.18)*cj3)))))+(
IKabs(((570.262)+(((2470.545)*cj3))+(((354.2)*sj3))+(((161000.0)*py*(
IKsqrt(((-1.0)*x112)))))))));
711 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
718 IkReal j1array[1], cj1array[1], sj1array[1];
719 bool j1valid[1]={
false};
721 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
723 IkReal x116=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
728 CheckValue<IkReal> x118 =
IKatan2WithCheck(IkReal(((23133.124)+(((89838.0)*cj3*sj3))+(((-161000.0)*(py*py)))+(((306870.025)*(cj3*cj3)))+(((20736.8)*sj3))+(((144639.18)*cj3)))),IkReal(((570.262)+(((2470.545)*cj3))+(((161000.0)*py*x116))+(((354.2)*sj3)))),
IKFAST_ATAN2_MAGTHRESH);
732 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.
value)))+(x118.
value));
733 sj1array[0]=
IKsin(j1array[0]);
734 cj1array[0]=
IKcos(j1array[0]);
735 if( j1array[0] >
IKPI )
739 else if( j1array[0] < -
IKPI )
743 for(
int ij1 = 0; ij1 < 1; ++ij1)
749 _ij1[0] = ij1; _ij1[1] = -1;
750 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
754 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
757 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
760 IkReal x119=
IKcos(j1);
761 IkReal x120=
IKsin(j1);
762 IkReal x121=((0.2)*sj3);
763 IkReal x122=((1.395)*cj3);
764 IkReal x123=(py*x119);
766 IkReal x125=(x120*x124);
767 evalcond[0]=((0.011)+(((-1.0)*py*x120))+((x119*x124)));
768 evalcond[1]=(py+(((0.322)*x119))+(((-0.011)*x120))+((x119*x121))+((x119*x122)));
769 evalcond[2]=((0.322)+x125+x122+x123+x121);
770 evalcond[3]=((-0.207368)+(((-0.644)*x123))+(((-0.644)*x125))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
771 evalcond[4]=(x124+(((0.011)*x119))+((x120*x121))+((x120*x122))+(((0.322)*x120)));
778 rotationfunction0(solutions);
789 IkReal j1array[1], cj1array[1], sj1array[1];
790 bool j1valid[1]={
false};
792 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
794 IkReal x698=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
803 j1array[0]=((-1.5707963267949)+(x699.
value)+(((1.5707963267949)*(x700.
value))));
804 sj1array[0]=
IKsin(j1array[0]);
805 cj1array[0]=
IKcos(j1array[0]);
806 if( j1array[0] >
IKPI )
810 else if( j1array[0] < -
IKPI )
814 for(
int ij1 = 0; ij1 < 1; ++ij1)
820 _ij1[0] = ij1; _ij1[1] = -1;
821 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
825 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
828 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
831 IkReal x701=
IKcos(j1);
832 IkReal x702=
IKsin(j1);
833 IkReal x703=((0.2)*sj3);
834 IkReal x704=((1.395)*cj3);
835 IkReal x705=(py*x701);
837 IkReal x707=(x702*x706);
838 evalcond[0]=((0.011)+((x701*x706))+(((-1.0)*py*x702)));
839 evalcond[1]=((((-0.011)*x702))+((x701*x704))+((x701*x703))+py+(((0.322)*x701)));
840 evalcond[2]=((0.322)+x703+x707+x704+x705);
841 evalcond[3]=((-0.207368)+(((-0.644)*x707))+(((-0.644)*x705))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
842 evalcond[4]=((((0.011)*x701))+((x702*x703))+((x702*x704))+(((0.322)*x702))+x706);
849 rotationfunction0(solutions);
860 IkReal j1array[1], cj1array[1], sj1array[1];
861 bool j1valid[1]={
false};
863 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
865 IkReal x708=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
874 j1array[0]=((-1.5707963267949)+(x709.
value)+(((1.5707963267949)*(x710.
value))));
875 sj1array[0]=
IKsin(j1array[0]);
876 cj1array[0]=
IKcos(j1array[0]);
877 if( j1array[0] >
IKPI )
881 else if( j1array[0] < -
IKPI )
885 for(
int ij1 = 0; ij1 < 1; ++ij1)
891 _ij1[0] = ij1; _ij1[1] = -1;
892 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
896 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
899 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
902 IkReal x711=
IKcos(j1);
903 IkReal x712=
IKsin(j1);
904 IkReal x713=((0.2)*sj3);
905 IkReal x714=((1.395)*cj3);
906 IkReal x715=(py*x711);
908 IkReal x717=(x712*x716);
909 evalcond[0]=((0.011)+((x711*x716))+(((-1.0)*py*x712)));
910 evalcond[1]=((((-0.011)*x712))+((x711*x713))+((x711*x714))+py+(((0.322)*x711)));
911 evalcond[2]=((0.322)+x713+x715+x714+x717);
912 evalcond[3]=((-0.207368)+(((-0.644)*x715))+(((-0.644)*x717))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
913 evalcond[4]=((((0.011)*x711))+(((0.322)*x712))+x716+((x712*x714))+((x712*x713)));
920 rotationfunction0(solutions);
930 if( bgotonextstatement )
932 bool bgotonextstatement =
true;
935 IkReal x718=
IKcos(pz);
936 IkReal x719=
IKsin(pz);
937 if((((-1.0)*(py*py))) < -0.00001)
939 IkReal x720=
IKsqrt(((-1.0)*(py*py)));
940 IkReal x721=
IKcos(x720);
941 IkReal x722=
IKsin(x720);
942 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
944 IkReal gconst3=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
945 IkReal gconst4=(((x719*x721))+(((-1.0)*x718*x722)));
946 IkReal gconst5=(((x718*x721))+((x719*x722)));
947 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
949 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
950 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
952 bgotonextstatement=
false;
955 IkReal x723=
IKcos(pz);
956 IkReal x724=
IKsin(pz);
958 IkReal x726=
IKcos(x725);
959 IkReal x727=
IKsin(x725);
960 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
962 IkReal x728=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
970 IkReal gconst4=((((-1.0)*x723*x727))+((x724*x726)));
971 IkReal gconst5=(((x723*x726))+((x724*x727)));
973 IkReal x730=(cj3*sj3);
974 j1eval[0]=((1.11649844720497)+(((4.33229813664596)*x730))+sj3+(((14.7983307453416)*x729))+(((6.975)*cj3)));
975 j1eval[1]=
IKsign(((143805.0)+(((1906025.0)*x729))+(((558000.0)*x730))+(((898380.0)*cj3))+(((128800.0)*sj3))));
976 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
980 IkReal x731=
IKcos(pz);
981 IkReal x732=
IKsin(pz);
983 IkReal x734=
IKcos(x733);
984 IkReal x735=
IKsin(x733);
985 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
987 IkReal x736=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
995 IkReal gconst4=((((-1.0)*x731*x735))+((x732*x734)));
996 IkReal gconst5=(((x731*x734))+((x732*x735)));
998 IkReal x738=((px*px)+(((-1.0)*pp))+(pz*pz));
999 if((x738) < -0.00001)
1001 IkReal x739=
IKsqrt(x738);
1003 j1eval[0]=((((-126.818181818182)*cj3*x740))+(((-29.2727272727273)*x740))+(((-18.1818181818182)*sj3*x740))+py);
1004 if((((-1.0)*x737)) < -0.00001)
1006 j1eval[1]=((
IKabs(((-3.542)+(((-15.345)*cj3))+(((1000.0)*py*(
IKsqrt(((-1.0)*x737)))))+(((-2.2)*sj3)))))+(
IKabs(((-143.684)+(((1000.0)*x737))+(((-128.8)*sj3))+(((-898.38)*cj3))+(((-558.0)*cj3*sj3))+(((-1906.025)*(cj3*cj3)))))));
1007 j1eval[2]=
IKsign(((((-200.0)*sj3*x740))+(((-1395.0)*cj3*x740))+(((11.0)*py))+(((-322.0)*x740))));
1008 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1012 IkReal x741=
IKcos(pz);
1013 IkReal x742=
IKsin(pz);
1015 IkReal x744=
IKcos(x743);
1016 IkReal x745=
IKsin(x743);
1017 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1019 IkReal x746=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1026 IkReal gconst3=x746;
1027 IkReal gconst4=(((x742*x744))+(((-1.0)*x741*x745)));
1028 IkReal gconst5=(((x742*x745))+((x741*x744)));
1030 IkReal x748=((px*px)+(((-1.0)*pp))+(pz*pz));
1031 if((x748) < -0.00001)
1033 IkReal x749=
IKsqrt(x748);
1035 j1eval[0]=((((18.1818181818182)*sj3*x750))+(((126.818181818182)*cj3*x750))+(((-1.0)*py))+(((29.2727272727273)*x750)));
1036 j1eval[1]=
IKsign(((((51842.0)*x750))+(((224595.0)*cj3*x750))+(((32200.0)*sj3*x750))+(((-1771.0)*py))));
1037 if((((-1.0)*x747)) < -0.00001)
1039 j1eval[2]=((
IKabs(((23133.124)+(((89838.0)*cj3*sj3))+(((306870.025)*(cj3*cj3)))+(((-161000.0)*x747))+(((20736.8)*sj3))+(((144639.18)*cj3)))))+(
IKabs(((570.262)+(((2470.545)*cj3))+(((-161000.0)*py*(
IKsqrt(((-1.0)*x747)))))+(((354.2)*sj3))))));
1040 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1047 IkReal j1array[1], cj1array[1], sj1array[1];
1048 bool j1valid[1]={
false};
1050 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1052 IkReal x751=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1057 CheckValue<IkReal> x753 =
IKatan2WithCheck(IkReal(((23133.124)+(((89838.0)*cj3*sj3))+(((-161000.0)*(py*py)))+(((306870.025)*(cj3*cj3)))+(((20736.8)*sj3))+(((144639.18)*cj3)))),IkReal(((570.262)+(((2470.545)*cj3))+(((-161000.0)*py*x751))+(((354.2)*sj3)))),
IKFAST_ATAN2_MAGTHRESH);
1061 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x752.
value)))+(x753.
value));
1062 sj1array[0]=
IKsin(j1array[0]);
1063 cj1array[0]=
IKcos(j1array[0]);
1064 if( j1array[0] >
IKPI )
1068 else if( j1array[0] < -
IKPI )
1069 { j1array[0]+=
IK2PI;
1072 for(
int ij1 = 0; ij1 < 1; ++ij1)
1078 _ij1[0] = ij1; _ij1[1] = -1;
1079 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1083 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1086 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1089 IkReal x754=
IKcos(j1);
1090 IkReal x755=
IKsin(j1);
1091 IkReal x756=((1.395)*cj3);
1092 IkReal x757=((0.2)*sj3);
1093 IkReal x758=(py*x754);
1095 IkReal x760=((1.0)*x759);
1096 IkReal x761=(x755*x759);
1097 evalcond[0]=((((0.322)*x754))+(((-0.011)*x755))+py+((x754*x757))+((x754*x756)));
1098 evalcond[1]=((0.011)+(((-1.0)*x754*x760))+(((-1.0)*py*x755)));
1099 evalcond[2]=((0.322)+(((-1.0)*x755*x760))+x757+x756+x758);
1100 evalcond[3]=((-0.207368)+(((-0.644)*x758))+(((-0.1288)*sj3))+(((0.644)*x761))+(((-0.89838)*cj3)));
1101 evalcond[4]=((((0.322)*x755))+(((-1.0)*x760))+((x755*x756))+((x755*x757))+(((0.011)*x754)));
1108 rotationfunction0(solutions);
1119 IkReal j1array[1], cj1array[1], sj1array[1];
1120 bool j1valid[1]={
false};
1122 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1124 IkReal x762=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1133 j1array[0]=((-1.5707963267949)+(x763.
value)+(((1.5707963267949)*(x764.
value))));
1134 sj1array[0]=
IKsin(j1array[0]);
1135 cj1array[0]=
IKcos(j1array[0]);
1136 if( j1array[0] >
IKPI )
1140 else if( j1array[0] < -
IKPI )
1141 { j1array[0]+=
IK2PI;
1144 for(
int ij1 = 0; ij1 < 1; ++ij1)
1150 _ij1[0] = ij1; _ij1[1] = -1;
1151 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1155 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1158 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1161 IkReal x765=
IKcos(j1);
1162 IkReal x766=
IKsin(j1);
1163 IkReal x767=((1.395)*cj3);
1164 IkReal x768=((0.2)*sj3);
1165 IkReal x769=(py*x765);
1167 IkReal x771=((1.0)*x770);
1168 IkReal x772=(x766*x770);
1169 evalcond[0]=((((0.322)*x765))+py+((x765*x767))+((x765*x768))+(((-0.011)*x766)));
1170 evalcond[1]=((0.011)+(((-1.0)*x765*x771))+(((-1.0)*py*x766)));
1171 evalcond[2]=((0.322)+(((-1.0)*x766*x771))+x768+x769+x767);
1172 evalcond[3]=((-0.207368)+(((0.644)*x772))+(((-0.1288)*sj3))+(((-0.644)*x769))+(((-0.89838)*cj3)));
1173 evalcond[4]=((((0.322)*x766))+((x766*x767))+((x766*x768))+(((-1.0)*x771))+(((0.011)*x765)));
1180 rotationfunction0(solutions);
1191 IkReal j1array[1], cj1array[1], sj1array[1];
1192 bool j1valid[1]={
false};
1194 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1196 IkReal x773=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1205 j1array[0]=((-1.5707963267949)+(x774.
value)+(((1.5707963267949)*(x775.
value))));
1206 sj1array[0]=
IKsin(j1array[0]);
1207 cj1array[0]=
IKcos(j1array[0]);
1208 if( j1array[0] >
IKPI )
1212 else if( j1array[0] < -
IKPI )
1213 { j1array[0]+=
IK2PI;
1216 for(
int ij1 = 0; ij1 < 1; ++ij1)
1222 _ij1[0] = ij1; _ij1[1] = -1;
1223 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1227 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1230 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1233 IkReal x776=
IKcos(j1);
1234 IkReal x777=
IKsin(j1);
1235 IkReal x778=((1.395)*cj3);
1236 IkReal x779=((0.2)*sj3);
1237 IkReal x780=(py*x776);
1239 IkReal x782=((1.0)*x781);
1240 IkReal x783=(x777*x781);
1241 evalcond[0]=(((x776*x778))+((x776*x779))+(((-0.011)*x777))+py+(((0.322)*x776)));
1242 evalcond[1]=((0.011)+(((-1.0)*py*x777))+(((-1.0)*x776*x782)));
1243 evalcond[2]=((0.322)+(((-1.0)*x777*x782))+x779+x778+x780);
1244 evalcond[3]=((-0.207368)+(((0.644)*x783))+(((-0.644)*x780))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
1245 evalcond[4]=((((0.011)*x776))+((x777*x778))+((x777*x779))+(((-1.0)*x782))+(((0.322)*x777)));
1252 rotationfunction0(solutions);
1262 if( bgotonextstatement )
1264 bool bgotonextstatement =
true;
1269 bgotonextstatement=
false;
1274 if( bgotonextstatement )
1284 IkReal j1array[1], cj1array[1], sj1array[1];
1285 bool j1valid[1]={
false};
1287 IkReal x784=(py*sj3);
1288 IkReal x785=((1000.0)*py);
1289 IkReal x786=(cj3*py);
1300 j1array[0]=
IKatan2(((x787.
value)*(((-3.542)+(((-15.345)*cj3))+((j0*x785))+(((-1.0)*pz*x785))+(((-2.2)*sj3))))), ((x788.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
1301 sj1array[0]=
IKsin(j1array[0]);
1302 cj1array[0]=
IKcos(j1array[0]);
1303 if( j1array[0] >
IKPI )
1307 else if( j1array[0] < -
IKPI )
1308 { j1array[0]+=
IK2PI;
1311 for(
int ij1 = 0; ij1 < 1; ++ij1)
1317 _ij1[0] = ij1; _ij1[1] = -1;
1318 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1322 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1325 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1328 IkReal x789=
IKcos(j1);
1329 IkReal x790=
IKsin(j1);
1330 IkReal x791=((1.395)*cj3);
1331 IkReal x792=((0.2)*sj3);
1332 IkReal x793=((1.0)*pz);
1333 IkReal x794=(py*x789);
1334 IkReal x795=((0.644)*x790);
1335 evalcond[0]=((0.011)+((j0*x789))+(((-1.0)*x789*x793))+(((-1.0)*py*x790)));
1336 evalcond[1]=((0.322)+((j0*x790))+(((-1.0)*x790*x793))+x791+x792+x794);
1337 evalcond[2]=((((0.322)*x789))+py+((x789*x791))+((x789*x792))+(((-0.011)*x790)));
1338 evalcond[3]=((-0.207368)+(((-1.0)*j0*x795))+((pz*x795))+(((-0.644)*x794))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
1339 evalcond[4]=((((0.011)*x789))+(((-1.0)*x793))+(((0.322)*x790))+((x790*x791))+((x790*x792))+j0);
1346 rotationfunction0(solutions);
1357 IkReal j1array[1], cj1array[1], sj1array[1];
1358 bool j1valid[1]={
false};
1360 IkReal x796=((1000.0)*py);
1361 IkReal x797=((200.0)*sj3);
1362 IkReal x798=((1395.0)*cj3);
1371 j1array[0]=((-1.5707963267949)+(x799.
value)+(((1.5707963267949)*(x800.
value))));
1372 sj1array[0]=
IKsin(j1array[0]);
1373 cj1array[0]=
IKcos(j1array[0]);
1374 if( j1array[0] >
IKPI )
1378 else if( j1array[0] < -
IKPI )
1379 { j1array[0]+=
IK2PI;
1382 for(
int ij1 = 0; ij1 < 1; ++ij1)
1388 _ij1[0] = ij1; _ij1[1] = -1;
1389 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1393 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1396 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1399 IkReal x801=
IKcos(j1);
1400 IkReal x802=
IKsin(j1);
1401 IkReal x803=((1.395)*cj3);
1402 IkReal x804=((0.2)*sj3);
1403 IkReal x805=((1.0)*pz);
1404 IkReal x806=(py*x801);
1405 IkReal x807=((0.644)*x802);
1406 evalcond[0]=((0.011)+(((-1.0)*py*x802))+((j0*x801))+(((-1.0)*x801*x805)));
1407 evalcond[1]=((0.322)+((j0*x802))+x804+x806+x803+(((-1.0)*x802*x805)));
1408 evalcond[2]=((((0.322)*x801))+(((-0.011)*x802))+py+((x801*x803))+((x801*x804)));
1409 evalcond[3]=((-0.207368)+(((-1.0)*j0*x807))+(((-0.1288)*sj3))+(((-0.644)*x806))+((pz*x807))+(((-0.89838)*cj3)));
1410 evalcond[4]=((((0.322)*x802))+(((0.011)*x801))+(((-1.0)*x805))+((x802*x803))+((x802*x804))+j0);
1417 rotationfunction0(solutions);
1428 IkReal j1array[1], cj1array[1], sj1array[1];
1429 bool j1valid[1]={
false};
1431 IkReal x808=((200.0)*sj3);
1432 IkReal x809=((1395.0)*cj3);
1437 CheckValue<IkReal> x811 =
IKatan2WithCheck(IkReal(((((-1.0)*j0*x808))+(((-1.0)*j0*x809))+(((322.0)*pz))+((pz*x808))+((pz*x809))+(((11.0)*py))+(((-322.0)*j0)))),IkReal(((((-322.0)*py))+(((-11.0)*j0))+(((11.0)*pz))+(((-1.0)*py*x809))+(((-1.0)*py*x808)))),
IKFAST_ATAN2_MAGTHRESH);
1441 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x810.
value)))+(x811.
value));
1442 sj1array[0]=
IKsin(j1array[0]);
1443 cj1array[0]=
IKcos(j1array[0]);
1444 if( j1array[0] >
IKPI )
1448 else if( j1array[0] < -
IKPI )
1449 { j1array[0]+=
IK2PI;
1452 for(
int ij1 = 0; ij1 < 1; ++ij1)
1458 _ij1[0] = ij1; _ij1[1] = -1;
1459 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1463 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1466 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1469 IkReal x812=
IKcos(j1);
1470 IkReal x813=
IKsin(j1);
1471 IkReal x814=((1.395)*cj3);
1472 IkReal x815=((0.2)*sj3);
1473 IkReal x816=((1.0)*pz);
1474 IkReal x817=(py*x812);
1475 IkReal x818=((0.644)*x813);
1476 evalcond[0]=((0.011)+(((-1.0)*x812*x816))+(((-1.0)*py*x813))+((j0*x812)));
1477 evalcond[1]=((0.322)+(((-1.0)*x813*x816))+x817+x814+x815+((j0*x813)));
1478 evalcond[2]=(((x812*x814))+((x812*x815))+(((0.322)*x812))+py+(((-0.011)*x813)));
1479 evalcond[3]=((-0.207368)+(((-0.644)*x817))+((pz*x818))+(((-1.0)*j0*x818))+(((-0.1288)*sj3))+(((-0.89838)*cj3)));
1480 evalcond[4]=((((-1.0)*x816))+((x813*x814))+((x813*x815))+(((0.322)*x813))+(((0.011)*x812))+j0);
1487 rotationfunction0(solutions);
1497 if( bgotonextstatement )
1499 bool bgotonextstatement =
true;
1502 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j2)))), 6.28318530717959)));
1503 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1505 bgotonextstatement=
false;
1510 j2=3.14159265358979;
1511 IkReal x819=cj3*cj3;
1512 IkReal x820=((200000.0)*sj3);
1513 IkReal x821=((1395000.0)*cj3);
1514 IkReal x822=(cj3*sj3);
1515 j1eval[0]=((1.11649844720497)+(((-1.0)*sj3))+(((-6.975)*cj3))+(((14.7983307453416)*x819))+(((4.33229813664596)*x822)));
1516 j1eval[1]=
IKsign(((143805.0)+(((1906025.0)*x819))+(((-128800.0)*sj3))+(((558000.0)*x822))+(((-898380.0)*cj3))));
1517 j1eval[2]=((
IKabs((((py*x820))+((py*x821))+(((-322000.0)*py))+(((-11000.0)*j0))+(((11000.0)*pz)))))+(
IKabs((((j0*x821))+((j0*x820))+(((-1.0)*pz*x821))+(((-1.0)*pz*x820))+(((322000.0)*pz))+(((-322000.0)*j0))+(((11000.0)*py))))));
1518 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1524 j2=3.14159265358979;
1527 IkReal x825=((200.0)*sj3);
1528 IkReal x826=((1395.0)*cj3);
1529 IkReal x827=(j0*pz);
1530 j1eval[0]=((((2.0)*x827))+(((-1.0)*pp))+(((-1.0)*x824))+x823);
1531 j1eval[1]=
IKsign(((((-1000.0)*pp))+(((1000.0)*x823))+(((2000.0)*x827))+(((-1000.0)*x824))));
1532 j1eval[2]=((
IKabs(((((-322.0)*pz))+((pz*x825))+((pz*x826))+(((322.0)*j0))+(((-11.0)*py))+(((-1.0)*j0*x825))+(((-1.0)*j0*x826)))))+(
IKabs(((((11.0)*j0))+(((-1.0)*py*x825))+(((-1.0)*py*x826))+(((-11.0)*pz))+(((322.0)*py))))));
1533 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1539 j2=3.14159265358979;
1540 j1eval[0]=((((18.1818181818182)*py*sj3))+(((126.818181818182)*cj3*py))+(((-1.0)*pz))+(((-29.2727272727273)*py))+j0);
1541 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
1545 bool bgotonextstatement =
true;
1548 IkReal x828=
IKcos(pz);
1549 IkReal x829=
IKsin(pz);
1550 if((((-1.0)*(py*py))) < -0.00001)
1552 IkReal x830=
IKsqrt(((-1.0)*(py*py)));
1553 IkReal x831=
IKcos(x830);
1554 IkReal x832=
IKsin(x830);
1555 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1557 IkReal gconst6=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1558 IkReal gconst7=(((x829*x831))+((x828*x832)));
1559 IkReal gconst8=((((-1.0)*x829*x832))+((x828*x831)));
1560 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1562 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
1563 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1565 bgotonextstatement=
false;
1568 IkReal x833=
IKcos(pz);
1569 IkReal x834=
IKsin(pz);
1571 IkReal x836=
IKcos(x835);
1572 IkReal x837=
IKsin(x835);
1573 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1575 IkReal x838=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1578 j2=3.14159265358979;
1582 IkReal gconst6=x838;
1583 IkReal gconst7=(((x833*x837))+((x834*x836)));
1584 IkReal gconst8=((((-1.0)*x834*x837))+((x833*x836)));
1585 IkReal x839=cj3*cj3;
1586 IkReal x840=(cj3*sj3);
1587 j1eval[0]=((1.11649844720497)+(((4.33229813664596)*x840))+(((-1.0)*sj3))+(((-6.975)*cj3))+(((14.7983307453416)*x839)));
1588 j1eval[1]=
IKsign(((143805.0)+(((1906025.0)*x839))+(((-128800.0)*sj3))+(((558000.0)*x840))+(((-898380.0)*cj3))));
1589 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
1593 IkReal x841=
IKcos(pz);
1594 IkReal x842=
IKsin(pz);
1596 IkReal x844=
IKcos(x843);
1597 IkReal x845=
IKsin(x843);
1598 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1600 IkReal x846=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1603 j2=3.14159265358979;
1607 IkReal gconst6=x846;
1608 IkReal gconst7=(((x842*x844))+((x841*x845)));
1609 IkReal gconst8=((((-1.0)*x842*x845))+((x841*x844)));
1611 IkReal x848=((px*px)+(((-1.0)*pp))+(pz*pz));
1612 if((x848) < -0.00001)
1614 IkReal x849=
IKsqrt(x848);
1616 j1eval[0]=((((-29.2727272727273)*x850))+(((-1.0)*py))+(((126.818181818182)*cj3*x850))+(((18.1818181818182)*sj3*x850)));
1617 j1eval[1]=
IKsign(((((1395.0)*cj3*x850))+(((-11.0)*py))+(((-322.0)*x850))+(((200.0)*sj3*x850))));
1618 if((((-1.0)*x847)) < -0.00001)
1620 j1eval[2]=((
IKabs(((3.542)+(((-15.345)*cj3))+(((1000.0)*py*(
IKsqrt(((-1.0)*x847)))))+(((-2.2)*sj3)))))+(
IKabs(((143.684)+(((-1000.0)*x847))+(((-128.8)*sj3))+(((-898.38)*cj3))+(((558.0)*cj3*sj3))+(((1906.025)*(cj3*cj3)))))));
1621 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1625 IkReal x851=
IKcos(pz);
1626 IkReal x852=
IKsin(pz);
1628 IkReal x854=
IKcos(x853);
1629 IkReal x855=
IKsin(x853);
1630 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1632 IkReal x856=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1635 j2=3.14159265358979;
1639 IkReal gconst6=x856;
1640 IkReal gconst7=(((x851*x855))+((x852*x854)));
1641 IkReal gconst8=((((-1.0)*x852*x855))+((x851*x854)));
1643 IkReal x858=((px*px)+(((-1.0)*pp))+(pz*pz));
1644 if((x858) < -0.00001)
1646 IkReal x859=
IKsqrt(x858);
1648 j1eval[0]=((((126.818181818182)*cj3*x860))+(((-1.0)*py))+(((18.1818181818182)*sj3*x860))+(((-29.2727272727273)*x860)));
1649 if((((-1.0)*x857)) < -0.00001)
1651 j1eval[1]=((
IKabs(((23133.124)+(((89838.0)*cj3*sj3))+(((-144639.18)*cj3))+(((306870.025)*(cj3*cj3)))+(((-161000.0)*x857))+(((-20736.8)*sj3)))))+(
IKabs(((570.262)+(((-2470.545)*cj3))+(((-354.2)*sj3))+(((161000.0)*py*(
IKsqrt(((-1.0)*x857)))))))));
1652 j1eval[2]=
IKsign(((((-51842.0)*x860))+(((-1771.0)*py))+(((224595.0)*cj3*x860))+(((32200.0)*sj3*x860))));
1653 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1660 IkReal j1array[1], cj1array[1], sj1array[1];
1661 bool j1valid[1]={
false};
1663 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1665 IkReal x861=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1666 CheckValue<IkReal> x862 =
IKatan2WithCheck(IkReal(((23133.124)+(((89838.0)*cj3*sj3))+(((-161000.0)*(py*py)))+(((-144639.18)*cj3))+(((306870.025)*(cj3*cj3)))+(((-20736.8)*sj3)))),IkReal(((570.262)+(((-2470.545)*cj3))+(((-354.2)*sj3))+(((161000.0)*py*x861)))),
IKFAST_ATAN2_MAGTHRESH);
1674 j1array[0]=((-1.5707963267949)+(x862.
value)+(((1.5707963267949)*(x863.
value))));
1675 sj1array[0]=
IKsin(j1array[0]);
1676 cj1array[0]=
IKcos(j1array[0]);
1677 if( j1array[0] >
IKPI )
1681 else if( j1array[0] < -
IKPI )
1682 { j1array[0]+=
IK2PI;
1685 for(
int ij1 = 0; ij1 < 1; ++ij1)
1691 _ij1[0] = ij1; _ij1[1] = -1;
1692 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1696 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1699 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1702 IkReal x864=
IKcos(j1);
1703 IkReal x865=
IKsin(j1);
1704 IkReal x866=((0.2)*sj3);
1705 IkReal x867=((1.395)*cj3);
1706 IkReal x868=((1.0)*x865);
1707 IkReal x869=(py*x864);
1709 evalcond[0]=((0.011)+(((-1.0)*py*x868))+((x864*x870)));
1710 evalcond[1]=((((0.322)*x864))+(((-1.0)*x864*x867))+(((-1.0)*x864*x866))+py+(((-0.011)*x865)));
1711 evalcond[2]=((-0.322)+(((-1.0)*x868*x870))+(((-1.0)*x869))+x867+x866);
1712 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*x865*x870))+(((-0.644)*x869))+(((0.89838)*cj3)));
1713 evalcond[4]=((((-1.0)*x865*x866))+(((-1.0)*x865*x867))+(((0.322)*x865))+(((0.011)*x864))+x870);
1720 rotationfunction0(solutions);
1731 IkReal j1array[1], cj1array[1], sj1array[1];
1732 bool j1valid[1]={
false};
1734 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1736 IkReal x871=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1745 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x872.
value)))+(x873.
value));
1746 sj1array[0]=
IKsin(j1array[0]);
1747 cj1array[0]=
IKcos(j1array[0]);
1748 if( j1array[0] >
IKPI )
1752 else if( j1array[0] < -
IKPI )
1753 { j1array[0]+=
IK2PI;
1756 for(
int ij1 = 0; ij1 < 1; ++ij1)
1762 _ij1[0] = ij1; _ij1[1] = -1;
1763 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1767 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1770 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1773 IkReal x874=
IKcos(j1);
1774 IkReal x875=
IKsin(j1);
1775 IkReal x876=((0.2)*sj3);
1776 IkReal x877=((1.395)*cj3);
1777 IkReal x878=((1.0)*x875);
1778 IkReal x879=(py*x874);
1780 evalcond[0]=((0.011)+((x874*x880))+(((-1.0)*py*x878)));
1781 evalcond[1]=((((0.322)*x874))+py+(((-0.011)*x875))+(((-1.0)*x874*x877))+(((-1.0)*x874*x876)));
1782 evalcond[2]=((-0.322)+(((-1.0)*x879))+x876+x877+(((-1.0)*x878*x880)));
1783 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*x879))+(((-0.644)*x875*x880))+(((0.89838)*cj3)));
1784 evalcond[4]=((((0.322)*x875))+(((0.011)*x874))+(((-1.0)*x875*x877))+(((-1.0)*x875*x876))+x880);
1791 rotationfunction0(solutions);
1802 IkReal j1array[1], cj1array[1], sj1array[1];
1803 bool j1valid[1]={
false};
1805 IkReal x881=((1395000.0)*cj3);
1806 IkReal x882=((200000.0)*sj3);
1807 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1809 IkReal x883=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1818 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x884.
value)))+(x885.
value));
1819 sj1array[0]=
IKsin(j1array[0]);
1820 cj1array[0]=
IKcos(j1array[0]);
1821 if( j1array[0] >
IKPI )
1825 else if( j1array[0] < -
IKPI )
1826 { j1array[0]+=
IK2PI;
1829 for(
int ij1 = 0; ij1 < 1; ++ij1)
1835 _ij1[0] = ij1; _ij1[1] = -1;
1836 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1840 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1843 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1846 IkReal x886=
IKcos(j1);
1847 IkReal x887=
IKsin(j1);
1848 IkReal x888=((0.2)*sj3);
1849 IkReal x889=((1.395)*cj3);
1850 IkReal x890=((1.0)*x887);
1851 IkReal x891=(py*x886);
1853 evalcond[0]=((0.011)+((x886*x892))+(((-1.0)*py*x890)));
1854 evalcond[1]=((((-1.0)*x886*x889))+(((-1.0)*x886*x888))+(((0.322)*x886))+py+(((-0.011)*x887)));
1855 evalcond[2]=((-0.322)+(((-1.0)*x891))+x889+x888+(((-1.0)*x890*x892)));
1856 evalcond[3]=((-0.207368)+(((-0.644)*x891))+(((0.1288)*sj3))+(((0.89838)*cj3))+(((-0.644)*x887*x892)));
1857 evalcond[4]=((((-1.0)*x887*x889))+(((-1.0)*x887*x888))+(((0.322)*x887))+(((0.011)*x886))+x892);
1864 rotationfunction0(solutions);
1874 if( bgotonextstatement )
1876 bool bgotonextstatement =
true;
1879 IkReal x893=
IKcos(pz);
1880 IkReal x894=
IKsin(pz);
1881 if((((-1.0)*(py*py))) < -0.00001)
1883 IkReal x895=
IKsqrt(((-1.0)*(py*py)));
1884 IkReal x896=
IKcos(x895);
1885 IkReal x897=
IKsin(x895);
1886 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1888 IkReal gconst9=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1889 IkReal gconst10=(((x894*x896))+(((-1.0)*x893*x897)));
1890 IkReal gconst11=(((x894*x897))+((x893*x896)));
1891 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1893 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
1894 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1896 bgotonextstatement=
false;
1899 IkReal x898=
IKcos(pz);
1900 IkReal x899=
IKsin(pz);
1902 IkReal x901=
IKcos(x900);
1903 IkReal x902=
IKsin(x900);
1904 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1906 IkReal x903=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1909 j2=3.14159265358979;
1913 IkReal gconst9=x903;
1914 IkReal gconst10=((((-1.0)*x898*x902))+((x899*x901)));
1915 IkReal gconst11=(((x899*x902))+((x898*x901)));
1916 IkReal x904=cj3*cj3;
1917 IkReal x905=(cj3*sj3);
1918 j1eval[0]=((1.11649844720497)+(((4.33229813664596)*x905))+(((-1.0)*sj3))+(((-6.975)*cj3))+(((14.7983307453416)*x904)));
1919 j1eval[1]=
IKsign(((143805.0)+(((1906025.0)*x904))+(((-128800.0)*sj3))+(((558000.0)*x905))+(((-898380.0)*cj3))));
1920 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
1924 IkReal x906=
IKcos(pz);
1925 IkReal x907=
IKsin(pz);
1927 IkReal x909=
IKcos(x908);
1928 IkReal x910=
IKsin(x908);
1929 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1931 IkReal x911=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1934 j2=3.14159265358979;
1938 IkReal gconst9=x911;
1939 IkReal gconst10=((((-1.0)*x906*x910))+((x907*x909)));
1940 IkReal gconst11=(((x906*x909))+((x907*x910)));
1942 IkReal x913=((px*px)+(((-1.0)*pp))+(pz*pz));
1943 if((x913) < -0.00001)
1945 IkReal x914=
IKsqrt(x913);
1947 j1eval[0]=((((-126.818181818182)*cj3*x915))+(((-1.0)*py))+(((-18.1818181818182)*sj3*x915))+(((29.2727272727273)*x915)));
1948 j1eval[1]=
IKsign(((((322.0)*x915))+(((-11.0)*py))+(((-1395.0)*cj3*x915))+(((-200.0)*sj3*x915))));
1949 if((((-1.0)*x912)) < -0.00001)
1951 j1eval[2]=((
IKabs(((3.542)+(((-1000.0)*py*(
IKsqrt(((-1.0)*x912)))))+(((-15.345)*cj3))+(((-2.2)*sj3)))))+(
IKabs(((143.684)+(((-128.8)*sj3))+(((-898.38)*cj3))+(((558.0)*cj3*sj3))+(((-1000.0)*x912))+(((1906.025)*(cj3*cj3)))))));
1952 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1956 IkReal x916=
IKcos(pz);
1957 IkReal x917=
IKsin(pz);
1959 IkReal x919=
IKcos(x918);
1960 IkReal x920=
IKsin(x918);
1961 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1963 IkReal x921=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1966 j2=3.14159265358979;
1970 IkReal gconst9=x921;
1971 IkReal gconst10=(((x917*x919))+(((-1.0)*x916*x920)));
1972 IkReal gconst11=(((x917*x920))+((x916*x919)));
1974 IkReal x923=((px*px)+(((-1.0)*pp))+(pz*pz));
1975 if((x923) < -0.00001)
1977 IkReal x924=
IKsqrt(x923);
1979 j1eval[0]=((((-1.0)*py))+(((-126.818181818182)*cj3*x925))+(((-18.1818181818182)*sj3*x925))+(((29.2727272727273)*x925)));
1980 if((((-1.0)*x922)) < -0.00001)
1982 j1eval[1]=((
IKabs(((23133.124)+(((89838.0)*cj3*sj3))+(((-144639.18)*cj3))+(((-161000.0)*x922))+(((306870.025)*(cj3*cj3)))+(((-20736.8)*sj3)))))+(
IKabs(((570.262)+(((-2470.545)*cj3))+(((-354.2)*sj3))+(((-161000.0)*py*(
IKsqrt(((-1.0)*x922)))))))));
1983 j1eval[2]=
IKsign(((((-224595.0)*cj3*x925))+(((-32200.0)*sj3*x925))+(((-1771.0)*py))+(((51842.0)*x925))));
1984 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1991 IkReal j1array[1], cj1array[1], sj1array[1];
1992 bool j1valid[1]={
false};
1994 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1996 IkReal x926=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
1997 CheckValue<IkReal> x927 =
IKatan2WithCheck(IkReal(((23133.124)+(((89838.0)*cj3*sj3))+(((-161000.0)*(py*py)))+(((-144639.18)*cj3))+(((306870.025)*(cj3*cj3)))+(((-20736.8)*sj3)))),IkReal(((570.262)+(((-2470.545)*cj3))+(((-354.2)*sj3))+(((-161000.0)*py*x926)))),
IKFAST_ATAN2_MAGTHRESH);
2005 j1array[0]=((-1.5707963267949)+(x927.
value)+(((1.5707963267949)*(x928.
value))));
2006 sj1array[0]=
IKsin(j1array[0]);
2007 cj1array[0]=
IKcos(j1array[0]);
2008 if( j1array[0] >
IKPI )
2012 else if( j1array[0] < -
IKPI )
2013 { j1array[0]+=
IK2PI;
2016 for(
int ij1 = 0; ij1 < 1; ++ij1)
2022 _ij1[0] = ij1; _ij1[1] = -1;
2023 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2027 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2030 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2033 IkReal x929=
IKcos(j1);
2034 IkReal x930=
IKsin(j1);
2035 IkReal x931=((1.0)*py);
2036 IkReal x932=((1.395)*cj3);
2037 IkReal x933=((0.2)*sj3);
2039 IkReal x935=((1.0)*x934);
2040 IkReal x936=(x930*x934);
2041 evalcond[0]=((((0.322)*x929))+py+(((-1.0)*x929*x932))+(((-1.0)*x929*x933))+(((-0.011)*x930)));
2042 evalcond[1]=((0.011)+(((-1.0)*x930*x931))+(((-1.0)*x929*x935)));
2043 evalcond[2]=((-0.322)+(((-1.0)*x929*x931))+x936+x933+x932);
2044 evalcond[3]=((-0.207368)+(((0.644)*x936))+(((0.1288)*sj3))+(((-0.644)*py*x929))+(((0.89838)*cj3)));
2045 evalcond[4]=((((-1.0)*x930*x933))+(((-1.0)*x930*x932))+(((0.322)*x930))+(((0.011)*x929))+(((-1.0)*x935)));
2052 rotationfunction0(solutions);
2063 IkReal j1array[1], cj1array[1], sj1array[1];
2064 bool j1valid[1]={
false};
2066 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2068 IkReal x937=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2077 j1array[0]=((-1.5707963267949)+(x938.
value)+(((1.5707963267949)*(x939.
value))));
2078 sj1array[0]=
IKsin(j1array[0]);
2079 cj1array[0]=
IKcos(j1array[0]);
2080 if( j1array[0] >
IKPI )
2084 else if( j1array[0] < -
IKPI )
2085 { j1array[0]+=
IK2PI;
2088 for(
int ij1 = 0; ij1 < 1; ++ij1)
2094 _ij1[0] = ij1; _ij1[1] = -1;
2095 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2099 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2102 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2105 IkReal x940=
IKcos(j1);
2106 IkReal x941=
IKsin(j1);
2107 IkReal x942=((1.0)*py);
2108 IkReal x943=((1.395)*cj3);
2109 IkReal x944=((0.2)*sj3);
2111 IkReal x946=((1.0)*x945);
2112 IkReal x947=(x941*x945);
2113 evalcond[0]=((((0.322)*x940))+py+(((-0.011)*x941))+(((-1.0)*x940*x944))+(((-1.0)*x940*x943)));
2114 evalcond[1]=((0.011)+(((-1.0)*x941*x942))+(((-1.0)*x940*x946)));
2115 evalcond[2]=((-0.322)+x943+x944+x947+(((-1.0)*x940*x942)));
2116 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*py*x940))+(((0.89838)*cj3))+(((0.644)*x947)));
2117 evalcond[4]=((((-1.0)*x946))+(((0.011)*x940))+(((-1.0)*x941*x944))+(((-1.0)*x941*x943))+(((0.322)*x941)));
2124 rotationfunction0(solutions);
2135 IkReal j1array[1], cj1array[1], sj1array[1];
2136 bool j1valid[1]={
false};
2138 IkReal x948=((1395000.0)*cj3);
2139 IkReal x949=((200000.0)*sj3);
2140 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2142 IkReal x950=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2151 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x951.
value)))+(x952.
value));
2152 sj1array[0]=
IKsin(j1array[0]);
2153 cj1array[0]=
IKcos(j1array[0]);
2154 if( j1array[0] >
IKPI )
2158 else if( j1array[0] < -
IKPI )
2159 { j1array[0]+=
IK2PI;
2162 for(
int ij1 = 0; ij1 < 1; ++ij1)
2168 _ij1[0] = ij1; _ij1[1] = -1;
2169 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2173 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2176 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2179 IkReal x953=
IKcos(j1);
2180 IkReal x954=
IKsin(j1);
2181 IkReal x955=((1.0)*py);
2182 IkReal x956=((1.395)*cj3);
2183 IkReal x957=((0.2)*sj3);
2185 IkReal x959=((1.0)*x958);
2186 IkReal x960=(x954*x958);
2187 evalcond[0]=((((0.322)*x953))+(((-0.011)*x954))+py+(((-1.0)*x953*x957))+(((-1.0)*x953*x956)));
2188 evalcond[1]=((0.011)+(((-1.0)*x953*x959))+(((-1.0)*x954*x955)));
2189 evalcond[2]=((-0.322)+(((-1.0)*x953*x955))+x960+x957+x956);
2190 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*py*x953))+(((0.89838)*cj3))+(((0.644)*x960)));
2191 evalcond[4]=((((0.322)*x954))+(((-1.0)*x954*x957))+(((-1.0)*x954*x956))+(((-1.0)*x959))+(((0.011)*x953)));
2198 rotationfunction0(solutions);
2208 if( bgotonextstatement )
2210 bool bgotonextstatement =
true;
2215 bgotonextstatement=
false;
2220 if( bgotonextstatement )
2230 IkReal j1array[1], cj1array[1], sj1array[1];
2231 bool j1valid[1]={
false};
2233 IkReal x961=(py*sj3);
2234 IkReal x962=((1000.0)*py);
2235 IkReal x963=(cj3*py);
2246 j1array[0]=
IKatan2(((x964.
value)*(((-3.542)+(((-1.0)*pz*x962))+(((15.345)*cj3))+(((2.2)*sj3))+((j0*x962))))), ((x965.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
2247 sj1array[0]=
IKsin(j1array[0]);
2248 cj1array[0]=
IKcos(j1array[0]);
2249 if( j1array[0] >
IKPI )
2253 else if( j1array[0] < -
IKPI )
2254 { j1array[0]+=
IK2PI;
2257 for(
int ij1 = 0; ij1 < 1; ++ij1)
2263 _ij1[0] = ij1; _ij1[1] = -1;
2264 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2268 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2271 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2274 IkReal x966=
IKcos(j1);
2275 IkReal x967=
IKsin(j1);
2276 IkReal x968=((1.395)*cj3);
2277 IkReal x969=((0.2)*sj3);
2278 IkReal x970=((1.0)*pz);
2279 IkReal x971=(py*x966);
2280 IkReal x972=(j0*x967);
2281 IkReal x973=(pz*x967);
2282 evalcond[0]=((0.011)+(((-1.0)*x966*x970))+(((-1.0)*py*x967))+((j0*x966)));
2283 evalcond[1]=((((0.322)*x966))+(((-0.011)*x967))+(((-1.0)*x966*x968))+(((-1.0)*x966*x969))+py);
2284 evalcond[2]=((-0.322)+(((-1.0)*x972))+(((-1.0)*x971))+x973+x968+x969);
2285 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*x971))+(((-0.644)*x972))+(((0.644)*x973))+(((0.89838)*cj3)));
2286 evalcond[4]=((((0.322)*x967))+(((-1.0)*x967*x969))+(((-1.0)*x967*x968))+(((0.011)*x966))+j0+(((-1.0)*x970)));
2293 rotationfunction0(solutions);
2304 IkReal j1array[1], cj1array[1], sj1array[1];
2305 bool j1valid[1]={
false};
2307 IkReal x974=((200.0)*sj3);
2308 IkReal x975=((1395.0)*cj3);
2313 CheckValue<IkReal> x977 =
IKatan2WithCheck(IkReal(((((-322.0)*pz))+(((322.0)*j0))+(((-11.0)*py))+(((-1.0)*j0*x974))+(((-1.0)*j0*x975))+((pz*x974))+((pz*x975)))),IkReal(((((11.0)*j0))+(((-1.0)*py*x975))+(((-1.0)*py*x974))+(((-11.0)*pz))+(((322.0)*py)))),
IKFAST_ATAN2_MAGTHRESH);
2317 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x976.
value)))+(x977.
value));
2318 sj1array[0]=
IKsin(j1array[0]);
2319 cj1array[0]=
IKcos(j1array[0]);
2320 if( j1array[0] >
IKPI )
2324 else if( j1array[0] < -
IKPI )
2325 { j1array[0]+=
IK2PI;
2328 for(
int ij1 = 0; ij1 < 1; ++ij1)
2334 _ij1[0] = ij1; _ij1[1] = -1;
2335 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2339 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2342 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2345 IkReal x978=
IKcos(j1);
2346 IkReal x979=
IKsin(j1);
2347 IkReal x980=((1.395)*cj3);
2348 IkReal x981=((0.2)*sj3);
2349 IkReal x982=((1.0)*pz);
2350 IkReal x983=(py*x978);
2351 IkReal x984=(j0*x979);
2352 IkReal x985=(pz*x979);
2353 evalcond[0]=((0.011)+(((-1.0)*x978*x982))+(((-1.0)*py*x979))+((j0*x978)));
2354 evalcond[1]=((((0.322)*x978))+(((-0.011)*x979))+(((-1.0)*x978*x981))+(((-1.0)*x978*x980))+py);
2355 evalcond[2]=((-0.322)+(((-1.0)*x983))+(((-1.0)*x984))+x985+x980+x981);
2356 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((-0.644)*x984))+(((-0.644)*x983))+(((0.644)*x985))+(((0.89838)*cj3)));
2357 evalcond[4]=((((-1.0)*x982))+(((0.322)*x979))+(((0.011)*x978))+(((-1.0)*x979*x981))+(((-1.0)*x979*x980))+j0);
2364 rotationfunction0(solutions);
2375 IkReal j1array[1], cj1array[1], sj1array[1];
2376 bool j1valid[1]={
false};
2378 IkReal x986=((200000.0)*sj3);
2379 IkReal x987=((1395000.0)*cj3);
2380 CheckValue<IkReal> x988 =
IKatan2WithCheck(IkReal(((((-1.0)*pz*x987))+(((-1.0)*pz*x986))+(((322000.0)*pz))+(((-322000.0)*j0))+((j0*x986))+((j0*x987))+(((11000.0)*py)))),IkReal((((py*x986))+((py*x987))+(((-322000.0)*py))+(((-11000.0)*j0))+(((11000.0)*pz)))),
IKFAST_ATAN2_MAGTHRESH);
2388 j1array[0]=((-1.5707963267949)+(x988.
value)+(((1.5707963267949)*(x989.
value))));
2389 sj1array[0]=
IKsin(j1array[0]);
2390 cj1array[0]=
IKcos(j1array[0]);
2391 if( j1array[0] >
IKPI )
2395 else if( j1array[0] < -
IKPI )
2396 { j1array[0]+=
IK2PI;
2399 for(
int ij1 = 0; ij1 < 1; ++ij1)
2405 _ij1[0] = ij1; _ij1[1] = -1;
2406 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2410 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2413 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2416 IkReal x990=
IKcos(j1);
2417 IkReal x991=
IKsin(j1);
2418 IkReal x992=((1.395)*cj3);
2419 IkReal x993=((0.2)*sj3);
2420 IkReal x994=((1.0)*pz);
2421 IkReal x995=(py*x990);
2422 IkReal x996=(j0*x991);
2423 IkReal x997=(pz*x991);
2424 evalcond[0]=((0.011)+(((-1.0)*x990*x994))+(((-1.0)*py*x991))+((j0*x990)));
2425 evalcond[1]=((((-0.011)*x991))+(((-1.0)*x990*x993))+(((-1.0)*x990*x992))+py+(((0.322)*x990)));
2426 evalcond[2]=((-0.322)+(((-1.0)*x996))+(((-1.0)*x995))+x993+x992+x997);
2427 evalcond[3]=((-0.207368)+(((0.1288)*sj3))+(((0.644)*x997))+(((-0.644)*x996))+(((-0.644)*x995))+(((0.89838)*cj3)));
2428 evalcond[4]=((((0.011)*x990))+(((-1.0)*x991*x993))+(((-1.0)*x991*x992))+(((-1.0)*x994))+(((0.322)*x991))+j0);
2435 rotationfunction0(solutions);
2445 if( bgotonextstatement )
2447 bool bgotonextstatement =
true;
2450 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j2)))), 6.28318530717959)));
2451 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2453 bgotonextstatement=
false;
2459 IkReal x998=cj3*cj3;
2460 IkReal x999=((1395000.0)*sj3);
2461 IkReal x1000=((200000.0)*cj3);
2462 IkReal x1001=(cj3*sj3);
2463 j1eval[0]=((6.97523347701149)+cj3+(((-3.42317708333333)*x998))+(((-6.975)*sj3))+(((-1.00215517241379)*x1001)));
2464 j1eval[1]=((
IKabs(((((-1.0)*j0*x1000))+(((-1.0)*pz*x999))+(((-1392000.0)*j0))+((pz*x1000))+((j0*x999))+(((1392000.0)*pz))+(((11000.0)*py)))))+(
IKabs(((((-1.0)*py*x1000))+((py*x999))+(((-1392000.0)*py))+(((-11000.0)*j0))+(((11000.0)*pz))))));
2465 j1eval[2]=
IKsign(((3883810.0)+(((-1906025.0)*x998))+(((556800.0)*cj3))+(((-3883680.0)*sj3))+(((-558000.0)*x1001))));
2466 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2475 IkReal x1004=((200.0)*cj3);
2476 IkReal x1005=((1395.0)*sj3);
2477 IkReal x1006=(j0*pz);
2478 j1eval[0]=((((-1.0)*x1003))+x1002+(((-1.0)*pp))+(((2.0)*x1006)));
2479 j1eval[1]=
IKsign(((((2000.0)*x1006))+(((-1000.0)*pp))+(((-1000.0)*x1003))+(((1000.0)*x1002))));
2480 j1eval[2]=((
IKabs(((((11.0)*j0))+(((-1.0)*py*x1005))+(((-11.0)*pz))+((py*x1004))+(((1392.0)*py)))))+(
IKabs(((((1392.0)*j0))+(((-1.0)*pz*x1004))+(((-1.0)*j0*x1005))+((j0*x1004))+(((-11.0)*py))+((pz*x1005))+(((-1392.0)*pz))))));
2481 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2488 j1eval[0]=((((-126.545454545455)*py))+(((126.818181818182)*py*sj3))+(((-1.0)*pz))+(((-18.1818181818182)*cj3*py))+j0);
2489 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
2493 bool bgotonextstatement =
true;
2496 IkReal x1007=
IKcos(pz);
2497 IkReal x1008=
IKsin(pz);
2498 if((((-1.0)*(py*py))) < -0.00001)
2500 IkReal x1009=
IKsqrt(((-1.0)*(py*py)));
2501 IkReal x1010=
IKcos(x1009);
2502 IkReal x1011=
IKsin(x1009);
2503 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2505 IkReal gconst12=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2506 IkReal gconst13=(((x1007*x1011))+((x1008*x1010)));
2507 IkReal gconst14=((((-1.0)*x1008*x1011))+((x1007*x1010)));
2508 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2510 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
2511 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2513 bgotonextstatement=
false;
2516 IkReal x1012=
IKcos(pz);
2517 IkReal x1013=
IKsin(pz);
2519 IkReal x1015=
IKcos(x1014);
2520 IkReal x1016=
IKsin(x1014);
2521 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2523 IkReal x1017=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2530 IkReal gconst12=x1017;
2531 IkReal gconst13=(((x1013*x1015))+((x1012*x1016)));
2532 IkReal gconst14=((((-1.0)*x1013*x1016))+((x1012*x1015)));
2533 IkReal x1018=cj3*cj3;
2534 IkReal x1019=(cj3*sj3);
2535 j1eval[0]=((6.97523347701149)+cj3+(((-3.42317708333333)*x1018))+(((-1.00215517241379)*x1019))+(((-6.975)*sj3)));
2536 j1eval[1]=
IKsign(((3883810.0)+(((556800.0)*cj3))+(((-1906025.0)*x1018))+(((-3883680.0)*sj3))+(((-558000.0)*x1019))));
2537 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
2541 IkReal x1020=
IKcos(pz);
2542 IkReal x1021=
IKsin(pz);
2544 IkReal x1023=
IKcos(x1022);
2545 IkReal x1024=
IKsin(x1022);
2546 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2548 IkReal x1025=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2555 IkReal gconst12=x1025;
2556 IkReal gconst13=(((x1021*x1023))+((x1020*x1024)));
2557 IkReal gconst14=((((-1.0)*x1021*x1024))+((x1020*x1023)));
2559 IkReal x1027=((px*px)+(((-1.0)*pp))+(pz*pz));
2560 if((x1027) < -0.00001)
2562 IkReal x1028=
IKsqrt(x1027);
2564 j1eval[0]=((((126.818181818182)*sj3*x1029))+(((-1.0)*py))+(((-126.545454545455)*x1029))+(((-18.1818181818182)*cj3*x1029)));
2565 j1eval[1]=
IKsign(((((-11.0)*py))+(((1395.0)*sj3*x1029))+(((-200.0)*cj3*x1029))+(((-1392.0)*x1029))));
2566 if((((-1.0)*x1026)) < -0.00001)
2568 j1eval[2]=((
IKabs(((3883.689)+(((556.8)*cj3))+(((-3883.68)*sj3))+(((-558.0)*cj3*sj3))+(((-1906.025)*(cj3*cj3)))+(((-1000.0)*x1026)))))+(
IKabs(((15.312)+(((2.2)*cj3))+(((-15.345)*sj3))+(((1000.0)*py*(
IKsqrt(((-1.0)*x1026)))))))));
2569 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2573 IkReal x1030=
IKcos(pz);
2574 IkReal x1031=
IKsin(pz);
2576 IkReal x1033=
IKcos(x1032);
2577 IkReal x1034=
IKsin(x1032);
2578 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2580 IkReal x1035=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2587 IkReal gconst12=x1035;
2588 IkReal gconst13=(((x1031*x1033))+((x1030*x1034)));
2589 IkReal gconst14=(((x1030*x1033))+(((-1.0)*x1031*x1034)));
2590 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2592 j1eval[0]=((((-126.545454545455)*py))+(((126.818181818182)*py*sj3))+(((-18.1818181818182)*cj3*py))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))));
2593 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
2600 IkReal j1array[1], cj1array[1], sj1array[1];
2601 bool j1valid[1]={
false};
2603 IkReal x1036=(cj3*py);
2604 IkReal x1037=(py*sj3);
2605 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2607 IkReal x1038=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2618 j1array[0]=
IKatan2(((x1039.
value)*(((-15.312)+(((15.345)*sj3))+(((1000.0)*py*x1038))+(((-2.2)*cj3))))), ((x1040.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
2619 sj1array[0]=
IKsin(j1array[0]);
2620 cj1array[0]=
IKcos(j1array[0]);
2621 if( j1array[0] >
IKPI )
2625 else if( j1array[0] < -
IKPI )
2626 { j1array[0]+=
IK2PI;
2629 for(
int ij1 = 0; ij1 < 1; ++ij1)
2635 _ij1[0] = ij1; _ij1[1] = -1;
2636 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2640 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2643 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2646 IkReal x1041=
IKcos(j1);
2647 IkReal x1042=
IKsin(j1);
2648 IkReal x1043=((0.2)*cj3);
2649 IkReal x1044=((1.395)*sj3);
2650 IkReal x1045=(py*x1041);
2651 IkReal x1046=((1.0)*x1042);
2653 evalcond[0]=((0.011)+((x1041*x1047))+(((-1.0)*py*x1046)));
2654 evalcond[1]=(((x1041*x1043))+(((-1.0)*x1041*x1044))+(((-0.011)*x1042))+py+(((1.392)*x1041)));
2655 evalcond[2]=((-1.392)+(((-1.0)*x1046*x1047))+x1044+(((-1.0)*x1045))+(((-1.0)*x1043)));
2656 evalcond[3]=((-3.875328)+(((-0.5568)*cj3))+(((-2.784)*x1045))+(((-2.784)*x1042*x1047))+(((3.88368)*sj3)));
2657 evalcond[4]=(x1047+((x1042*x1043))+(((0.011)*x1041))+(((-1.0)*x1042*x1044))+(((1.392)*x1042)));
2664 rotationfunction0(solutions);
2675 IkReal j1array[1], cj1array[1], sj1array[1];
2676 bool j1valid[1]={
false};
2678 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2680 IkReal x1048=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2689 j1array[0]=((-1.5707963267949)+(x1049.
value)+(((1.5707963267949)*(x1050.
value))));
2690 sj1array[0]=
IKsin(j1array[0]);
2691 cj1array[0]=
IKcos(j1array[0]);
2692 if( j1array[0] >
IKPI )
2696 else if( j1array[0] < -
IKPI )
2697 { j1array[0]+=
IK2PI;
2700 for(
int ij1 = 0; ij1 < 1; ++ij1)
2706 _ij1[0] = ij1; _ij1[1] = -1;
2707 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2711 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2714 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2717 IkReal x1051=
IKcos(j1);
2718 IkReal x1052=
IKsin(j1);
2719 IkReal x1053=((0.2)*cj3);
2720 IkReal x1054=((1.395)*sj3);
2721 IkReal x1055=(py*x1051);
2722 IkReal x1056=((1.0)*x1052);
2724 evalcond[0]=((0.011)+((x1051*x1057))+(((-1.0)*py*x1056)));
2725 evalcond[1]=((((-0.011)*x1052))+((x1051*x1053))+(((1.392)*x1051))+py+(((-1.0)*x1051*x1054)));
2726 evalcond[2]=((-1.392)+(((-1.0)*x1053))+x1054+(((-1.0)*x1055))+(((-1.0)*x1056*x1057)));
2727 evalcond[3]=((-3.875328)+(((-0.5568)*cj3))+(((-2.784)*x1052*x1057))+(((-2.784)*x1055))+(((3.88368)*sj3)));
2728 evalcond[4]=((((-1.0)*x1052*x1054))+x1057+((x1052*x1053))+(((0.011)*x1051))+(((1.392)*x1052)));
2735 rotationfunction0(solutions);
2746 IkReal j1array[1], cj1array[1], sj1array[1];
2747 bool j1valid[1]={
false};
2749 IkReal x1058=((1395000.0)*sj3);
2750 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2752 IkReal x1059=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2761 j1array[0]=((-1.5707963267949)+(x1060.
value)+(((1.5707963267949)*(x1061.
value))));
2762 sj1array[0]=
IKsin(j1array[0]);
2763 cj1array[0]=
IKcos(j1array[0]);
2764 if( j1array[0] >
IKPI )
2768 else if( j1array[0] < -
IKPI )
2769 { j1array[0]+=
IK2PI;
2772 for(
int ij1 = 0; ij1 < 1; ++ij1)
2778 _ij1[0] = ij1; _ij1[1] = -1;
2779 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2783 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2786 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2789 IkReal x1062=
IKcos(j1);
2790 IkReal x1063=
IKsin(j1);
2791 IkReal x1064=((0.2)*cj3);
2792 IkReal x1065=((1.395)*sj3);
2793 IkReal x1066=(py*x1062);
2794 IkReal x1067=((1.0)*x1063);
2796 evalcond[0]=((0.011)+(((-1.0)*py*x1067))+((x1062*x1068)));
2797 evalcond[1]=((((-1.0)*x1062*x1065))+((x1062*x1064))+(((1.392)*x1062))+(((-0.011)*x1063))+py);
2798 evalcond[2]=((-1.392)+x1065+(((-1.0)*x1067*x1068))+(((-1.0)*x1064))+(((-1.0)*x1066)));
2799 evalcond[3]=((-3.875328)+(((-2.784)*x1063*x1068))+(((-0.5568)*cj3))+(((-2.784)*x1066))+(((3.88368)*sj3)));
2800 evalcond[4]=((((-1.0)*x1063*x1065))+x1068+(((1.392)*x1063))+((x1063*x1064))+(((0.011)*x1062)));
2807 rotationfunction0(solutions);
2817 if( bgotonextstatement )
2819 bool bgotonextstatement =
true;
2822 IkReal x1069=
IKcos(pz);
2823 IkReal x1070=
IKsin(pz);
2824 if((((-1.0)*(py*py))) < -0.00001)
2826 IkReal x1071=
IKsqrt(((-1.0)*(py*py)));
2827 IkReal x1072=
IKcos(x1071);
2828 IkReal x1073=
IKsin(x1071);
2829 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2831 IkReal gconst15=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2832 IkReal gconst16=(((x1070*x1072))+(((-1.0)*x1069*x1073)));
2833 IkReal gconst17=(((x1069*x1072))+((x1070*x1073)));
2834 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2836 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
2837 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2839 bgotonextstatement=
false;
2842 IkReal x1074=
IKcos(pz);
2843 IkReal x1075=
IKsin(pz);
2845 IkReal x1077=
IKcos(x1076);
2846 IkReal x1078=
IKsin(x1076);
2847 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2849 IkReal x1079=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2856 IkReal gconst15=x1079;
2857 IkReal gconst16=(((x1075*x1077))+(((-1.0)*x1074*x1078)));
2858 IkReal gconst17=(((x1074*x1077))+((x1075*x1078)));
2859 IkReal x1080=cj3*cj3;
2860 IkReal x1081=(cj3*sj3);
2861 j1eval[0]=((6.97523347701149)+(((-3.42317708333333)*x1080))+cj3+(((-1.00215517241379)*x1081))+(((-6.975)*sj3)));
2862 j1eval[1]=
IKsign(((3883810.0)+(((556800.0)*cj3))+(((-3883680.0)*sj3))+(((-1906025.0)*x1080))+(((-558000.0)*x1081))));
2863 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
2867 IkReal x1082=
IKcos(pz);
2868 IkReal x1083=
IKsin(pz);
2870 IkReal x1085=
IKcos(x1084);
2871 IkReal x1086=
IKsin(x1084);
2872 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2874 IkReal x1087=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2881 IkReal gconst15=x1087;
2882 IkReal gconst16=((((-1.0)*x1082*x1086))+((x1083*x1085)));
2883 IkReal gconst17=(((x1082*x1085))+((x1083*x1086)));
2885 IkReal x1089=((px*px)+(((-1.0)*pp))+(pz*pz));
2886 if((x1089) < -0.00001)
2888 IkReal x1090=
IKsqrt(x1089);
2890 j1eval[0]=((((-126.818181818182)*sj3*x1091))+(((-1.0)*py))+(((126.545454545455)*x1091))+(((18.1818181818182)*cj3*x1091)));
2891 if((((-1.0)*x1088)) < -0.00001)
2893 j1eval[1]=((
IKabs(((3883.689)+(((556.8)*cj3))+(((-3883.68)*sj3))+(((-558.0)*cj3*sj3))+(((-1000.0)*x1088))+(((-1906.025)*(cj3*cj3))))))+(
IKabs(((15.312)+(((2.2)*cj3))+(((-15.345)*sj3))+(((-1000.0)*py*(
IKsqrt(((-1.0)*x1088)))))))));
2894 j1eval[2]=
IKsign(((((-11.0)*py))+(((200.0)*cj3*x1091))+(((1392.0)*x1091))+(((-1395.0)*sj3*x1091))));
2895 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2899 IkReal x1092=
IKcos(pz);
2900 IkReal x1093=
IKsin(pz);
2902 IkReal x1095=
IKcos(x1094);
2903 IkReal x1096=
IKsin(x1094);
2904 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2906 IkReal x1097=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2913 IkReal gconst15=x1097;
2914 IkReal gconst16=((((-1.0)*x1092*x1096))+((x1093*x1095)));
2915 IkReal gconst17=(((x1092*x1095))+((x1093*x1096)));
2916 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2918 j1eval[0]=((((126.545454545455)*py))+(((18.1818181818182)*cj3*py))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+(((-126.818181818182)*py*sj3)));
2919 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
2926 IkReal j1array[1], cj1array[1], sj1array[1];
2927 bool j1valid[1]={
false};
2929 IkReal x1098=(cj3*py);
2930 IkReal x1099=(py*sj3);
2931 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2933 IkReal x1100=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
2944 j1array[0]=
IKatan2(((x1101.
value)*(((15.312)+(((2.2)*cj3))+(((-15.345)*sj3))+(((1000.0)*py*x1100))))), ((x1102.
value)*(((121.0)+(((-1000000.0)*(py*py)))))));
2945 sj1array[0]=
IKsin(j1array[0]);
2946 cj1array[0]=
IKcos(j1array[0]);
2947 if( j1array[0] >
IKPI )
2951 else if( j1array[0] < -
IKPI )
2952 { j1array[0]+=
IK2PI;
2955 for(
int ij1 = 0; ij1 < 1; ++ij1)
2961 _ij1[0] = ij1; _ij1[1] = -1;
2962 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2966 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2969 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2972 IkReal x1103=
IKcos(j1);
2973 IkReal x1104=
IKsin(j1);
2974 IkReal x1105=((0.2)*cj3);
2975 IkReal x1106=((1.0)*py);
2976 IkReal x1107=((1.395)*sj3);
2978 IkReal x1109=((1.0)*x1108);
2979 IkReal x1110=(x1104*x1108);
2980 evalcond[0]=((((-1.0)*x1103*x1107))+(((-0.011)*x1104))+py+((x1103*x1105))+(((1.392)*x1103)));
2981 evalcond[1]=((0.011)+(((-1.0)*x1103*x1109))+(((-1.0)*x1104*x1106)));
2982 evalcond[2]=((-1.392)+(((-1.0)*x1103*x1106))+x1107+x1110+(((-1.0)*x1105)));
2983 evalcond[3]=((-3.875328)+(((-0.5568)*cj3))+(((-2.784)*py*x1103))+(((2.784)*x1110))+(((3.88368)*sj3)));
2984 evalcond[4]=(((x1104*x1105))+(((0.011)*x1103))+(((-1.0)*x1104*x1107))+(((-1.0)*x1109))+(((1.392)*x1104)));
2991 rotationfunction0(solutions);
3002 IkReal j1array[1], cj1array[1], sj1array[1];
3003 bool j1valid[1]={
false};
3005 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3007 IkReal x1111=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3016 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1112.
value)))+(x1113.
value));
3017 sj1array[0]=
IKsin(j1array[0]);
3018 cj1array[0]=
IKcos(j1array[0]);
3019 if( j1array[0] >
IKPI )
3023 else if( j1array[0] < -
IKPI )
3024 { j1array[0]+=
IK2PI;
3027 for(
int ij1 = 0; ij1 < 1; ++ij1)
3033 _ij1[0] = ij1; _ij1[1] = -1;
3034 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3038 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3041 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3044 IkReal x1114=
IKcos(j1);
3045 IkReal x1115=
IKsin(j1);
3046 IkReal x1116=((0.2)*cj3);
3047 IkReal x1117=((1.0)*py);
3048 IkReal x1118=((1.395)*sj3);
3050 IkReal x1120=((1.0)*x1119);
3051 IkReal x1121=(x1115*x1119);
3052 evalcond[0]=((((1.392)*x1114))+((x1114*x1116))+(((-1.0)*x1114*x1118))+(((-0.011)*x1115))+py);
3053 evalcond[1]=((0.011)+(((-1.0)*x1114*x1120))+(((-1.0)*x1115*x1117)));
3054 evalcond[2]=((-1.392)+x1118+x1121+(((-1.0)*x1114*x1117))+(((-1.0)*x1116)));
3055 evalcond[3]=((-3.875328)+(((2.784)*x1121))+(((-0.5568)*cj3))+(((-2.784)*py*x1114))+(((3.88368)*sj3)));
3056 evalcond[4]=((((1.392)*x1115))+(((0.011)*x1114))+(((-1.0)*x1120))+((x1115*x1116))+(((-1.0)*x1115*x1118)));
3063 rotationfunction0(solutions);
3074 IkReal j1array[1], cj1array[1], sj1array[1];
3075 bool j1valid[1]={
false};
3077 IkReal x1122=((1395000.0)*sj3);
3078 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3080 IkReal x1123=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3089 j1array[0]=((-1.5707963267949)+(x1124.
value)+(((1.5707963267949)*(x1125.
value))));
3090 sj1array[0]=
IKsin(j1array[0]);
3091 cj1array[0]=
IKcos(j1array[0]);
3092 if( j1array[0] >
IKPI )
3096 else if( j1array[0] < -
IKPI )
3097 { j1array[0]+=
IK2PI;
3100 for(
int ij1 = 0; ij1 < 1; ++ij1)
3106 _ij1[0] = ij1; _ij1[1] = -1;
3107 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3111 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3114 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3117 IkReal x1126=
IKcos(j1);
3118 IkReal x1127=
IKsin(j1);
3119 IkReal x1128=((0.2)*cj3);
3120 IkReal x1129=((1.0)*py);
3121 IkReal x1130=((1.395)*sj3);
3123 IkReal x1132=((1.0)*x1131);
3124 IkReal x1133=(x1127*x1131);
3125 evalcond[0]=((((-0.011)*x1127))+(((-1.0)*x1126*x1130))+py+(((1.392)*x1126))+((x1126*x1128)));
3126 evalcond[1]=((0.011)+(((-1.0)*x1126*x1132))+(((-1.0)*x1127*x1129)));
3127 evalcond[2]=((-1.392)+x1133+x1130+(((-1.0)*x1126*x1129))+(((-1.0)*x1128)));
3128 evalcond[3]=((-3.875328)+(((-0.5568)*cj3))+(((2.784)*x1133))+(((-2.784)*py*x1126))+(((3.88368)*sj3)));
3129 evalcond[4]=(((x1127*x1128))+(((-1.0)*x1127*x1130))+(((0.011)*x1126))+(((-1.0)*x1132))+(((1.392)*x1127)));
3136 rotationfunction0(solutions);
3146 if( bgotonextstatement )
3148 bool bgotonextstatement =
true;
3153 bgotonextstatement=
false;
3158 if( bgotonextstatement )
3168 IkReal j1array[1], cj1array[1], sj1array[1];
3169 bool j1valid[1]={
false};
3171 IkReal x1134=(py*sj3);
3172 IkReal x1135=(cj3*py);
3173 IkReal x1136=((1000.0)*py);
3184 j1array[0]=
IKatan2(((x1137.
value)*(((-15.312)+(((15.345)*sj3))+(((-1.0)*pz*x1136))+((j0*x1136))+(((-2.2)*cj3))))), ((x1138.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
3185 sj1array[0]=
IKsin(j1array[0]);
3186 cj1array[0]=
IKcos(j1array[0]);
3187 if( j1array[0] >
IKPI )
3191 else if( j1array[0] < -
IKPI )
3192 { j1array[0]+=
IK2PI;
3195 for(
int ij1 = 0; ij1 < 1; ++ij1)
3201 _ij1[0] = ij1; _ij1[1] = -1;
3202 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3206 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3209 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3212 IkReal x1139=
IKcos(j1);
3213 IkReal x1140=
IKsin(j1);
3214 IkReal x1141=((0.2)*cj3);
3215 IkReal x1142=((1.395)*sj3);
3216 IkReal x1143=((1.0)*pz);
3217 IkReal x1144=((2.784)*x1140);
3218 IkReal x1145=(py*x1139);
3219 IkReal x1146=((1.0)*x1140);
3220 evalcond[0]=((0.011)+((j0*x1139))+(((-1.0)*x1139*x1143))+(((-1.0)*py*x1146)));
3221 evalcond[1]=((((1.392)*x1139))+py+((x1139*x1141))+(((-1.0)*x1139*x1142))+(((-0.011)*x1140)));
3222 evalcond[2]=((-1.392)+x1142+((pz*x1140))+(((-1.0)*x1141))+(((-1.0)*x1145))+(((-1.0)*j0*x1146)));
3223 evalcond[3]=((-3.875328)+(((-2.784)*x1145))+(((-0.5568)*cj3))+((pz*x1144))+(((-1.0)*j0*x1144))+(((3.88368)*sj3)));
3224 evalcond[4]=((((-1.0)*x1140*x1142))+((x1140*x1141))+(((-1.0)*x1143))+(((0.011)*x1139))+(((1.392)*x1140))+j0);
3231 rotationfunction0(solutions);
3242 IkReal j1array[1], cj1array[1], sj1array[1];
3243 bool j1valid[1]={
false};
3245 IkReal x1147=((200.0)*cj3);
3246 IkReal x1148=((1395.0)*sj3);
3251 CheckValue<IkReal> x1150 =
IKatan2WithCheck(IkReal(((((1392.0)*j0))+((pz*x1148))+((j0*x1147))+(((-11.0)*py))+(((-1.0)*j0*x1148))+(((-1.0)*pz*x1147))+(((-1392.0)*pz)))),IkReal(((((11.0)*j0))+((py*x1147))+(((-11.0)*pz))+(((1392.0)*py))+(((-1.0)*py*x1148)))),
IKFAST_ATAN2_MAGTHRESH);
3255 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1149.
value)))+(x1150.
value));
3256 sj1array[0]=
IKsin(j1array[0]);
3257 cj1array[0]=
IKcos(j1array[0]);
3258 if( j1array[0] >
IKPI )
3262 else if( j1array[0] < -
IKPI )
3263 { j1array[0]+=
IK2PI;
3266 for(
int ij1 = 0; ij1 < 1; ++ij1)
3272 _ij1[0] = ij1; _ij1[1] = -1;
3273 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3277 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3280 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3283 IkReal x1151=
IKcos(j1);
3284 IkReal x1152=
IKsin(j1);
3285 IkReal x1153=((0.2)*cj3);
3286 IkReal x1154=((1.395)*sj3);
3287 IkReal x1155=((1.0)*pz);
3288 IkReal x1156=((2.784)*x1152);
3289 IkReal x1157=(py*x1151);
3290 IkReal x1158=((1.0)*x1152);
3291 evalcond[0]=((0.011)+((j0*x1151))+(((-1.0)*py*x1158))+(((-1.0)*x1151*x1155)));
3292 evalcond[1]=((((1.392)*x1151))+py+((x1151*x1153))+(((-1.0)*x1151*x1154))+(((-0.011)*x1152)));
3293 evalcond[2]=((-1.392)+((pz*x1152))+x1154+(((-1.0)*x1153))+(((-1.0)*j0*x1158))+(((-1.0)*x1157)));
3294 evalcond[3]=((-3.875328)+((pz*x1156))+(((-0.5568)*cj3))+(((-2.784)*x1157))+(((-1.0)*j0*x1156))+(((3.88368)*sj3)));
3295 evalcond[4]=((((0.011)*x1151))+(((-1.0)*x1155))+((x1152*x1153))+(((1.392)*x1152))+(((-1.0)*x1152*x1154))+j0);
3302 rotationfunction0(solutions);
3313 IkReal j1array[1], cj1array[1], sj1array[1];
3314 bool j1valid[1]={
false};
3316 IkReal x1159=((1395000.0)*sj3);
3317 IkReal x1160=((200000.0)*cj3);
3322 CheckValue<IkReal> x1162 =
IKatan2WithCheck(IkReal((((j0*x1159))+(((-1392000.0)*j0))+((pz*x1160))+(((-1.0)*pz*x1159))+(((1392000.0)*pz))+(((-1.0)*j0*x1160))+(((11000.0)*py)))),IkReal(((((-1392000.0)*py))+(((-1.0)*py*x1160))+(((-11000.0)*j0))+(((11000.0)*pz))+((py*x1159)))),
IKFAST_ATAN2_MAGTHRESH);
3326 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1161.
value)))+(x1162.
value));
3327 sj1array[0]=
IKsin(j1array[0]);
3328 cj1array[0]=
IKcos(j1array[0]);
3329 if( j1array[0] >
IKPI )
3333 else if( j1array[0] < -
IKPI )
3334 { j1array[0]+=
IK2PI;
3337 for(
int ij1 = 0; ij1 < 1; ++ij1)
3343 _ij1[0] = ij1; _ij1[1] = -1;
3344 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3348 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3351 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3354 IkReal x1163=
IKcos(j1);
3355 IkReal x1164=
IKsin(j1);
3356 IkReal x1165=((0.2)*cj3);
3357 IkReal x1166=((1.395)*sj3);
3358 IkReal x1167=((1.0)*pz);
3359 IkReal x1168=((2.784)*x1164);
3360 IkReal x1169=(py*x1163);
3361 IkReal x1170=((1.0)*x1164);
3362 evalcond[0]=((0.011)+(((-1.0)*x1163*x1167))+(((-1.0)*py*x1170))+((j0*x1163)));
3363 evalcond[1]=((((1.392)*x1163))+((x1163*x1165))+(((-1.0)*x1163*x1166))+(((-0.011)*x1164))+py);
3364 evalcond[2]=((-1.392)+x1166+((pz*x1164))+(((-1.0)*x1165))+(((-1.0)*x1169))+(((-1.0)*j0*x1170)));
3365 evalcond[3]=((-3.875328)+(((-0.5568)*cj3))+(((-2.784)*x1169))+((pz*x1168))+(((-1.0)*j0*x1168))+(((3.88368)*sj3)));
3366 evalcond[4]=(((x1164*x1165))+(((1.392)*x1164))+(((0.011)*x1163))+(((-1.0)*x1164*x1166))+(((-1.0)*x1167))+j0);
3373 rotationfunction0(solutions);
3383 if( bgotonextstatement )
3385 bool bgotonextstatement =
true;
3388 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j2)))), 6.28318530717959)));
3389 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3391 bgotonextstatement=
false;
3396 j2=-1.5707963267949;
3399 IkReal x1173=((200.0)*cj3);
3400 IkReal x1174=((1395.0)*sj3);
3401 IkReal x1175=(j0*pz);
3402 j1eval[0]=(x1171+(((-2.0)*x1175))+pp+(((-1.0)*x1172)));
3403 j1eval[1]=((
IKabs(((((748.0)*j0))+(((-1.0)*pz*x1173))+(((-748.0)*pz))+((pz*x1174))+((j0*x1173))+(((11.0)*py))+(((-1.0)*j0*x1174)))))+(
IKabs(((((-11.0)*j0))+(((-1.0)*py*x1174))+((py*x1173))+(((11.0)*pz))+(((748.0)*py))))));
3404 j1eval[2]=
IKsign(((((-1000.0)*x1172))+(((1000.0)*x1171))+(((-2000.0)*x1175))+(((1000.0)*pp))));
3405 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3411 j2=-1.5707963267949;
3412 IkReal x1176=(cj3*j0);
3413 IkReal x1177=(cj3*pz);
3414 IkReal x1178=((1395.0)*sj3);
3415 IkReal x1179=((1000.0)*py);
3416 IkReal x1180=((126.818181818182)*sj3);
3417 j1eval[0]=((((68.0)*pz))+(((-1.0)*pz*x1180))+(((-68.0)*j0))+py+((j0*x1180))+(((-18.1818181818182)*x1176))+(((18.1818181818182)*x1177)));
3418 j1eval[1]=((
IKabs(((8.228)+(((2.2)*cj3))+(((-15.345)*sj3))+((pz*x1179))+(((-1.0)*j0*x1179)))))+(
IKabs(((-2505.529)+(((558.0)*cj3*sj3))+(((-299.2)*cj3))+(((1906.025)*(cj3*cj3)))+(((2086.92)*sj3))+((py*x1179))))));
3419 j1eval[2]=
IKsign(((((-748.0)*j0))+(((-200.0)*x1176))+(((-1.0)*pz*x1178))+(((200.0)*x1177))+((j0*x1178))+(((11.0)*py))+(((748.0)*pz))));
3420 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3426 j2=-1.5707963267949;
3427 j1eval[0]=((((68.0)*py))+(((-1.0)*pz))+(((18.1818181818182)*cj3*py))+(((-126.818181818182)*py*sj3))+j0);
3428 if(
IKabs(j1eval[0]) < 0.0000010000000000 )
3432 bool bgotonextstatement =
true;
3435 IkReal x1181=
IKcos(pz);
3436 IkReal x1182=
IKsin(pz);
3437 if((((-1.0)*(py*py))) < -0.00001)
3439 IkReal x1183=
IKsqrt(((-1.0)*(py*py)));
3440 IkReal x1184=
IKcos(x1183);
3441 IkReal x1185=
IKsin(x1183);
3442 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3444 IkReal gconst18=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
3445 IkReal gconst19=(((x1182*x1184))+((x1181*x1185)));
3446 IkReal gconst20=((((-1.0)*x1182*x1185))+((x1181*x1184)));
3447 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3449 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
3450 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3452 bgotonextstatement=
false;
3455 IkReal x1186=
IKcos(pz);
3456 IkReal x1187=
IKsin(pz);
3458 IkReal x1189=
IKcos(x1188);
3459 IkReal x1190=
IKsin(x1188);
3460 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3462 IkReal x1191=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
3465 j2=-1.5707963267949;
3469 IkReal gconst18=x1191;
3470 IkReal gconst19=(((x1187*x1189))+((x1186*x1190)));
3471 IkReal gconst20=(((x1186*x1189))+(((-1.0)*x1187*x1190)));
3472 IkReal x1192=cj3*cj3;
3473 IkReal x1193=(cj3*sj3);
3474 j1eval[0]=((8.3744986631016)+cj3+(((-1.86497326203209)*x1193))+(((-6.37040441176471)*x1192))+(((-6.975)*sj3)));
3475 j1eval[1]=
IKsign(((2505650.0)+(((299200.0)*cj3))+(((-558000.0)*x1193))+(((-1906025.0)*x1192))+(((-2086920.0)*sj3))));
3476 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
3480 IkReal x1194=
IKcos(pz);
3481 IkReal x1195=
IKsin(pz);
3483 IkReal x1197=
IKcos(x1196);
3484 IkReal x1198=
IKsin(x1196);
3485 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3487 IkReal x1199=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
3490 j2=-1.5707963267949;
3494 IkReal gconst18=x1199;
3495 IkReal gconst19=(((x1194*x1198))+((x1195*x1197)));
3496 IkReal gconst20=((((-1.0)*x1195*x1198))+((x1194*x1197)));
3498 IkReal x1201=((px*px)+(((-1.0)*pp))+(pz*pz));
3499 if((x1201) < -0.00001)
3501 IkReal x1202=
IKsqrt(x1201);
3503 j1eval[0]=((((-18.1818181818182)*cj3*x1203))+py+(((-68.0)*x1203))+(((126.818181818182)*sj3*x1203)));
3504 j1eval[1]=
IKsign(((((1395.0)*sj3*x1203))+(((-748.0)*x1203))+(((-200.0)*cj3*x1203))+(((11.0)*py))));
3505 if((((-1.0)*x1200)) < -0.00001)
3507 j1eval[2]=((
IKabs(((-2505.529)+(((1000.0)*x1200))+(((558.0)*cj3*sj3))+(((-299.2)*cj3))+(((1906.025)*(cj3*cj3)))+(((2086.92)*sj3)))))+(
IKabs(((8.228)+(((2.2)*cj3))+(((-1000.0)*py*(
IKsqrt(((-1.0)*x1200)))))+(((-15.345)*sj3))))));
3508 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3512 IkReal x1204=
IKcos(pz);
3513 IkReal x1205=
IKsin(pz);
3515 IkReal x1207=
IKcos(x1206);
3516 IkReal x1208=
IKsin(x1206);
3517 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3519 IkReal x1209=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
3522 j2=-1.5707963267949;
3526 IkReal gconst18=x1209;
3527 IkReal gconst19=(((x1204*x1208))+((x1205*x1207)));
3528 IkReal gconst20=(((x1204*x1207))+(((-1.0)*x1205*x1208)));
3530 IkReal x1211=((px*px)+(((-1.0)*pp))+(pz*pz));
3531 if((x1211) < -0.00001)
3533 IkReal x1212=
IKsqrt(x1211);
3535 j1eval[0]=((((-18.1818181818182)*cj3*x1213))+(((126.818181818182)*sj3*x1213))+py+(((-68.0)*x1213)));
3536 if((((-1.0)*x1210)) < -0.00001)
3538 j1eval[1]=((
IKabs(((-468533.923)+(((-55950.4)*cj3))+(((187000.0)*x1210))+(((104346.0)*cj3*sj3))+(((390254.04)*sj3))+(((356426.675)*(cj3*cj3))))))+(
IKabs(((1538.636)+(((-2869.515)*sj3))+(((-187000.0)*py*(
IKsqrt(((-1.0)*x1210)))))+(((411.4)*cj3))))));
3539 j1eval[2]=
IKsign(((((-37400.0)*cj3*x1213))+(((2057.0)*py))+(((-139876.0)*x1213))+(((260865.0)*sj3*x1213))));
3540 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3547 IkReal j1array[1], cj1array[1], sj1array[1];
3548 bool j1valid[1]={
false};
3550 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3552 IkReal x1214=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3557 CheckValue<IkReal> x1216 =
IKatan2WithCheck(IkReal(((-468533.923)+(((-55950.4)*cj3))+(((104346.0)*cj3*sj3))+(((390254.04)*sj3))+(((356426.675)*(cj3*cj3)))+(((187000.0)*(py*py))))),IkReal(((1538.636)+(((-187000.0)*py*x1214))+(((-2869.515)*sj3))+(((411.4)*cj3)))),
IKFAST_ATAN2_MAGTHRESH);
3561 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1215.
value)))+(x1216.
value));
3562 sj1array[0]=
IKsin(j1array[0]);
3563 cj1array[0]=
IKcos(j1array[0]);
3564 if( j1array[0] >
IKPI )
3568 else if( j1array[0] < -
IKPI )
3569 { j1array[0]+=
IK2PI;
3572 for(
int ij1 = 0; ij1 < 1; ++ij1)
3578 _ij1[0] = ij1; _ij1[1] = -1;
3579 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3583 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3586 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3589 IkReal x1217=
IKcos(j1);
3590 IkReal x1218=
IKsin(j1);
3591 IkReal x1219=((0.2)*cj3);
3592 IkReal x1220=((1.395)*sj3);
3593 IkReal x1221=(py*x1217);
3595 IkReal x1223=(x1218*x1222);
3596 evalcond[0]=((0.011)+((x1217*x1222))+(((-1.0)*py*x1218)));
3597 evalcond[1]=(((x1217*x1220))+(((-0.748)*x1217))+(((-1.0)*x1217*x1219))+py+(((-0.011)*x1218)));
3598 evalcond[2]=((-0.748)+x1223+x1221+x1220+(((-1.0)*x1219)));
3599 evalcond[3]=((-1.119008)+(((1.496)*x1223))+(((1.496)*x1221))+(((2.08692)*sj3))+(((-0.2992)*cj3)));
3600 evalcond[4]=(x1222+(((-0.748)*x1218))+(((-1.0)*x1218*x1219))+((x1218*x1220))+(((0.011)*x1217)));
3607 rotationfunction0(solutions);
3618 IkReal j1array[1], cj1array[1], sj1array[1];
3619 bool j1valid[1]={
false};
3621 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3623 IkReal x1224=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3632 j1array[0]=((-1.5707963267949)+(x1225.
value)+(((1.5707963267949)*(x1226.
value))));
3633 sj1array[0]=
IKsin(j1array[0]);
3634 cj1array[0]=
IKcos(j1array[0]);
3635 if( j1array[0] >
IKPI )
3639 else if( j1array[0] < -
IKPI )
3640 { j1array[0]+=
IK2PI;
3643 for(
int ij1 = 0; ij1 < 1; ++ij1)
3649 _ij1[0] = ij1; _ij1[1] = -1;
3650 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3654 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3657 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3660 IkReal x1227=
IKcos(j1);
3661 IkReal x1228=
IKsin(j1);
3662 IkReal x1229=((0.2)*cj3);
3663 IkReal x1230=((1.395)*sj3);
3664 IkReal x1231=(py*x1227);
3666 IkReal x1233=(x1228*x1232);
3667 evalcond[0]=((0.011)+(((-1.0)*py*x1228))+((x1227*x1232)));
3668 evalcond[1]=((((-0.748)*x1227))+(((-1.0)*x1227*x1229))+(((-0.011)*x1228))+py+((x1227*x1230)));
3669 evalcond[2]=((-0.748)+x1230+x1231+x1233+(((-1.0)*x1229)));
3670 evalcond[3]=((-1.119008)+(((1.496)*x1231))+(((1.496)*x1233))+(((2.08692)*sj3))+(((-0.2992)*cj3)));
3671 evalcond[4]=(x1232+(((-0.748)*x1228))+(((-1.0)*x1228*x1229))+((x1228*x1230))+(((0.011)*x1227)));
3678 rotationfunction0(solutions);
3689 IkReal j1array[1], cj1array[1], sj1array[1];
3690 bool j1valid[1]={
false};
3692 IkReal x1234=((200000.0)*cj3);
3693 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3695 IkReal x1235=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3704 j1array[0]=((-1.5707963267949)+(x1236.
value)+(((1.5707963267949)*(x1237.
value))));
3705 sj1array[0]=
IKsin(j1array[0]);
3706 cj1array[0]=
IKcos(j1array[0]);
3707 if( j1array[0] >
IKPI )
3711 else if( j1array[0] < -
IKPI )
3712 { j1array[0]+=
IK2PI;
3715 for(
int ij1 = 0; ij1 < 1; ++ij1)
3721 _ij1[0] = ij1; _ij1[1] = -1;
3722 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3726 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3729 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3732 IkReal x1238=
IKcos(j1);
3733 IkReal x1239=
IKsin(j1);
3734 IkReal x1240=((0.2)*cj3);
3735 IkReal x1241=((1.395)*sj3);
3736 IkReal x1242=(py*x1238);
3738 IkReal x1244=(x1239*x1243);
3739 evalcond[0]=((0.011)+(((-1.0)*py*x1239))+((x1238*x1243)));
3740 evalcond[1]=((((-0.748)*x1238))+(((-0.011)*x1239))+(((-1.0)*x1238*x1240))+py+((x1238*x1241)));
3741 evalcond[2]=((-0.748)+x1241+x1242+x1244+(((-1.0)*x1240)));
3742 evalcond[3]=((-1.119008)+(((2.08692)*sj3))+(((-0.2992)*cj3))+(((1.496)*x1242))+(((1.496)*x1244)));
3743 evalcond[4]=((((-1.0)*x1239*x1240))+x1243+((x1239*x1241))+(((-0.748)*x1239))+(((0.011)*x1238)));
3750 rotationfunction0(solutions);
3760 if( bgotonextstatement )
3762 bool bgotonextstatement =
true;
3765 IkReal x1245=
IKcos(pz);
3766 IkReal x1246=
IKsin(pz);
3767 if((((-1.0)*(py*py))) < -0.00001)
3769 IkReal x1247=
IKsqrt(((-1.0)*(py*py)));
3770 IkReal x1248=
IKcos(x1247);
3771 IkReal x1249=
IKsin(x1247);
3772 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3774 IkReal gconst21=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
3775 IkReal gconst22=((((-1.0)*x1245*x1249))+((x1246*x1248)));
3776 IkReal gconst23=(((x1246*x1249))+((x1245*x1248)));
3777 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3779 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
3780 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3782 bgotonextstatement=
false;
3785 IkReal x1250=
IKcos(pz);
3786 IkReal x1251=
IKsin(pz);
3788 IkReal x1253=
IKcos(x1252);
3789 IkReal x1254=
IKsin(x1252);
3790 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3792 IkReal x1255=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
3795 j2=-1.5707963267949;
3799 IkReal gconst21=x1255;
3800 IkReal gconst22=(((x1251*x1253))+(((-1.0)*x1250*x1254)));
3801 IkReal gconst23=(((x1250*x1253))+((x1251*x1254)));
3802 IkReal x1256=cj3*cj3;
3803 IkReal x1257=(cj3*sj3);
3804 j1eval[0]=((8.3744986631016)+(((-1.86497326203209)*x1257))+cj3+(((-6.975)*sj3))+(((-6.37040441176471)*x1256)));
3805 j1eval[1]=
IKsign(((2505650.0)+(((299200.0)*cj3))+(((-558000.0)*x1257))+(((-1906025.0)*x1256))+(((-2086920.0)*sj3))));
3806 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
3810 IkReal x1258=
IKcos(pz);
3811 IkReal x1259=
IKsin(pz);
3813 IkReal x1261=
IKcos(x1260);
3814 IkReal x1262=
IKsin(x1260);
3815 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3817 IkReal x1263=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
3820 j2=-1.5707963267949;
3824 IkReal gconst21=x1263;
3825 IkReal gconst22=(((x1259*x1261))+(((-1.0)*x1258*x1262)));
3826 IkReal gconst23=(((x1258*x1261))+((x1259*x1262)));
3828 IkReal x1265=((px*px)+(((-1.0)*pp))+(pz*pz));
3829 if((x1265) < -0.00001)
3831 IkReal x1266=
IKsqrt(x1265);
3833 j1eval[0]=((((68.0)*x1267))+(((18.1818181818182)*cj3*x1267))+py+(((-126.818181818182)*sj3*x1267)));
3834 if((((-1.0)*x1264)) < -0.00001)
3836 j1eval[1]=((
IKabs(((-2505.529)+(((558.0)*cj3*sj3))+(((1000.0)*x1264))+(((-299.2)*cj3))+(((1906.025)*(cj3*cj3)))+(((2086.92)*sj3)))))+(
IKabs(((8.228)+(((2.2)*cj3))+(((1000.0)*py*(
IKsqrt(((-1.0)*x1264)))))+(((-15.345)*sj3))))));
3837 j1eval[2]=
IKsign(((((200.0)*cj3*x1267))+(((748.0)*x1267))+(((11.0)*py))+(((-1395.0)*sj3*x1267))));
3838 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3842 IkReal x1268=
IKcos(pz);
3843 IkReal x1269=
IKsin(pz);
3845 IkReal x1271=
IKcos(x1270);
3846 IkReal x1272=
IKsin(x1270);
3847 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3849 IkReal x1273=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
3852 j2=-1.5707963267949;
3856 IkReal gconst21=x1273;
3857 IkReal gconst22=(((x1269*x1271))+(((-1.0)*x1268*x1272)));
3858 IkReal gconst23=(((x1269*x1272))+((x1268*x1271)));
3860 IkReal x1275=((px*px)+(((-1.0)*pp))+(pz*pz));
3861 if((x1275) < -0.00001)
3863 IkReal x1276=
IKsqrt(x1275);
3865 j1eval[0]=((((68.0)*x1277))+py+(((18.1818181818182)*cj3*x1277))+(((-126.818181818182)*sj3*x1277)));
3866 j1eval[1]=
IKsign(((((2057.0)*py))+(((-260865.0)*sj3*x1277))+(((37400.0)*cj3*x1277))+(((139876.0)*x1277))));
3867 if((((-1.0)*x1274)) < -0.00001)
3869 j1eval[2]=((
IKabs(((-468533.923)+(((-55950.4)*cj3))+(((187000.0)*x1274))+(((104346.0)*cj3*sj3))+(((390254.04)*sj3))+(((356426.675)*(cj3*cj3))))))+(
IKabs(((1538.636)+(((187000.0)*py*(
IKsqrt(((-1.0)*x1274)))))+(((-2869.515)*sj3))+(((411.4)*cj3))))));
3870 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
3877 IkReal j1array[1], cj1array[1], sj1array[1];
3878 bool j1valid[1]={
false};
3880 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3882 IkReal x1278=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3883 CheckValue<IkReal> x1279 =
IKatan2WithCheck(IkReal(((-468533.923)+(((-55950.4)*cj3))+(((104346.0)*cj3*sj3))+(((390254.04)*sj3))+(((356426.675)*(cj3*cj3)))+(((187000.0)*(py*py))))),IkReal(((1538.636)+(((187000.0)*py*x1278))+(((-2869.515)*sj3))+(((411.4)*cj3)))),
IKFAST_ATAN2_MAGTHRESH);
3891 j1array[0]=((-1.5707963267949)+(x1279.
value)+(((1.5707963267949)*(x1280.
value))));
3892 sj1array[0]=
IKsin(j1array[0]);
3893 cj1array[0]=
IKcos(j1array[0]);
3894 if( j1array[0] >
IKPI )
3898 else if( j1array[0] < -
IKPI )
3899 { j1array[0]+=
IK2PI;
3902 for(
int ij1 = 0; ij1 < 1; ++ij1)
3908 _ij1[0] = ij1; _ij1[1] = -1;
3909 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3913 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3916 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3919 IkReal x1281=
IKcos(j1);
3920 IkReal x1282=
IKsin(j1);
3921 IkReal x1283=((0.2)*cj3);
3922 IkReal x1284=((1.395)*sj3);
3923 IkReal x1285=(py*x1281);
3925 IkReal x1287=((1.0)*x1286);
3926 evalcond[0]=((((-0.748)*x1281))+py+((x1281*x1284))+(((-0.011)*x1282))+(((-1.0)*x1281*x1283)));
3927 evalcond[1]=((0.011)+(((-1.0)*py*x1282))+(((-1.0)*x1281*x1287)));
3928 evalcond[2]=((-0.748)+x1285+x1284+(((-1.0)*x1283))+(((-1.0)*x1282*x1287)));
3929 evalcond[3]=((-1.119008)+(((-1.496)*x1282*x1286))+(((2.08692)*sj3))+(((-0.2992)*cj3))+(((1.496)*x1285)));
3930 evalcond[4]=((((-0.748)*x1282))+(((-1.0)*x1287))+((x1282*x1284))+(((0.011)*x1281))+(((-1.0)*x1282*x1283)));
3937 rotationfunction0(solutions);
3948 IkReal j1array[1], cj1array[1], sj1array[1];
3949 bool j1valid[1]={
false};
3951 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
3953 IkReal x1288=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
3962 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1289.
value)))+(x1290.
value));
3963 sj1array[0]=
IKsin(j1array[0]);
3964 cj1array[0]=
IKcos(j1array[0]);
3965 if( j1array[0] >
IKPI )
3969 else if( j1array[0] < -
IKPI )
3970 { j1array[0]+=
IK2PI;
3973 for(
int ij1 = 0; ij1 < 1; ++ij1)
3979 _ij1[0] = ij1; _ij1[1] = -1;
3980 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3984 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3987 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3990 IkReal x1291=
IKcos(j1);
3991 IkReal x1292=
IKsin(j1);
3992 IkReal x1293=((0.2)*cj3);
3993 IkReal x1294=((1.395)*sj3);
3994 IkReal x1295=(py*x1291);
3996 IkReal x1297=((1.0)*x1296);
3997 evalcond[0]=(((x1291*x1294))+(((-0.011)*x1292))+py+(((-0.748)*x1291))+(((-1.0)*x1291*x1293)));
3998 evalcond[1]=((0.011)+(((-1.0)*py*x1292))+(((-1.0)*x1291*x1297)));
3999 evalcond[2]=((-0.748)+x1294+x1295+(((-1.0)*x1293))+(((-1.0)*x1292*x1297)));
4000 evalcond[3]=((-1.119008)+(((-1.496)*x1292*x1296))+(((2.08692)*sj3))+(((1.496)*x1295))+(((-0.2992)*cj3)));
4001 evalcond[4]=((((-1.0)*x1297))+(((0.011)*x1291))+(((-1.0)*x1292*x1293))+(((-0.748)*x1292))+((x1292*x1294)));
4008 rotationfunction0(solutions);
4019 IkReal j1array[1], cj1array[1], sj1array[1];
4020 bool j1valid[1]={
false};
4022 IkReal x1298=((200000.0)*cj3);
4023 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
4025 IkReal x1299=
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)));
4034 j1array[0]=((-1.5707963267949)+(x1300.
value)+(((1.5707963267949)*(x1301.
value))));
4035 sj1array[0]=
IKsin(j1array[0]);
4036 cj1array[0]=
IKcos(j1array[0]);
4037 if( j1array[0] >
IKPI )
4041 else if( j1array[0] < -
IKPI )
4042 { j1array[0]+=
IK2PI;
4045 for(
int ij1 = 0; ij1 < 1; ++ij1)
4051 _ij1[0] = ij1; _ij1[1] = -1;
4052 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4056 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4059 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4062 IkReal x1302=
IKcos(j1);
4063 IkReal x1303=
IKsin(j1);
4064 IkReal x1304=((0.2)*cj3);
4065 IkReal x1305=((1.395)*sj3);
4066 IkReal x1306=(py*x1302);
4068 IkReal x1308=((1.0)*x1307);
4069 evalcond[0]=((((-0.011)*x1303))+((x1302*x1305))+py+(((-0.748)*x1302))+(((-1.0)*x1302*x1304)));
4070 evalcond[1]=((0.011)+(((-1.0)*py*x1303))+(((-1.0)*x1302*x1308)));
4071 evalcond[2]=((-0.748)+x1305+x1306+(((-1.0)*x1304))+(((-1.0)*x1303*x1308)));
4072 evalcond[3]=((-1.119008)+(((-1.496)*x1303*x1307))+(((2.08692)*sj3))+(((-0.2992)*cj3))+(((1.496)*x1306)));
4073 evalcond[4]=((((-1.0)*x1308))+(((-1.0)*x1303*x1304))+((x1303*x1305))+(((-0.748)*x1303))+(((0.011)*x1302)));
4080 rotationfunction0(solutions);
4090 if( bgotonextstatement )
4092 bool bgotonextstatement =
true;
4097 bgotonextstatement=
false;
4102 if( bgotonextstatement )
4112 IkReal j1array[1], cj1array[1], sj1array[1];
4113 bool j1valid[1]={
false};
4115 IkReal x1309=(py*sj3);
4116 IkReal x1310=(cj3*py);
4117 IkReal x1311=((1000.0)*py);
4128 j1array[0]=
IKatan2(((x1312.
value)*(((8.228)+(((2.2)*cj3))+((j0*x1311))+(((-1.0)*pz*x1311))+(((-15.345)*sj3))))), ((x1313.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
4129 sj1array[0]=
IKsin(j1array[0]);
4130 cj1array[0]=
IKcos(j1array[0]);
4131 if( j1array[0] >
IKPI )
4135 else if( j1array[0] < -
IKPI )
4136 { j1array[0]+=
IK2PI;
4139 for(
int ij1 = 0; ij1 < 1; ++ij1)
4145 _ij1[0] = ij1; _ij1[1] = -1;
4146 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4150 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4153 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4156 IkReal x1314=
IKcos(j1);
4157 IkReal x1315=
IKsin(j1);
4158 IkReal x1316=((0.2)*cj3);
4159 IkReal x1317=((1.0)*pz);
4160 IkReal x1318=((1.395)*sj3);
4161 IkReal x1319=((1.496)*x1315);
4162 IkReal x1320=(py*x1314);
4163 evalcond[0]=((0.011)+((j0*x1314))+(((-1.0)*x1314*x1317))+(((-1.0)*py*x1315)));
4164 evalcond[1]=((-0.748)+x1318+x1320+(((-1.0)*x1315*x1317))+((j0*x1315))+(((-1.0)*x1316)));
4165 evalcond[2]=(((x1314*x1318))+(((-0.011)*x1315))+(((-1.0)*x1314*x1316))+(((-0.748)*x1314))+py);
4166 evalcond[3]=((-1.119008)+((j0*x1319))+(((2.08692)*sj3))+(((-0.2992)*cj3))+(((-1.0)*pz*x1319))+(((1.496)*x1320)));
4167 evalcond[4]=((((-1.0)*x1315*x1316))+(((-1.0)*x1317))+(((-0.748)*x1315))+(((0.011)*x1314))+((x1315*x1318))+j0);
4174 rotationfunction0(solutions);
4185 IkReal j1array[1], cj1array[1], sj1array[1];
4186 bool j1valid[1]={
false};
4188 IkReal x1321=((200.0)*cj3);
4189 IkReal x1322=((1000.0)*py);
4190 IkReal x1323=((1395.0)*sj3);
4199 j1array[0]=((-1.5707963267949)+(x1324.
value)+(((1.5707963267949)*(x1325.
value))));
4200 sj1array[0]=
IKsin(j1array[0]);
4201 cj1array[0]=
IKcos(j1array[0]);
4202 if( j1array[0] >
IKPI )
4206 else if( j1array[0] < -
IKPI )
4207 { j1array[0]+=
IK2PI;
4210 for(
int ij1 = 0; ij1 < 1; ++ij1)
4216 _ij1[0] = ij1; _ij1[1] = -1;
4217 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4221 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4224 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4227 IkReal x1326=
IKcos(j1);
4228 IkReal x1327=
IKsin(j1);
4229 IkReal x1328=((0.2)*cj3);
4230 IkReal x1329=((1.0)*pz);
4231 IkReal x1330=((1.395)*sj3);
4232 IkReal x1331=((1.496)*x1327);
4233 IkReal x1332=(py*x1326);
4234 evalcond[0]=((0.011)+((j0*x1326))+(((-1.0)*py*x1327))+(((-1.0)*x1326*x1329)));
4235 evalcond[1]=((-0.748)+x1330+x1332+((j0*x1327))+(((-1.0)*x1328))+(((-1.0)*x1327*x1329)));
4236 evalcond[2]=((((-0.011)*x1327))+(((-1.0)*x1326*x1328))+(((-0.748)*x1326))+((x1326*x1330))+py);
4237 evalcond[3]=((-1.119008)+(((1.496)*x1332))+(((2.08692)*sj3))+(((-1.0)*pz*x1331))+((j0*x1331))+(((-0.2992)*cj3)));
4238 evalcond[4]=((((-1.0)*x1329))+(((-1.0)*x1327*x1328))+(((-0.748)*x1327))+((x1327*x1330))+(((0.011)*x1326))+j0);
4245 rotationfunction0(solutions);
4256 IkReal j1array[1], cj1array[1], sj1array[1];
4257 bool j1valid[1]={
false};
4259 IkReal x1333=((200.0)*cj3);
4260 IkReal x1334=((1395.0)*sj3);
4265 CheckValue<IkReal> x1336 =
IKatan2WithCheck(IkReal(((((-1.0)*j0*x1334))+(((748.0)*j0))+((pz*x1334))+(((-1.0)*pz*x1333))+((j0*x1333))+(((-748.0)*pz))+(((11.0)*py)))),IkReal(((((-11.0)*j0))+(((-1.0)*py*x1334))+((py*x1333))+(((11.0)*pz))+(((748.0)*py)))),
IKFAST_ATAN2_MAGTHRESH);
4269 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1335.
value)))+(x1336.
value));
4270 sj1array[0]=
IKsin(j1array[0]);
4271 cj1array[0]=
IKcos(j1array[0]);
4272 if( j1array[0] >
IKPI )
4276 else if( j1array[0] < -
IKPI )
4277 { j1array[0]+=
IK2PI;
4280 for(
int ij1 = 0; ij1 < 1; ++ij1)
4286 _ij1[0] = ij1; _ij1[1] = -1;
4287 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4291 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4294 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4297 IkReal x1337=
IKcos(j1);
4298 IkReal x1338=
IKsin(j1);
4299 IkReal x1339=((0.2)*cj3);
4300 IkReal x1340=((1.0)*pz);
4301 IkReal x1341=((1.395)*sj3);
4302 IkReal x1342=((1.496)*x1338);
4303 IkReal x1343=(py*x1337);
4304 evalcond[0]=((0.011)+((j0*x1337))+(((-1.0)*py*x1338))+(((-1.0)*x1337*x1340)));
4305 evalcond[1]=((-0.748)+x1341+x1343+(((-1.0)*x1338*x1340))+((j0*x1338))+(((-1.0)*x1339)));
4306 evalcond[2]=((((-0.011)*x1338))+(((-1.0)*x1337*x1339))+((x1337*x1341))+py+(((-0.748)*x1337)));
4307 evalcond[3]=((-1.119008)+(((-1.0)*pz*x1342))+(((2.08692)*sj3))+(((1.496)*x1343))+(((-0.2992)*cj3))+((j0*x1342)));
4308 evalcond[4]=((((-1.0)*x1340))+(((0.011)*x1337))+((x1338*x1341))+(((-1.0)*x1338*x1339))+j0+(((-0.748)*x1338)));
4315 rotationfunction0(solutions);
4325 if( bgotonextstatement )
4327 bool bgotonextstatement =
true;
4332 bgotonextstatement=
false;
4337 if( bgotonextstatement )
4349 IkReal j1array[1], cj1array[1], sj1array[1];
4350 bool j1valid[1]={
false};
4352 IkReal x1344=(cj2*cj3);
4353 IkReal x1345=(cj3*sj2);
4354 IkReal x1346=((200.0)*py);
4355 IkReal x1347=(py*sj2);
4356 IkReal x1348=(cj2*sj3);
4357 IkReal x1349=((1000.0)*py);
4358 IkReal x1350=((200000.0)*py);
4359 CheckValue<IkReal> x1351=
IKPowWithIntegerCheck(((((-322.0)*py))+(((11.0)*j0))+(((1395.0)*sj3*x1347))+(((-1070.0)*x1347))+(((-1.0)*x1346*x1348))+(((-11.0)*pz))+(((-1395.0)*py*x1344))+(((-1.0)*x1345*x1346))),-1);
4363 CheckValue<IkReal> x1352=
IKPowWithIntegerCheck(((((1395000.0)*sj3*x1347))+(((-1.0)*x1345*x1350))+(((-1395000.0)*py*x1344))+(((11000.0)*j0))+(((-1.0)*x1348*x1350))+(((-322000.0)*py))+(((-1070000.0)*x1347))+(((-11000.0)*pz))),-1);
4369 j1array[0]=
IKatan2(((x1351.
value)*(((-3.542)+(((-15.345)*x1344))+(((-1.0)*pz*x1349))+(((15.345)*sj2*sj3))+(((-2.2)*x1348))+(((-2.2)*x1345))+(((-11.77)*sj2))+((j0*x1349))))), ((x1352.
value)*(((-121.0)+(((1000000.0)*(py*py)))))));
4370 sj1array[0]=
IKsin(j1array[0]);
4371 cj1array[0]=
IKcos(j1array[0]);
4372 if( j1array[0] >
IKPI )
4376 else if( j1array[0] < -
IKPI )
4377 { j1array[0]+=
IK2PI;
4380 for(
int ij1 = 0; ij1 < 1; ++ij1)
4386 _ij1[0] = ij1; _ij1[1] = -1;
4387 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4391 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4394 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4397 IkReal x1353=
IKcos(j1);
4398 IkReal x1354=
IKsin(j1);
4399 IkReal x1355=((0.2)*cj3);
4400 IkReal x1356=((1.395)*cj3);
4401 IkReal x1357=((1.395)*sj3);
4402 IkReal x1358=((1.0)*pz);
4403 IkReal x1359=((0.2)*sj3);
4404 IkReal x1360=(cj2*px);
4405 IkReal x1361=((1.0)*py);
4406 IkReal x1362=(sj2*x1354);
4407 IkReal x1363=(cj2*x1354);
4408 IkReal x1364=(sj2*x1353);
4409 IkReal x1365=((0.644)*x1354);
4410 IkReal x1366=(cj2*x1353);
4411 evalcond[0]=((0.011)+(((-1.0)*x1354*x1361))+((j0*x1353))+(((-1.0)*x1353*x1358)));
4412 evalcond[1]=(x1359+x1356+(((-0.551)*sj2))+(((0.322)*cj2))+((j0*x1363))+(((-1.0)*px*sj2))+((py*x1366))+(((-1.0)*x1358*x1363)));
4413 evalcond[2]=((-1.07)+x1357+(((-0.322)*sj2))+(((-1.0)*x1355))+(((-1.0)*j0*x1362))+(((-1.0)*x1360))+((pz*x1362))+(((-1.0)*x1361*x1364))+(((-0.551)*cj2)));
4414 evalcond[3]=(((x1356*x1366))+((x1355*x1364))+(((1.07)*x1364))+((x1359*x1366))+py+(((-1.0)*x1357*x1364))+(((0.322)*x1353))+(((-0.011)*x1354)));
4415 evalcond[4]=(((x1356*x1363))+((x1355*x1362))+(((-1.0)*x1358))+(((1.07)*x1362))+((x1359*x1363))+(((-1.0)*x1357*x1362))+(((0.322)*x1354))+j0+(((0.011)*x1353)));
4416 evalcond[5]=((0.433961)+(((2.14)*pz*x1362))+(((-2.14)*j0*x1362))+(((-2.14)*py*x1364))+(((-1.102)*px))+(((-2.14)*x1360))+(((-1.0)*(j0*j0)))+(((-0.644)*py*x1353))+(((-1.0)*pp))+(((2.0)*j0*pz))+(((-1.17914)*cj2))+(((-1.0)*j0*x1365))+(((-0.68908)*sj2))+((pz*x1365)));
4423 rotationfunction0(solutions);
4434 IkReal j1array[1], cj1array[1], sj1array[1];
4435 bool j1valid[1]={
false};
4437 IkReal x1367=((11.0)*sj2);
4438 IkReal x1368=((200.0)*cj3);
4439 IkReal x1369=((1395.0)*sj3);
4440 IkReal x1370=((551.0)*cj2);
4441 IkReal x1371=((322.0)*sj2);
4442 IkReal x1372=((1000.0)*sj2);
4443 IkReal x1373=((1000.0)*cj2*px);
4444 CheckValue<IkReal> x1374 =
IKatan2WithCheck(IkReal(((((-1.0)*pz*x1369))+((pz*x1373))+((pz*x1370))+((pz*x1371))+(((-1070.0)*j0))+(((-1.0)*j0*x1373))+(((-1.0)*j0*x1370))+(((-1.0)*j0*x1371))+(((-1.0)*j0*x1368))+((j0*x1369))+((py*x1367))+((pz*x1368))+(((1070.0)*pz)))),IkReal(((((-1070.0)*py))+(((-1.0)*j0*x1367))+((py*x1369))+((pz*x1367))+(((-1.0)*py*x1371))+(((-1.0)*py*x1370))+(((-1.0)*py*x1373))+(((-1.0)*py*x1368)))),
IKFAST_ATAN2_MAGTHRESH);
4452 j1array[0]=((-1.5707963267949)+(x1374.
value)+(((1.5707963267949)*(x1375.
value))));
4453 sj1array[0]=
IKsin(j1array[0]);
4454 cj1array[0]=
IKcos(j1array[0]);
4455 if( j1array[0] >
IKPI )
4459 else if( j1array[0] < -
IKPI )
4460 { j1array[0]+=
IK2PI;
4463 for(
int ij1 = 0; ij1 < 1; ++ij1)
4469 _ij1[0] = ij1; _ij1[1] = -1;
4470 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4474 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4477 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4480 IkReal x1376=
IKcos(j1);
4481 IkReal x1377=
IKsin(j1);
4482 IkReal x1378=((0.2)*cj3);
4483 IkReal x1379=((1.395)*cj3);
4484 IkReal x1380=((1.395)*sj3);
4485 IkReal x1381=((1.0)*pz);
4486 IkReal x1382=((0.2)*sj3);
4487 IkReal x1383=(cj2*px);
4488 IkReal x1384=((1.0)*py);
4489 IkReal x1385=(sj2*x1377);
4490 IkReal x1386=(cj2*x1377);
4491 IkReal x1387=(sj2*x1376);
4492 IkReal x1388=((0.644)*x1377);
4493 IkReal x1389=(cj2*x1376);
4494 evalcond[0]=((0.011)+((j0*x1376))+(((-1.0)*x1377*x1384))+(((-1.0)*x1376*x1381)));
4495 evalcond[1]=(x1379+x1382+(((-0.551)*sj2))+(((0.322)*cj2))+((py*x1389))+(((-1.0)*x1381*x1386))+(((-1.0)*px*sj2))+((j0*x1386)));
4496 evalcond[2]=((-1.07)+x1380+(((-0.322)*sj2))+(((-1.0)*x1384*x1387))+((pz*x1385))+(((-1.0)*x1378))+(((-1.0)*x1383))+(((-1.0)*j0*x1385))+(((-0.551)*cj2)));
4497 evalcond[3]=((((-0.011)*x1377))+(((0.322)*x1376))+(((1.07)*x1387))+py+(((-1.0)*x1380*x1387))+((x1379*x1389))+((x1378*x1387))+((x1382*x1389)));
4498 evalcond[4]=((((-1.0)*x1381))+(((0.322)*x1377))+(((1.07)*x1385))+(((0.011)*x1376))+(((-1.0)*x1380*x1385))+((x1379*x1386))+((x1378*x1385))+j0+((x1382*x1386)));
4499 evalcond[5]=((0.433961)+(((2.14)*pz*x1385))+(((-2.14)*x1383))+(((-1.102)*px))+(((-1.0)*(j0*j0)))+((pz*x1388))+(((-2.14)*j0*x1385))+(((-1.0)*pp))+(((2.0)*j0*pz))+(((-0.644)*py*x1376))+(((-2.14)*py*x1387))+(((-1.17914)*cj2))+(((-1.0)*j0*x1388))+(((-0.68908)*sj2)));
4506 rotationfunction0(solutions);
4517 IkReal j1array[1], cj1array[1], sj1array[1];
4518 bool j1valid[1]={
false};
4520 IkReal x1390=((1395.0)*cj3);
4521 IkReal x1391=((11.0)*cj2);
4522 IkReal x1392=((322.0)*cj2);
4523 IkReal x1393=((200.0)*sj3);
4524 IkReal x1394=((551.0)*sj2);
4525 IkReal x1395=((1000.0)*cj2);
4526 IkReal x1396=((1000.0)*px*sj2);
4527 CheckValue<IkReal> x1397 =
IKatan2WithCheck(IkReal((((py*x1391))+((pz*x1390))+((pz*x1392))+((pz*x1393))+(((-1.0)*j0*x1390))+(((-1.0)*j0*x1392))+(((-1.0)*j0*x1393))+((j0*x1396))+((j0*x1394))+(((-1.0)*pz*x1394))+(((-1.0)*pz*x1396)))),IkReal((((py*x1396))+((py*x1394))+((pz*x1391))+(((-1.0)*j0*x1391))+(((-1.0)*py*x1392))+(((-1.0)*py*x1393))+(((-1.0)*py*x1390)))),
IKFAST_ATAN2_MAGTHRESH);
4535 j1array[0]=((-1.5707963267949)+(x1397.
value)+(((1.5707963267949)*(x1398.
value))));
4536 sj1array[0]=
IKsin(j1array[0]);
4537 cj1array[0]=
IKcos(j1array[0]);
4538 if( j1array[0] >
IKPI )
4542 else if( j1array[0] < -
IKPI )
4543 { j1array[0]+=
IK2PI;
4546 for(
int ij1 = 0; ij1 < 1; ++ij1)
4552 _ij1[0] = ij1; _ij1[1] = -1;
4553 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4557 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4560 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4563 IkReal x1399=
IKcos(j1);
4564 IkReal x1400=
IKsin(j1);
4565 IkReal x1401=((0.2)*cj3);
4566 IkReal x1402=((1.395)*cj3);
4567 IkReal x1403=((1.395)*sj3);
4568 IkReal x1404=((1.0)*pz);
4569 IkReal x1405=((0.2)*sj3);
4570 IkReal x1406=(cj2*px);
4571 IkReal x1407=((1.0)*py);
4572 IkReal x1408=(sj2*x1400);
4573 IkReal x1409=(cj2*x1400);
4574 IkReal x1410=(sj2*x1399);
4575 IkReal x1411=((0.644)*x1400);
4576 IkReal x1412=(cj2*x1399);
4577 evalcond[0]=((0.011)+(((-1.0)*x1399*x1404))+(((-1.0)*x1400*x1407))+((j0*x1399)));
4578 evalcond[1]=(((py*x1412))+(((-0.551)*sj2))+(((0.322)*cj2))+(((-1.0)*x1404*x1409))+x1402+x1405+((j0*x1409))+(((-1.0)*px*sj2)));
4579 evalcond[2]=((-1.07)+(((-0.322)*sj2))+(((-1.0)*x1406))+((pz*x1408))+(((-1.0)*x1401))+(((-1.0)*x1407*x1410))+x1403+(((-1.0)*j0*x1408))+(((-0.551)*cj2)));
4580 evalcond[3]=((((-0.011)*x1400))+((x1402*x1412))+((x1401*x1410))+((x1405*x1412))+(((0.322)*x1399))+py+(((-1.0)*x1403*x1410))+(((1.07)*x1410)));
4581 evalcond[4]=((((0.322)*x1400))+(((0.011)*x1399))+(((-1.0)*x1404))+((x1405*x1409))+(((1.07)*x1408))+((x1402*x1409))+(((-1.0)*x1403*x1408))+((x1401*x1408))+j0);
4582 evalcond[5]=((0.433961)+((pz*x1411))+(((-0.644)*py*x1399))+(((-2.14)*x1406))+(((-1.102)*px))+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((-2.14)*py*x1410))+(((2.0)*j0*pz))+(((-1.17914)*cj2))+(((-2.14)*j0*x1408))+(((-1.0)*j0*x1411))+(((-0.68908)*sj2))+(((2.14)*pz*x1408)));
4589 rotationfunction0(solutions);
4608 for(
int rotationiter = 0; rotationiter < 1; ++rotationiter) {
4609 IkReal x126=(cj2*sj3);
4610 IkReal x127=(r20*sj1);
4611 IkReal x128=(cj3*sj2);
4612 IkReal x129=(r22*sj1);
4613 IkReal x130=(r21*sj1);
4614 IkReal x131=((1.0)*cj1*r11);
4615 IkReal x132=((1.0)*cj1*r10);
4616 IkReal x133=((1.0)*cj1*r12);
4617 IkReal x134=(((cj2*cj3))+(((-1.0)*sj2*sj3)));
4618 IkReal x135=(x126+x128);
4619 IkReal x136=((((-1.0)*x126))+(((-1.0)*x128)));
4620 new_r00=(((x127*x136))+((r00*x134))+(((-1.0)*x132*x136)));
4621 new_r01=(((r01*x134))+((x130*x136))+(((-1.0)*x131*x136)));
4622 new_r02=(((x129*x136))+((r02*x134))+(((-1.0)*x133*x136)));
4623 new_r10=(((r10*sj1))+((cj1*r20)));
4624 new_r11=(((r11*sj1))+((cj1*r21)));
4625 new_r12=(((cj1*r22))+((r12*sj1)));
4626 new_r20=(((x127*x134))+((r00*x135))+(((-1.0)*x132*x134)));
4627 new_r21=(((r01*x135))+((x130*x134))+(((-1.0)*x131*x134)));
4628 new_r22=(((x129*x134))+((r02*x135))+(((-1.0)*x133*x134)));
4630 IkReal j5array[2], cj5array[2], sj5array[2];
4631 bool j5valid[2]={
false};
4633 cj5array[0]=new_r22;
4636 j5valid[0] = j5valid[1] =
true;
4637 j5array[0] =
IKacos(cj5array[0]);
4638 sj5array[0] =
IKsin(j5array[0]);
4639 cj5array[1] = cj5array[0];
4640 j5array[1] = -j5array[0];
4641 sj5array[1] = -sj5array[0];
4643 else if( isnan(cj5array[0]) )
4647 cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
4649 for(
int ij5 = 0; ij5 < 2; ++ij5)
4655 _ij5[0] = ij5; _ij5[1] = -1;
4656 for(
int iij5 = ij5+1; iij5 < 2; ++iij5)
4660 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
4663 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4668 j4eval[1]=((
IKabs(new_r12))+(
IKabs(new_r02)));
4670 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 ||
IKabs(j4eval[2]) < 0.0000010000000000 )
4675 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
4677 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
4683 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 )
4687 bool bgotonextstatement =
true;
4690 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
4691 evalcond[1]=new_r20;
4692 evalcond[2]=new_r02;
4693 evalcond[3]=new_r12;
4694 evalcond[4]=new_r21;
4695 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 )
4697 bgotonextstatement=
false;
4703 j4=
IKatan2(((-1.0)*new_r01), new_r00);
4705 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4706 vinfos[0].jointtype = 17;
4707 vinfos[0].foffset = j0;
4708 vinfos[0].indices[0] = _ij0[0];
4709 vinfos[0].indices[1] = _ij0[1];
4710 vinfos[0].maxsolutions = _nj0;
4711 vinfos[1].jointtype = 1;
4712 vinfos[1].foffset = j1;
4713 vinfos[1].indices[0] = _ij1[0];
4714 vinfos[1].indices[1] = _ij1[1];
4715 vinfos[1].maxsolutions = _nj1;
4716 vinfos[2].jointtype = 1;
4717 vinfos[2].foffset = j2;
4718 vinfos[2].indices[0] = _ij2[0];
4719 vinfos[2].indices[1] = _ij2[1];
4720 vinfos[2].maxsolutions = _nj2;
4721 vinfos[3].jointtype = 1;
4722 vinfos[3].foffset = j3;
4723 vinfos[3].indices[0] = _ij3[0];
4724 vinfos[3].indices[1] = _ij3[1];
4725 vinfos[3].maxsolutions = _nj3;
4726 vinfos[4].jointtype = 1;
4727 vinfos[4].foffset = j4;
4728 vinfos[4].fmul = j4mul;
4729 vinfos[4].freeind = 0;
4730 vinfos[4].maxsolutions = 0;
4731 vinfos[5].jointtype = 1;
4732 vinfos[5].foffset = j5;
4733 vinfos[5].indices[0] = _ij5[0];
4734 vinfos[5].indices[1] = _ij5[1];
4735 vinfos[5].maxsolutions = _nj5;
4736 vinfos[6].jointtype = 1;
4737 vinfos[6].foffset = j6;
4738 vinfos[6].fmul = j6mul;
4739 vinfos[6].freeind = 0;
4740 vinfos[6].maxsolutions = 0;
4741 std::vector<int> vfree(1);
4748 if( bgotonextstatement )
4750 bool bgotonextstatement =
true;
4753 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
4754 evalcond[1]=new_r20;
4755 evalcond[2]=new_r02;
4756 evalcond[3]=new_r12;
4757 evalcond[4]=new_r21;
4758 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 )
4760 bgotonextstatement=
false;
4766 j4=
IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
4768 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4769 vinfos[0].jointtype = 17;
4770 vinfos[0].foffset = j0;
4771 vinfos[0].indices[0] = _ij0[0];
4772 vinfos[0].indices[1] = _ij0[1];
4773 vinfos[0].maxsolutions = _nj0;
4774 vinfos[1].jointtype = 1;
4775 vinfos[1].foffset = j1;
4776 vinfos[1].indices[0] = _ij1[0];
4777 vinfos[1].indices[1] = _ij1[1];
4778 vinfos[1].maxsolutions = _nj1;
4779 vinfos[2].jointtype = 1;
4780 vinfos[2].foffset = j2;
4781 vinfos[2].indices[0] = _ij2[0];
4782 vinfos[2].indices[1] = _ij2[1];
4783 vinfos[2].maxsolutions = _nj2;
4784 vinfos[3].jointtype = 1;
4785 vinfos[3].foffset = j3;
4786 vinfos[3].indices[0] = _ij3[0];
4787 vinfos[3].indices[1] = _ij3[1];
4788 vinfos[3].maxsolutions = _nj3;
4789 vinfos[4].jointtype = 1;
4790 vinfos[4].foffset = j4;
4791 vinfos[4].fmul = j4mul;
4792 vinfos[4].freeind = 0;
4793 vinfos[4].maxsolutions = 0;
4794 vinfos[5].jointtype = 1;
4795 vinfos[5].foffset = j5;
4796 vinfos[5].indices[0] = _ij5[0];
4797 vinfos[5].indices[1] = _ij5[1];
4798 vinfos[5].maxsolutions = _nj5;
4799 vinfos[6].jointtype = 1;
4800 vinfos[6].foffset = j6;
4801 vinfos[6].fmul = j6mul;
4802 vinfos[6].freeind = 0;
4803 vinfos[6].maxsolutions = 0;
4804 std::vector<int> vfree(1);
4811 if( bgotonextstatement )
4813 bool bgotonextstatement =
true;
4816 evalcond[0]=((
IKabs(new_r12))+(
IKabs(new_r02)));
4817 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4819 bgotonextstatement=
false;
4826 IkReal x137=new_r22*new_r22;
4827 IkReal x138=((16.0)*new_r10);
4828 IkReal x139=((16.0)*new_r01);
4829 IkReal x140=((16.0)*new_r22);
4830 IkReal x141=((8.0)*new_r11);
4831 IkReal x142=((8.0)*new_r00);
4832 IkReal x143=(x137*x138);
4833 IkReal x144=(x137*x139);
4834 j4eval[0]=((
IKabs((((new_r11*x140))+(((16.0)*new_r00))+(((-32.0)*new_r00*x137)))))+(
IKabs((((x137*x141))+(((-1.0)*new_r22*x142)))))+(
IKabs(((((-1.0)*x143))+x138)))+(
IKabs(((((32.0)*new_r11))+(((-16.0)*new_r11*x137))+(((-1.0)*new_r00*x140)))))+(
IKabs((x143+(((-1.0)*x138)))))+(
IKabs(((((-1.0)*x144))+x139)))+(
IKabs(((((-1.0)*x142))+((new_r22*x141)))))+(
IKabs((x144+(((-1.0)*x139))))));
4835 if(
IKabs(j4eval[0]) < 0.0000000100000000 )
4841 IkReal op[4+1], zeror[4];
4843 IkReal j4evalpoly[1];
4844 IkReal x145=new_r22*new_r22;
4845 IkReal x146=((16.0)*new_r10);
4846 IkReal x147=(new_r11*new_r22);
4847 IkReal x148=(x145*x146);
4848 IkReal x149=((((8.0)*x147))+(((-8.0)*new_r00)));
4850 op[1]=((((-1.0)*x148))+x146);
4851 op[2]=((((16.0)*new_r00))+(((16.0)*x147))+(((-32.0)*new_r00*x145)));
4852 op[3]=((((-1.0)*x146))+x148);
4854 polyroots4(op,zeror,numroots);
4855 IkReal j4array[4], cj4array[4], sj4array[4], tempj4array[1];
4856 int numsolutions = 0;
4857 for(
int ij4 = 0; ij4 < numroots; ++ij4)
4859 IkReal htj4 = zeror[ij4];
4860 tempj4array[0]=((2.0)*(
atan(htj4)));
4861 for(
int kj4 = 0; kj4 < 1; ++kj4)
4863 j4array[numsolutions] = tempj4array[kj4];
4864 if( j4array[numsolutions] >
IKPI )
4866 j4array[numsolutions]-=
IK2PI;
4868 else if( j4array[numsolutions] < -
IKPI )
4870 j4array[numsolutions]+=
IK2PI;
4872 sj4array[numsolutions] =
IKsin(j4array[numsolutions]);
4873 cj4array[numsolutions] =
IKcos(j4array[numsolutions]);
4877 bool j4valid[4]={
true,
true,
true,
true};
4879 for(
int ij4 = 0; ij4 < numsolutions; ++ij4)
4885 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
4888 IkReal x150=new_r22*new_r22;
4889 IkReal x151=((16.0)*new_r01);
4890 IkReal x152=(new_r00*new_r22);
4891 IkReal x153=((8.0)*x152);
4892 IkReal x154=(new_r11*x150);
4893 IkReal x155=(x150*x151);
4894 IkReal x156=((8.0)*x154);
4895 j4evalpoly[0]=((((-1.0)*x153))+(((htj4*htj4*htj4*htj4)*(((((-1.0)*x153))+x156))))+(((htj4*htj4*htj4)*(((((-1.0)*x151))+x155))))+((htj4*(((((-1.0)*x155))+x151))))+(((htj4*htj4)*(((((32.0)*new_r11))+(((-16.0)*x152))+(((-16.0)*x154))))))+x156);
4896 if(
IKabs(j4evalpoly[0]) > 0.0000001000000000 )
4900 _ij4[0] = ij4; _ij4[1] = -1;
4901 for(
int iij4 = ij4+1; iij4 < numsolutions; ++iij4)
4905 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
4914 IkReal x157=cj4*cj4;
4915 IkReal x158=(cj4*new_r22);
4916 IkReal x159=((-1.0)+x157+(((-1.0)*x157*(new_r22*new_r22))));
4918 j6eval[1]=((
IKabs((((new_r01*x158))+((new_r00*sj4)))))+(
IKabs((((new_r01*sj4))+(((-1.0)*new_r00*x158))))));
4920 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
4929 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
4937 IkReal x160=new_r22*new_r22;
4938 j6eval[0]=(((cj4*x160))+(((-1.0)*cj4)));
4939 j6eval[1]=((((-1.0)*sj4))+((sj4*x160)));
4940 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
4944 bool bgotonextstatement =
true;
4947 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
4948 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4950 bgotonextstatement=
false;
4952 IkReal j6array[1], cj6array[1], sj6array[1];
4953 bool j6valid[1]={
false};
4957 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4958 sj6array[0]=
IKsin(j6array[0]);
4959 cj6array[0]=
IKcos(j6array[0]);
4960 if( j6array[0] >
IKPI )
4964 else if( j6array[0] < -
IKPI )
4965 { j6array[0]+=
IK2PI;
4968 for(
int ij6 = 0; ij6 < 1; ++ij6)
4974 _ij6[0] = ij6; _ij6[1] = -1;
4975 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4979 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4982 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4985 IkReal x161=
IKsin(j6);
4986 IkReal x162=
IKcos(j6);
4988 evalcond[1]=((-1.0)*x162);
4989 evalcond[2]=((((-1.0)*x161))+(((-1.0)*new_r00)));
4990 evalcond[3]=((((-1.0)*x162))+(((-1.0)*new_r01)));
4998 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4999 vinfos[0].jointtype = 17;
5000 vinfos[0].foffset = j0;
5001 vinfos[0].indices[0] = _ij0[0];
5002 vinfos[0].indices[1] = _ij0[1];
5003 vinfos[0].maxsolutions = _nj0;
5004 vinfos[1].jointtype = 1;
5005 vinfos[1].foffset = j1;
5006 vinfos[1].indices[0] = _ij1[0];
5007 vinfos[1].indices[1] = _ij1[1];
5008 vinfos[1].maxsolutions = _nj1;
5009 vinfos[2].jointtype = 1;
5010 vinfos[2].foffset = j2;
5011 vinfos[2].indices[0] = _ij2[0];
5012 vinfos[2].indices[1] = _ij2[1];
5013 vinfos[2].maxsolutions = _nj2;
5014 vinfos[3].jointtype = 1;
5015 vinfos[3].foffset = j3;
5016 vinfos[3].indices[0] = _ij3[0];
5017 vinfos[3].indices[1] = _ij3[1];
5018 vinfos[3].maxsolutions = _nj3;
5019 vinfos[4].jointtype = 1;
5020 vinfos[4].foffset = j4;
5021 vinfos[4].indices[0] = _ij4[0];
5022 vinfos[4].indices[1] = _ij4[1];
5023 vinfos[4].maxsolutions = _nj4;
5024 vinfos[5].jointtype = 1;
5025 vinfos[5].foffset = j5;
5026 vinfos[5].indices[0] = _ij5[0];
5027 vinfos[5].indices[1] = _ij5[1];
5028 vinfos[5].maxsolutions = _nj5;
5029 vinfos[6].jointtype = 1;
5030 vinfos[6].foffset = j6;
5031 vinfos[6].indices[0] = _ij6[0];
5032 vinfos[6].indices[1] = _ij6[1];
5033 vinfos[6].maxsolutions = _nj6;
5034 std::vector<int> vfree(0);
5042 if( bgotonextstatement )
5044 bool bgotonextstatement =
true;
5047 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
5048 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5050 bgotonextstatement=
false;
5052 IkReal j6array[1], cj6array[1], sj6array[1];
5053 bool j6valid[1]={
false};
5057 j6array[0]=
IKatan2(new_r00, new_r01);
5058 sj6array[0]=
IKsin(j6array[0]);
5059 cj6array[0]=
IKcos(j6array[0]);
5060 if( j6array[0] >
IKPI )
5064 else if( j6array[0] < -
IKPI )
5065 { j6array[0]+=
IK2PI;
5068 for(
int ij6 = 0; ij6 < 1; ++ij6)
5074 _ij6[0] = ij6; _ij6[1] = -1;
5075 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5079 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5082 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5085 IkReal x163=
IKsin(j6);
5086 IkReal x164=
IKcos(j6);
5088 evalcond[1]=((-1.0)*x164);
5089 evalcond[2]=((((-1.0)*x163))+new_r00);
5090 evalcond[3]=((((-1.0)*x164))+new_r01);
5098 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5099 vinfos[0].jointtype = 17;
5100 vinfos[0].foffset = j0;
5101 vinfos[0].indices[0] = _ij0[0];
5102 vinfos[0].indices[1] = _ij0[1];
5103 vinfos[0].maxsolutions = _nj0;
5104 vinfos[1].jointtype = 1;
5105 vinfos[1].foffset = j1;
5106 vinfos[1].indices[0] = _ij1[0];
5107 vinfos[1].indices[1] = _ij1[1];
5108 vinfos[1].maxsolutions = _nj1;
5109 vinfos[2].jointtype = 1;
5110 vinfos[2].foffset = j2;
5111 vinfos[2].indices[0] = _ij2[0];
5112 vinfos[2].indices[1] = _ij2[1];
5113 vinfos[2].maxsolutions = _nj2;
5114 vinfos[3].jointtype = 1;
5115 vinfos[3].foffset = j3;
5116 vinfos[3].indices[0] = _ij3[0];
5117 vinfos[3].indices[1] = _ij3[1];
5118 vinfos[3].maxsolutions = _nj3;
5119 vinfos[4].jointtype = 1;
5120 vinfos[4].foffset = j4;
5121 vinfos[4].indices[0] = _ij4[0];
5122 vinfos[4].indices[1] = _ij4[1];
5123 vinfos[4].maxsolutions = _nj4;
5124 vinfos[5].jointtype = 1;
5125 vinfos[5].foffset = j5;
5126 vinfos[5].indices[0] = _ij5[0];
5127 vinfos[5].indices[1] = _ij5[1];
5128 vinfos[5].maxsolutions = _nj5;
5129 vinfos[6].jointtype = 1;
5130 vinfos[6].foffset = j6;
5131 vinfos[6].indices[0] = _ij6[0];
5132 vinfos[6].indices[1] = _ij6[1];
5133 vinfos[6].maxsolutions = _nj6;
5134 std::vector<int> vfree(0);
5142 if( bgotonextstatement )
5144 bool bgotonextstatement =
true;
5147 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
5148 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5150 bgotonextstatement=
false;
5152 IkReal j6array[1], cj6array[1], sj6array[1];
5153 bool j6valid[1]={
false};
5157 j6array[0]=
IKatan2(new_r10, new_r11);
5158 sj6array[0]=
IKsin(j6array[0]);
5159 cj6array[0]=
IKcos(j6array[0]);
5160 if( j6array[0] >
IKPI )
5164 else if( j6array[0] < -
IKPI )
5165 { j6array[0]+=
IK2PI;
5168 for(
int ij6 = 0; ij6 < 1; ++ij6)
5174 _ij6[0] = ij6; _ij6[1] = -1;
5175 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5179 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5182 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5185 IkReal x165=
IKsin(j6);
5186 IkReal x166=
IKcos(j6);
5188 evalcond[1]=((-1.0)*x166);
5189 evalcond[2]=((((-1.0)*x165))+new_r10);
5190 evalcond[3]=((((-1.0)*x166))+new_r11);
5198 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5199 vinfos[0].jointtype = 17;
5200 vinfos[0].foffset = j0;
5201 vinfos[0].indices[0] = _ij0[0];
5202 vinfos[0].indices[1] = _ij0[1];
5203 vinfos[0].maxsolutions = _nj0;
5204 vinfos[1].jointtype = 1;
5205 vinfos[1].foffset = j1;
5206 vinfos[1].indices[0] = _ij1[0];
5207 vinfos[1].indices[1] = _ij1[1];
5208 vinfos[1].maxsolutions = _nj1;
5209 vinfos[2].jointtype = 1;
5210 vinfos[2].foffset = j2;
5211 vinfos[2].indices[0] = _ij2[0];
5212 vinfos[2].indices[1] = _ij2[1];
5213 vinfos[2].maxsolutions = _nj2;
5214 vinfos[3].jointtype = 1;
5215 vinfos[3].foffset = j3;
5216 vinfos[3].indices[0] = _ij3[0];
5217 vinfos[3].indices[1] = _ij3[1];
5218 vinfos[3].maxsolutions = _nj3;
5219 vinfos[4].jointtype = 1;
5220 vinfos[4].foffset = j4;
5221 vinfos[4].indices[0] = _ij4[0];
5222 vinfos[4].indices[1] = _ij4[1];
5223 vinfos[4].maxsolutions = _nj4;
5224 vinfos[5].jointtype = 1;
5225 vinfos[5].foffset = j5;
5226 vinfos[5].indices[0] = _ij5[0];
5227 vinfos[5].indices[1] = _ij5[1];
5228 vinfos[5].maxsolutions = _nj5;
5229 vinfos[6].jointtype = 1;
5230 vinfos[6].foffset = j6;
5231 vinfos[6].indices[0] = _ij6[0];
5232 vinfos[6].indices[1] = _ij6[1];
5233 vinfos[6].maxsolutions = _nj6;
5234 std::vector<int> vfree(0);
5242 if( bgotonextstatement )
5244 bool bgotonextstatement =
true;
5247 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
5248 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5250 bgotonextstatement=
false;
5252 IkReal j6array[1], cj6array[1], sj6array[1];
5253 bool j6valid[1]={
false};
5257 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
5258 sj6array[0]=
IKsin(j6array[0]);
5259 cj6array[0]=
IKcos(j6array[0]);
5260 if( j6array[0] >
IKPI )
5264 else if( j6array[0] < -
IKPI )
5265 { j6array[0]+=
IK2PI;
5268 for(
int ij6 = 0; ij6 < 1; ++ij6)
5274 _ij6[0] = ij6; _ij6[1] = -1;
5275 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5279 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5282 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5285 IkReal x167=
IKsin(j6);
5286 IkReal x168=
IKcos(j6);
5288 evalcond[1]=((-1.0)*x168);
5289 evalcond[2]=((((-1.0)*x167))+(((-1.0)*new_r10)));
5290 evalcond[3]=((((-1.0)*x168))+(((-1.0)*new_r11)));
5298 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5299 vinfos[0].jointtype = 17;
5300 vinfos[0].foffset = j0;
5301 vinfos[0].indices[0] = _ij0[0];
5302 vinfos[0].indices[1] = _ij0[1];
5303 vinfos[0].maxsolutions = _nj0;
5304 vinfos[1].jointtype = 1;
5305 vinfos[1].foffset = j1;
5306 vinfos[1].indices[0] = _ij1[0];
5307 vinfos[1].indices[1] = _ij1[1];
5308 vinfos[1].maxsolutions = _nj1;
5309 vinfos[2].jointtype = 1;
5310 vinfos[2].foffset = j2;
5311 vinfos[2].indices[0] = _ij2[0];
5312 vinfos[2].indices[1] = _ij2[1];
5313 vinfos[2].maxsolutions = _nj2;
5314 vinfos[3].jointtype = 1;
5315 vinfos[3].foffset = j3;
5316 vinfos[3].indices[0] = _ij3[0];
5317 vinfos[3].indices[1] = _ij3[1];
5318 vinfos[3].maxsolutions = _nj3;
5319 vinfos[4].jointtype = 1;
5320 vinfos[4].foffset = j4;
5321 vinfos[4].indices[0] = _ij4[0];
5322 vinfos[4].indices[1] = _ij4[1];
5323 vinfos[4].maxsolutions = _nj4;
5324 vinfos[5].jointtype = 1;
5325 vinfos[5].foffset = j5;
5326 vinfos[5].indices[0] = _ij5[0];
5327 vinfos[5].indices[1] = _ij5[1];
5328 vinfos[5].maxsolutions = _nj5;
5329 vinfos[6].jointtype = 1;
5330 vinfos[6].foffset = j6;
5331 vinfos[6].indices[0] = _ij6[0];
5332 vinfos[6].indices[1] = _ij6[1];
5333 vinfos[6].maxsolutions = _nj6;
5334 std::vector<int> vfree(0);
5342 if( bgotonextstatement )
5344 bool bgotonextstatement =
true;
5351 if((x169.
value) < -0.00001)
5354 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj4)))))+(
IKabs((cj4+(((-1.0)*gconst24)))))), 6.28318530717959)));
5355 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5357 bgotonextstatement=
false;
5364 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5366 sj4=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
5375 if((x170.
value) < -0.00001)
5378 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5379 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
5382 IkReal j6array[1], cj6array[1], sj6array[1];
5383 bool j6valid[1]={
false};
5385 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5393 j6array[0]=
IKatan2(((((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x171.
value)));
5394 sj6array[0]=
IKsin(j6array[0]);
5395 cj6array[0]=
IKcos(j6array[0]);
5396 if( j6array[0] >
IKPI )
5400 else if( j6array[0] < -
IKPI )
5401 { j6array[0]+=
IK2PI;
5404 for(
int ij6 = 0; ij6 < 1; ++ij6)
5410 _ij6[0] = ij6; _ij6[1] = -1;
5411 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5415 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5418 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5421 IkReal x172=
IKcos(j6);
5422 IkReal x173=
IKsin(j6);
5423 IkReal x174=((1.0)*x172);
5424 IkReal x175=((1.0)*x173);
5425 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5427 IkReal x176=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
5428 IkReal x177=((1.0)*x176);
5430 evalcond[1]=((-1.0)*x172);
5431 evalcond[2]=(new_r11+(((-1.0)*gconst24*x174)));
5432 evalcond[3]=(new_r10+(((-1.0)*gconst24*x175)));
5433 evalcond[4]=(((x172*x176))+new_r01);
5434 evalcond[5]=(((x173*x176))+new_r00);
5435 evalcond[6]=((((-1.0)*x175))+((gconst24*new_r10))+(((-1.0)*new_r00*x177)));
5436 evalcond[7]=((((-1.0)*x174))+((gconst24*new_r11))+(((-1.0)*new_r01*x177)));
5444 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5445 vinfos[0].jointtype = 17;
5446 vinfos[0].foffset = j0;
5447 vinfos[0].indices[0] = _ij0[0];
5448 vinfos[0].indices[1] = _ij0[1];
5449 vinfos[0].maxsolutions = _nj0;
5450 vinfos[1].jointtype = 1;
5451 vinfos[1].foffset = j1;
5452 vinfos[1].indices[0] = _ij1[0];
5453 vinfos[1].indices[1] = _ij1[1];
5454 vinfos[1].maxsolutions = _nj1;
5455 vinfos[2].jointtype = 1;
5456 vinfos[2].foffset = j2;
5457 vinfos[2].indices[0] = _ij2[0];
5458 vinfos[2].indices[1] = _ij2[1];
5459 vinfos[2].maxsolutions = _nj2;
5460 vinfos[3].jointtype = 1;
5461 vinfos[3].foffset = j3;
5462 vinfos[3].indices[0] = _ij3[0];
5463 vinfos[3].indices[1] = _ij3[1];
5464 vinfos[3].maxsolutions = _nj3;
5465 vinfos[4].jointtype = 1;
5466 vinfos[4].foffset = j4;
5467 vinfos[4].indices[0] = _ij4[0];
5468 vinfos[4].indices[1] = _ij4[1];
5469 vinfos[4].maxsolutions = _nj4;
5470 vinfos[5].jointtype = 1;
5471 vinfos[5].foffset = j5;
5472 vinfos[5].indices[0] = _ij5[0];
5473 vinfos[5].indices[1] = _ij5[1];
5474 vinfos[5].maxsolutions = _nj5;
5475 vinfos[6].jointtype = 1;
5476 vinfos[6].foffset = j6;
5477 vinfos[6].indices[0] = _ij6[0];
5478 vinfos[6].indices[1] = _ij6[1];
5479 vinfos[6].maxsolutions = _nj6;
5480 std::vector<int> vfree(0);
5489 IkReal j6array[1], cj6array[1], sj6array[1];
5490 bool j6valid[1]={
false};
5500 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x178.
value)))+(x179.
value));
5501 sj6array[0]=
IKsin(j6array[0]);
5502 cj6array[0]=
IKcos(j6array[0]);
5503 if( j6array[0] >
IKPI )
5507 else if( j6array[0] < -
IKPI )
5508 { j6array[0]+=
IK2PI;
5511 for(
int ij6 = 0; ij6 < 1; ++ij6)
5517 _ij6[0] = ij6; _ij6[1] = -1;
5518 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5522 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5525 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5528 IkReal x180=
IKcos(j6);
5529 IkReal x181=
IKsin(j6);
5530 IkReal x182=((1.0)*x180);
5531 IkReal x183=((1.0)*x181);
5532 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5534 IkReal x184=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
5535 IkReal x185=((1.0)*x184);
5537 evalcond[1]=((-1.0)*x180);
5538 evalcond[2]=((((-1.0)*gconst24*x182))+new_r11);
5539 evalcond[3]=((((-1.0)*gconst24*x183))+new_r10);
5540 evalcond[4]=(((x180*x184))+new_r01);
5541 evalcond[5]=(new_r00+((x181*x184)));
5542 evalcond[6]=((((-1.0)*x183))+((gconst24*new_r10))+(((-1.0)*new_r00*x185)));
5543 evalcond[7]=((((-1.0)*x182))+(((-1.0)*new_r01*x185))+((gconst24*new_r11)));
5551 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5552 vinfos[0].jointtype = 17;
5553 vinfos[0].foffset = j0;
5554 vinfos[0].indices[0] = _ij0[0];
5555 vinfos[0].indices[1] = _ij0[1];
5556 vinfos[0].maxsolutions = _nj0;
5557 vinfos[1].jointtype = 1;
5558 vinfos[1].foffset = j1;
5559 vinfos[1].indices[0] = _ij1[0];
5560 vinfos[1].indices[1] = _ij1[1];
5561 vinfos[1].maxsolutions = _nj1;
5562 vinfos[2].jointtype = 1;
5563 vinfos[2].foffset = j2;
5564 vinfos[2].indices[0] = _ij2[0];
5565 vinfos[2].indices[1] = _ij2[1];
5566 vinfos[2].maxsolutions = _nj2;
5567 vinfos[3].jointtype = 1;
5568 vinfos[3].foffset = j3;
5569 vinfos[3].indices[0] = _ij3[0];
5570 vinfos[3].indices[1] = _ij3[1];
5571 vinfos[3].maxsolutions = _nj3;
5572 vinfos[4].jointtype = 1;
5573 vinfos[4].foffset = j4;
5574 vinfos[4].indices[0] = _ij4[0];
5575 vinfos[4].indices[1] = _ij4[1];
5576 vinfos[4].maxsolutions = _nj4;
5577 vinfos[5].jointtype = 1;
5578 vinfos[5].foffset = j5;
5579 vinfos[5].indices[0] = _ij5[0];
5580 vinfos[5].indices[1] = _ij5[1];
5581 vinfos[5].maxsolutions = _nj5;
5582 vinfos[6].jointtype = 1;
5583 vinfos[6].foffset = j6;
5584 vinfos[6].indices[0] = _ij6[0];
5585 vinfos[6].indices[1] = _ij6[1];
5586 vinfos[6].maxsolutions = _nj6;
5587 std::vector<int> vfree(0);
5599 if( bgotonextstatement )
5601 bool bgotonextstatement =
true;
5608 if((x186.
value) < -0.00001)
5611 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj4+(((-1.0)*gconst24)))))+(
IKabs(((1.0)+(
IKsign(sj4)))))), 6.28318530717959)));
5612 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5614 bgotonextstatement=
false;
5621 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5623 sj4=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
5627 j4=((-1.0)*(
IKacos(gconst24)));
5632 if((x187.
value) < -0.00001)
5635 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5636 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
5639 IkReal j6array[1], cj6array[1], sj6array[1];
5640 bool j6valid[1]={
false};
5642 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5650 j6array[0]=
IKatan2((((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x188.
value)));
5651 sj6array[0]=
IKsin(j6array[0]);
5652 cj6array[0]=
IKcos(j6array[0]);
5653 if( j6array[0] >
IKPI )
5657 else if( j6array[0] < -
IKPI )
5658 { j6array[0]+=
IK2PI;
5661 for(
int ij6 = 0; ij6 < 1; ++ij6)
5667 _ij6[0] = ij6; _ij6[1] = -1;
5668 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5672 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5675 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5678 IkReal x189=
IKcos(j6);
5679 IkReal x190=
IKsin(j6);
5680 IkReal x191=((1.0)*x189);
5681 IkReal x192=((1.0)*x190);
5682 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5684 IkReal x193=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
5686 evalcond[1]=((-1.0)*x189);
5687 evalcond[2]=((((-1.0)*gconst24*x191))+new_r11);
5688 evalcond[3]=((((-1.0)*gconst24*x192))+new_r10);
5689 evalcond[4]=((((-1.0)*x191*x193))+new_r01);
5690 evalcond[5]=((((-1.0)*x192*x193))+new_r00);
5691 evalcond[6]=(((new_r00*x193))+(((-1.0)*x192))+((gconst24*new_r10)));
5692 evalcond[7]=(((new_r01*x193))+(((-1.0)*x191))+((gconst24*new_r11)));
5700 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5701 vinfos[0].jointtype = 17;
5702 vinfos[0].foffset = j0;
5703 vinfos[0].indices[0] = _ij0[0];
5704 vinfos[0].indices[1] = _ij0[1];
5705 vinfos[0].maxsolutions = _nj0;
5706 vinfos[1].jointtype = 1;
5707 vinfos[1].foffset = j1;
5708 vinfos[1].indices[0] = _ij1[0];
5709 vinfos[1].indices[1] = _ij1[1];
5710 vinfos[1].maxsolutions = _nj1;
5711 vinfos[2].jointtype = 1;
5712 vinfos[2].foffset = j2;
5713 vinfos[2].indices[0] = _ij2[0];
5714 vinfos[2].indices[1] = _ij2[1];
5715 vinfos[2].maxsolutions = _nj2;
5716 vinfos[3].jointtype = 1;
5717 vinfos[3].foffset = j3;
5718 vinfos[3].indices[0] = _ij3[0];
5719 vinfos[3].indices[1] = _ij3[1];
5720 vinfos[3].maxsolutions = _nj3;
5721 vinfos[4].jointtype = 1;
5722 vinfos[4].foffset = j4;
5723 vinfos[4].indices[0] = _ij4[0];
5724 vinfos[4].indices[1] = _ij4[1];
5725 vinfos[4].maxsolutions = _nj4;
5726 vinfos[5].jointtype = 1;
5727 vinfos[5].foffset = j5;
5728 vinfos[5].indices[0] = _ij5[0];
5729 vinfos[5].indices[1] = _ij5[1];
5730 vinfos[5].maxsolutions = _nj5;
5731 vinfos[6].jointtype = 1;
5732 vinfos[6].foffset = j6;
5733 vinfos[6].indices[0] = _ij6[0];
5734 vinfos[6].indices[1] = _ij6[1];
5735 vinfos[6].maxsolutions = _nj6;
5736 std::vector<int> vfree(0);
5745 IkReal j6array[1], cj6array[1], sj6array[1];
5746 bool j6valid[1]={
false};
5756 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x194.
value)))+(x195.
value));
5757 sj6array[0]=
IKsin(j6array[0]);
5758 cj6array[0]=
IKcos(j6array[0]);
5759 if( j6array[0] >
IKPI )
5763 else if( j6array[0] < -
IKPI )
5764 { j6array[0]+=
IK2PI;
5767 for(
int ij6 = 0; ij6 < 1; ++ij6)
5773 _ij6[0] = ij6; _ij6[1] = -1;
5774 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5778 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5781 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5784 IkReal x196=
IKcos(j6);
5785 IkReal x197=
IKsin(j6);
5786 IkReal x198=((1.0)*x196);
5787 IkReal x199=((1.0)*x197);
5788 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
5790 IkReal x200=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
5792 evalcond[1]=((-1.0)*x196);
5793 evalcond[2]=((((-1.0)*gconst24*x198))+new_r11);
5794 evalcond[3]=((((-1.0)*gconst24*x199))+new_r10);
5795 evalcond[4]=((((-1.0)*x198*x200))+new_r01);
5796 evalcond[5]=((((-1.0)*x199*x200))+new_r00);
5797 evalcond[6]=(((new_r00*x200))+(((-1.0)*x199))+((gconst24*new_r10)));
5798 evalcond[7]=((((-1.0)*x198))+((new_r01*x200))+((gconst24*new_r11)));
5806 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5807 vinfos[0].jointtype = 17;
5808 vinfos[0].foffset = j0;
5809 vinfos[0].indices[0] = _ij0[0];
5810 vinfos[0].indices[1] = _ij0[1];
5811 vinfos[0].maxsolutions = _nj0;
5812 vinfos[1].jointtype = 1;
5813 vinfos[1].foffset = j1;
5814 vinfos[1].indices[0] = _ij1[0];
5815 vinfos[1].indices[1] = _ij1[1];
5816 vinfos[1].maxsolutions = _nj1;
5817 vinfos[2].jointtype = 1;
5818 vinfos[2].foffset = j2;
5819 vinfos[2].indices[0] = _ij2[0];
5820 vinfos[2].indices[1] = _ij2[1];
5821 vinfos[2].maxsolutions = _nj2;
5822 vinfos[3].jointtype = 1;
5823 vinfos[3].foffset = j3;
5824 vinfos[3].indices[0] = _ij3[0];
5825 vinfos[3].indices[1] = _ij3[1];
5826 vinfos[3].maxsolutions = _nj3;
5827 vinfos[4].jointtype = 1;
5828 vinfos[4].foffset = j4;
5829 vinfos[4].indices[0] = _ij4[0];
5830 vinfos[4].indices[1] = _ij4[1];
5831 vinfos[4].maxsolutions = _nj4;
5832 vinfos[5].jointtype = 1;
5833 vinfos[5].foffset = j5;
5834 vinfos[5].indices[0] = _ij5[0];
5835 vinfos[5].indices[1] = _ij5[1];
5836 vinfos[5].maxsolutions = _nj5;
5837 vinfos[6].jointtype = 1;
5838 vinfos[6].foffset = j6;
5839 vinfos[6].indices[0] = _ij6[0];
5840 vinfos[6].indices[1] = _ij6[1];
5841 vinfos[6].maxsolutions = _nj6;
5842 std::vector<int> vfree(0);
5854 if( bgotonextstatement )
5856 bool bgotonextstatement =
true;
5863 if((x201.
value) < -0.00001)
5866 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj4)))))+(
IKabs((cj4+(((-1.0)*gconst25)))))), 6.28318530717959)));
5867 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5869 bgotonextstatement=
false;
5876 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
5878 sj4=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
5887 if((x202.
value) < -0.00001)
5890 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5891 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
5894 IkReal j6array[1], cj6array[1], sj6array[1];
5895 bool j6valid[1]={
false};
5897 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
5905 j6array[0]=
IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x203.
value)));
5906 sj6array[0]=
IKsin(j6array[0]);
5907 cj6array[0]=
IKcos(j6array[0]);
5908 if( j6array[0] >
IKPI )
5912 else if( j6array[0] < -
IKPI )
5913 { j6array[0]+=
IK2PI;
5916 for(
int ij6 = 0; ij6 < 1; ++ij6)
5922 _ij6[0] = ij6; _ij6[1] = -1;
5923 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5927 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5930 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5933 IkReal x204=
IKcos(j6);
5934 IkReal x205=
IKsin(j6);
5935 IkReal x206=((1.0)*x204);
5936 IkReal x207=((1.0)*x205);
5937 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
5939 IkReal x208=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
5940 IkReal x209=((1.0)*x208);
5942 evalcond[1]=((-1.0)*x204);
5943 evalcond[2]=((((-1.0)*gconst25*x206))+new_r11);
5944 evalcond[3]=((((-1.0)*gconst25*x207))+new_r10);
5945 evalcond[4]=(((x204*x208))+new_r01);
5946 evalcond[5]=(((x205*x208))+new_r00);
5947 evalcond[6]=((((-1.0)*new_r00*x209))+((gconst25*new_r10))+(((-1.0)*x207)));
5948 evalcond[7]=(((gconst25*new_r11))+(((-1.0)*x206))+(((-1.0)*new_r01*x209)));
5956 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5957 vinfos[0].jointtype = 17;
5958 vinfos[0].foffset = j0;
5959 vinfos[0].indices[0] = _ij0[0];
5960 vinfos[0].indices[1] = _ij0[1];
5961 vinfos[0].maxsolutions = _nj0;
5962 vinfos[1].jointtype = 1;
5963 vinfos[1].foffset = j1;
5964 vinfos[1].indices[0] = _ij1[0];
5965 vinfos[1].indices[1] = _ij1[1];
5966 vinfos[1].maxsolutions = _nj1;
5967 vinfos[2].jointtype = 1;
5968 vinfos[2].foffset = j2;
5969 vinfos[2].indices[0] = _ij2[0];
5970 vinfos[2].indices[1] = _ij2[1];
5971 vinfos[2].maxsolutions = _nj2;
5972 vinfos[3].jointtype = 1;
5973 vinfos[3].foffset = j3;
5974 vinfos[3].indices[0] = _ij3[0];
5975 vinfos[3].indices[1] = _ij3[1];
5976 vinfos[3].maxsolutions = _nj3;
5977 vinfos[4].jointtype = 1;
5978 vinfos[4].foffset = j4;
5979 vinfos[4].indices[0] = _ij4[0];
5980 vinfos[4].indices[1] = _ij4[1];
5981 vinfos[4].maxsolutions = _nj4;
5982 vinfos[5].jointtype = 1;
5983 vinfos[5].foffset = j5;
5984 vinfos[5].indices[0] = _ij5[0];
5985 vinfos[5].indices[1] = _ij5[1];
5986 vinfos[5].maxsolutions = _nj5;
5987 vinfos[6].jointtype = 1;
5988 vinfos[6].foffset = j6;
5989 vinfos[6].indices[0] = _ij6[0];
5990 vinfos[6].indices[1] = _ij6[1];
5991 vinfos[6].maxsolutions = _nj6;
5992 std::vector<int> vfree(0);
6001 IkReal j6array[1], cj6array[1], sj6array[1];
6002 bool j6valid[1]={
false};
6012 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x210.
value)))+(x211.
value));
6013 sj6array[0]=
IKsin(j6array[0]);
6014 cj6array[0]=
IKcos(j6array[0]);
6015 if( j6array[0] >
IKPI )
6019 else if( j6array[0] < -
IKPI )
6020 { j6array[0]+=
IK2PI;
6023 for(
int ij6 = 0; ij6 < 1; ++ij6)
6029 _ij6[0] = ij6; _ij6[1] = -1;
6030 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6034 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6037 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6040 IkReal x212=
IKcos(j6);
6041 IkReal x213=
IKsin(j6);
6042 IkReal x214=((1.0)*x212);
6043 IkReal x215=((1.0)*x213);
6044 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
6046 IkReal x216=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
6047 IkReal x217=((1.0)*x216);
6049 evalcond[1]=((-1.0)*x212);
6050 evalcond[2]=((((-1.0)*gconst25*x214))+new_r11);
6051 evalcond[3]=((((-1.0)*gconst25*x215))+new_r10);
6052 evalcond[4]=(((x212*x216))+new_r01);
6053 evalcond[5]=(((x213*x216))+new_r00);
6054 evalcond[6]=(((gconst25*new_r10))+(((-1.0)*x215))+(((-1.0)*new_r00*x217)));
6055 evalcond[7]=(((gconst25*new_r11))+(((-1.0)*new_r01*x217))+(((-1.0)*x214)));
6063 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6064 vinfos[0].jointtype = 17;
6065 vinfos[0].foffset = j0;
6066 vinfos[0].indices[0] = _ij0[0];
6067 vinfos[0].indices[1] = _ij0[1];
6068 vinfos[0].maxsolutions = _nj0;
6069 vinfos[1].jointtype = 1;
6070 vinfos[1].foffset = j1;
6071 vinfos[1].indices[0] = _ij1[0];
6072 vinfos[1].indices[1] = _ij1[1];
6073 vinfos[1].maxsolutions = _nj1;
6074 vinfos[2].jointtype = 1;
6075 vinfos[2].foffset = j2;
6076 vinfos[2].indices[0] = _ij2[0];
6077 vinfos[2].indices[1] = _ij2[1];
6078 vinfos[2].maxsolutions = _nj2;
6079 vinfos[3].jointtype = 1;
6080 vinfos[3].foffset = j3;
6081 vinfos[3].indices[0] = _ij3[0];
6082 vinfos[3].indices[1] = _ij3[1];
6083 vinfos[3].maxsolutions = _nj3;
6084 vinfos[4].jointtype = 1;
6085 vinfos[4].foffset = j4;
6086 vinfos[4].indices[0] = _ij4[0];
6087 vinfos[4].indices[1] = _ij4[1];
6088 vinfos[4].maxsolutions = _nj4;
6089 vinfos[5].jointtype = 1;
6090 vinfos[5].foffset = j5;
6091 vinfos[5].indices[0] = _ij5[0];
6092 vinfos[5].indices[1] = _ij5[1];
6093 vinfos[5].maxsolutions = _nj5;
6094 vinfos[6].jointtype = 1;
6095 vinfos[6].foffset = j6;
6096 vinfos[6].indices[0] = _ij6[0];
6097 vinfos[6].indices[1] = _ij6[1];
6098 vinfos[6].maxsolutions = _nj6;
6099 std::vector<int> vfree(0);
6111 if( bgotonextstatement )
6113 bool bgotonextstatement =
true;
6120 if((x218.
value) < -0.00001)
6123 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj4+(((-1.0)*gconst25)))))+(
IKabs(((1.0)+(
IKsign(sj4)))))), 6.28318530717959)));
6124 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
6126 bgotonextstatement=
false;
6133 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
6135 sj4=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
6139 j4=((-1.0)*(
IKacos(gconst25)));
6144 if((x219.
value) < -0.00001)
6147 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
6148 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
6151 IkReal j6array[1], cj6array[1], sj6array[1];
6152 bool j6valid[1]={
false};
6154 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
6162 j6array[0]=
IKatan2((((gconst25*new_r10))+((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x220.
value)));
6163 sj6array[0]=
IKsin(j6array[0]);
6164 cj6array[0]=
IKcos(j6array[0]);
6165 if( j6array[0] >
IKPI )
6169 else if( j6array[0] < -
IKPI )
6170 { j6array[0]+=
IK2PI;
6173 for(
int ij6 = 0; ij6 < 1; ++ij6)
6179 _ij6[0] = ij6; _ij6[1] = -1;
6180 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6184 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6187 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6190 IkReal x221=
IKcos(j6);
6191 IkReal x222=
IKsin(j6);
6192 IkReal x223=((1.0)*x221);
6193 IkReal x224=((1.0)*x222);
6194 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
6196 IkReal x225=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
6198 evalcond[1]=((-1.0)*x221);
6199 evalcond[2]=((((-1.0)*gconst25*x223))+new_r11);
6200 evalcond[3]=((((-1.0)*gconst25*x224))+new_r10);
6201 evalcond[4]=((((-1.0)*x223*x225))+new_r01);
6202 evalcond[5]=((((-1.0)*x224*x225))+new_r00);
6203 evalcond[6]=(((new_r00*x225))+((gconst25*new_r10))+(((-1.0)*x224)));
6204 evalcond[7]=(((gconst25*new_r11))+((new_r01*x225))+(((-1.0)*x223)));
6212 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6213 vinfos[0].jointtype = 17;
6214 vinfos[0].foffset = j0;
6215 vinfos[0].indices[0] = _ij0[0];
6216 vinfos[0].indices[1] = _ij0[1];
6217 vinfos[0].maxsolutions = _nj0;
6218 vinfos[1].jointtype = 1;
6219 vinfos[1].foffset = j1;
6220 vinfos[1].indices[0] = _ij1[0];
6221 vinfos[1].indices[1] = _ij1[1];
6222 vinfos[1].maxsolutions = _nj1;
6223 vinfos[2].jointtype = 1;
6224 vinfos[2].foffset = j2;
6225 vinfos[2].indices[0] = _ij2[0];
6226 vinfos[2].indices[1] = _ij2[1];
6227 vinfos[2].maxsolutions = _nj2;
6228 vinfos[3].jointtype = 1;
6229 vinfos[3].foffset = j3;
6230 vinfos[3].indices[0] = _ij3[0];
6231 vinfos[3].indices[1] = _ij3[1];
6232 vinfos[3].maxsolutions = _nj3;
6233 vinfos[4].jointtype = 1;
6234 vinfos[4].foffset = j4;
6235 vinfos[4].indices[0] = _ij4[0];
6236 vinfos[4].indices[1] = _ij4[1];
6237 vinfos[4].maxsolutions = _nj4;
6238 vinfos[5].jointtype = 1;
6239 vinfos[5].foffset = j5;
6240 vinfos[5].indices[0] = _ij5[0];
6241 vinfos[5].indices[1] = _ij5[1];
6242 vinfos[5].maxsolutions = _nj5;
6243 vinfos[6].jointtype = 1;
6244 vinfos[6].foffset = j6;
6245 vinfos[6].indices[0] = _ij6[0];
6246 vinfos[6].indices[1] = _ij6[1];
6247 vinfos[6].maxsolutions = _nj6;
6248 std::vector<int> vfree(0);
6257 IkReal j6array[1], cj6array[1], sj6array[1];
6258 bool j6valid[1]={
false};
6268 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x226.
value)))+(x227.
value));
6269 sj6array[0]=
IKsin(j6array[0]);
6270 cj6array[0]=
IKcos(j6array[0]);
6271 if( j6array[0] >
IKPI )
6275 else if( j6array[0] < -
IKPI )
6276 { j6array[0]+=
IK2PI;
6279 for(
int ij6 = 0; ij6 < 1; ++ij6)
6285 _ij6[0] = ij6; _ij6[1] = -1;
6286 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6290 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6293 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6296 IkReal x228=
IKcos(j6);
6297 IkReal x229=
IKsin(j6);
6298 IkReal x230=((1.0)*x228);
6299 IkReal x231=((1.0)*x229);
6300 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
6302 IkReal x232=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
6304 evalcond[1]=((-1.0)*x228);
6305 evalcond[2]=((((-1.0)*gconst25*x230))+new_r11);
6306 evalcond[3]=((((-1.0)*gconst25*x231))+new_r10);
6307 evalcond[4]=((((-1.0)*x230*x232))+new_r01);
6308 evalcond[5]=((((-1.0)*x231*x232))+new_r00);
6309 evalcond[6]=(((gconst25*new_r10))+((new_r00*x232))+(((-1.0)*x231)));
6310 evalcond[7]=(((gconst25*new_r11))+((new_r01*x232))+(((-1.0)*x230)));
6318 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6319 vinfos[0].jointtype = 17;
6320 vinfos[0].foffset = j0;
6321 vinfos[0].indices[0] = _ij0[0];
6322 vinfos[0].indices[1] = _ij0[1];
6323 vinfos[0].maxsolutions = _nj0;
6324 vinfos[1].jointtype = 1;
6325 vinfos[1].foffset = j1;
6326 vinfos[1].indices[0] = _ij1[0];
6327 vinfos[1].indices[1] = _ij1[1];
6328 vinfos[1].maxsolutions = _nj1;
6329 vinfos[2].jointtype = 1;
6330 vinfos[2].foffset = j2;
6331 vinfos[2].indices[0] = _ij2[0];
6332 vinfos[2].indices[1] = _ij2[1];
6333 vinfos[2].maxsolutions = _nj2;
6334 vinfos[3].jointtype = 1;
6335 vinfos[3].foffset = j3;
6336 vinfos[3].indices[0] = _ij3[0];
6337 vinfos[3].indices[1] = _ij3[1];
6338 vinfos[3].maxsolutions = _nj3;
6339 vinfos[4].jointtype = 1;
6340 vinfos[4].foffset = j4;
6341 vinfos[4].indices[0] = _ij4[0];
6342 vinfos[4].indices[1] = _ij4[1];
6343 vinfos[4].maxsolutions = _nj4;
6344 vinfos[5].jointtype = 1;
6345 vinfos[5].foffset = j5;
6346 vinfos[5].indices[0] = _ij5[0];
6347 vinfos[5].indices[1] = _ij5[1];
6348 vinfos[5].maxsolutions = _nj5;
6349 vinfos[6].jointtype = 1;
6350 vinfos[6].foffset = j6;
6351 vinfos[6].indices[0] = _ij6[0];
6352 vinfos[6].indices[1] = _ij6[1];
6353 vinfos[6].maxsolutions = _nj6;
6354 std::vector<int> vfree(0);
6366 if( bgotonextstatement )
6368 bool bgotonextstatement =
true;
6373 bgotonextstatement=
false;
6378 if( bgotonextstatement )
6394 IkReal j6array[1], cj6array[1], sj6array[1];
6395 bool j6valid[1]={
false};
6397 IkReal x233=new_r22*new_r22;
6408 j6array[0]=
IKatan2(((x234.
value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))), ((x235.
value)*((((new_r10*new_r22))+new_r01))));
6409 sj6array[0]=
IKsin(j6array[0]);
6410 cj6array[0]=
IKcos(j6array[0]);
6411 if( j6array[0] >
IKPI )
6415 else if( j6array[0] < -
IKPI )
6416 { j6array[0]+=
IK2PI;
6419 for(
int ij6 = 0; ij6 < 1; ++ij6)
6425 _ij6[0] = ij6; _ij6[1] = -1;
6426 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6430 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6433 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6435 IkReal evalcond[10];
6436 IkReal x236=
IKsin(j6);
6437 IkReal x237=
IKcos(j6);
6438 IkReal x238=((1.0)*sj4);
6439 IkReal x239=(cj4*new_r01);
6440 IkReal x240=(new_r10*sj4);
6441 IkReal x241=(new_r11*sj4);
6442 IkReal x242=(cj4*new_r00);
6443 IkReal x243=((1.0)*x237);
6444 IkReal x244=(sj4*x236);
6445 IkReal x245=(new_r22*x236);
6446 IkReal x246=((1.0)*x236);
6447 evalcond[0]=(x239+x241+x245);
6448 evalcond[1]=(((new_r22*x241))+((new_r22*x239))+x236);
6449 evalcond[2]=(((cj4*new_r10))+(((-1.0)*x246))+(((-1.0)*new_r00*x238)));
6450 evalcond[3]=(((cj4*new_r11))+(((-1.0)*x243))+(((-1.0)*new_r01*x238)));
6451 evalcond[4]=(((cj4*x245))+((sj4*x237))+new_r01);
6452 evalcond[5]=((((-1.0)*new_r22*x243))+x242+x240);
6453 evalcond[6]=((((-1.0)*cj4*new_r22*x243))+x244+new_r00);
6454 evalcond[7]=((((-1.0)*cj4*x243))+((new_r22*x244))+new_r11);
6455 evalcond[8]=(((new_r22*x240))+((new_r22*x242))+(((-1.0)*x243)));
6456 evalcond[9]=((((-1.0)*cj4*x246))+(((-1.0)*new_r22*x237*x238))+new_r10);
6464 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6465 vinfos[0].jointtype = 17;
6466 vinfos[0].foffset = j0;
6467 vinfos[0].indices[0] = _ij0[0];
6468 vinfos[0].indices[1] = _ij0[1];
6469 vinfos[0].maxsolutions = _nj0;
6470 vinfos[1].jointtype = 1;
6471 vinfos[1].foffset = j1;
6472 vinfos[1].indices[0] = _ij1[0];
6473 vinfos[1].indices[1] = _ij1[1];
6474 vinfos[1].maxsolutions = _nj1;
6475 vinfos[2].jointtype = 1;
6476 vinfos[2].foffset = j2;
6477 vinfos[2].indices[0] = _ij2[0];
6478 vinfos[2].indices[1] = _ij2[1];
6479 vinfos[2].maxsolutions = _nj2;
6480 vinfos[3].jointtype = 1;
6481 vinfos[3].foffset = j3;
6482 vinfos[3].indices[0] = _ij3[0];
6483 vinfos[3].indices[1] = _ij3[1];
6484 vinfos[3].maxsolutions = _nj3;
6485 vinfos[4].jointtype = 1;
6486 vinfos[4].foffset = j4;
6487 vinfos[4].indices[0] = _ij4[0];
6488 vinfos[4].indices[1] = _ij4[1];
6489 vinfos[4].maxsolutions = _nj4;
6490 vinfos[5].jointtype = 1;
6491 vinfos[5].foffset = j5;
6492 vinfos[5].indices[0] = _ij5[0];
6493 vinfos[5].indices[1] = _ij5[1];
6494 vinfos[5].maxsolutions = _nj5;
6495 vinfos[6].jointtype = 1;
6496 vinfos[6].foffset = j6;
6497 vinfos[6].indices[0] = _ij6[0];
6498 vinfos[6].indices[1] = _ij6[1];
6499 vinfos[6].maxsolutions = _nj6;
6500 std::vector<int> vfree(0);
6513 IkReal j6array[1], cj6array[1], sj6array[1];
6514 bool j6valid[1]={
false};
6516 IkReal x247=((1.0)*sj4);
6523 j6array[0]=
IKatan2(((x248.
value)*(((((-1.0)*cj4*new_r01))+(((-1.0)*new_r11*x247))))), ((((-1.0)*new_r01*x247))+((cj4*new_r11))));
6524 sj6array[0]=
IKsin(j6array[0]);
6525 cj6array[0]=
IKcos(j6array[0]);
6526 if( j6array[0] >
IKPI )
6530 else if( j6array[0] < -
IKPI )
6531 { j6array[0]+=
IK2PI;
6534 for(
int ij6 = 0; ij6 < 1; ++ij6)
6540 _ij6[0] = ij6; _ij6[1] = -1;
6541 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6545 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6548 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6550 IkReal evalcond[10];
6551 IkReal x249=
IKsin(j6);
6552 IkReal x250=
IKcos(j6);
6553 IkReal x251=((1.0)*sj4);
6554 IkReal x252=(cj4*new_r01);
6555 IkReal x253=(new_r10*sj4);
6556 IkReal x254=(new_r11*sj4);
6557 IkReal x255=(cj4*new_r00);
6558 IkReal x256=((1.0)*x250);
6559 IkReal x257=(sj4*x249);
6560 IkReal x258=(new_r22*x249);
6561 IkReal x259=((1.0)*x249);
6562 evalcond[0]=(x258+x254+x252);
6563 evalcond[1]=(x249+((new_r22*x252))+((new_r22*x254)));
6564 evalcond[2]=(((cj4*new_r10))+(((-1.0)*new_r00*x251))+(((-1.0)*x259)));
6565 evalcond[3]=((((-1.0)*new_r01*x251))+((cj4*new_r11))+(((-1.0)*x256)));
6566 evalcond[4]=(((sj4*x250))+new_r01+((cj4*x258)));
6567 evalcond[5]=(x255+x253+(((-1.0)*new_r22*x256)));
6568 evalcond[6]=(x257+(((-1.0)*cj4*new_r22*x256))+new_r00);
6569 evalcond[7]=((((-1.0)*cj4*x256))+((new_r22*x257))+new_r11);
6570 evalcond[8]=(((new_r22*x253))+((new_r22*x255))+(((-1.0)*x256)));
6571 evalcond[9]=((((-1.0)*cj4*x259))+(((-1.0)*new_r22*x250*x251))+new_r10);
6579 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6580 vinfos[0].jointtype = 17;
6581 vinfos[0].foffset = j0;
6582 vinfos[0].indices[0] = _ij0[0];
6583 vinfos[0].indices[1] = _ij0[1];
6584 vinfos[0].maxsolutions = _nj0;
6585 vinfos[1].jointtype = 1;
6586 vinfos[1].foffset = j1;
6587 vinfos[1].indices[0] = _ij1[0];
6588 vinfos[1].indices[1] = _ij1[1];
6589 vinfos[1].maxsolutions = _nj1;
6590 vinfos[2].jointtype = 1;
6591 vinfos[2].foffset = j2;
6592 vinfos[2].indices[0] = _ij2[0];
6593 vinfos[2].indices[1] = _ij2[1];
6594 vinfos[2].maxsolutions = _nj2;
6595 vinfos[3].jointtype = 1;
6596 vinfos[3].foffset = j3;
6597 vinfos[3].indices[0] = _ij3[0];
6598 vinfos[3].indices[1] = _ij3[1];
6599 vinfos[3].maxsolutions = _nj3;
6600 vinfos[4].jointtype = 1;
6601 vinfos[4].foffset = j4;
6602 vinfos[4].indices[0] = _ij4[0];
6603 vinfos[4].indices[1] = _ij4[1];
6604 vinfos[4].maxsolutions = _nj4;
6605 vinfos[5].jointtype = 1;
6606 vinfos[5].foffset = j5;
6607 vinfos[5].indices[0] = _ij5[0];
6608 vinfos[5].indices[1] = _ij5[1];
6609 vinfos[5].maxsolutions = _nj5;
6610 vinfos[6].jointtype = 1;
6611 vinfos[6].foffset = j6;
6612 vinfos[6].indices[0] = _ij6[0];
6613 vinfos[6].indices[1] = _ij6[1];
6614 vinfos[6].maxsolutions = _nj6;
6615 std::vector<int> vfree(0);
6628 IkReal j6array[1], cj6array[1], sj6array[1];
6629 bool j6valid[1]={
false};
6631 IkReal x260=cj4*cj4;
6632 IkReal x261=(cj4*new_r22);
6641 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x262.
value)))+(x263.
value));
6642 sj6array[0]=
IKsin(j6array[0]);
6643 cj6array[0]=
IKcos(j6array[0]);
6644 if( j6array[0] >
IKPI )
6648 else if( j6array[0] < -
IKPI )
6649 { j6array[0]+=
IK2PI;
6652 for(
int ij6 = 0; ij6 < 1; ++ij6)
6658 _ij6[0] = ij6; _ij6[1] = -1;
6659 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6663 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6666 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6668 IkReal evalcond[10];
6669 IkReal x264=
IKsin(j6);
6670 IkReal x265=
IKcos(j6);
6671 IkReal x266=((1.0)*sj4);
6672 IkReal x267=(cj4*new_r01);
6673 IkReal x268=(new_r10*sj4);
6674 IkReal x269=(new_r11*sj4);
6675 IkReal x270=(cj4*new_r00);
6676 IkReal x271=((1.0)*x265);
6677 IkReal x272=(sj4*x264);
6678 IkReal x273=(new_r22*x264);
6679 IkReal x274=((1.0)*x264);
6680 evalcond[0]=(x267+x269+x273);
6681 evalcond[1]=(((new_r22*x269))+((new_r22*x267))+x264);
6682 evalcond[2]=((((-1.0)*new_r00*x266))+((cj4*new_r10))+(((-1.0)*x274)));
6683 evalcond[3]=((((-1.0)*new_r01*x266))+((cj4*new_r11))+(((-1.0)*x271)));
6684 evalcond[4]=(((cj4*x273))+new_r01+((sj4*x265)));
6685 evalcond[5]=((((-1.0)*new_r22*x271))+x268+x270);
6686 evalcond[6]=((((-1.0)*cj4*new_r22*x271))+x272+new_r00);
6687 evalcond[7]=((((-1.0)*cj4*x271))+((new_r22*x272))+new_r11);
6688 evalcond[8]=(((new_r22*x268))+((new_r22*x270))+(((-1.0)*x271)));
6689 evalcond[9]=((((-1.0)*cj4*x274))+(((-1.0)*new_r22*x265*x266))+new_r10);
6697 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6698 vinfos[0].jointtype = 17;
6699 vinfos[0].foffset = j0;
6700 vinfos[0].indices[0] = _ij0[0];
6701 vinfos[0].indices[1] = _ij0[1];
6702 vinfos[0].maxsolutions = _nj0;
6703 vinfos[1].jointtype = 1;
6704 vinfos[1].foffset = j1;
6705 vinfos[1].indices[0] = _ij1[0];
6706 vinfos[1].indices[1] = _ij1[1];
6707 vinfos[1].maxsolutions = _nj1;
6708 vinfos[2].jointtype = 1;
6709 vinfos[2].foffset = j2;
6710 vinfos[2].indices[0] = _ij2[0];
6711 vinfos[2].indices[1] = _ij2[1];
6712 vinfos[2].maxsolutions = _nj2;
6713 vinfos[3].jointtype = 1;
6714 vinfos[3].foffset = j3;
6715 vinfos[3].indices[0] = _ij3[0];
6716 vinfos[3].indices[1] = _ij3[1];
6717 vinfos[3].maxsolutions = _nj3;
6718 vinfos[4].jointtype = 1;
6719 vinfos[4].foffset = j4;
6720 vinfos[4].indices[0] = _ij4[0];
6721 vinfos[4].indices[1] = _ij4[1];
6722 vinfos[4].maxsolutions = _nj4;
6723 vinfos[5].jointtype = 1;
6724 vinfos[5].foffset = j5;
6725 vinfos[5].indices[0] = _ij5[0];
6726 vinfos[5].indices[1] = _ij5[1];
6727 vinfos[5].maxsolutions = _nj5;
6728 vinfos[6].jointtype = 1;
6729 vinfos[6].foffset = j6;
6730 vinfos[6].indices[0] = _ij6[0];
6731 vinfos[6].indices[1] = _ij6[1];
6732 vinfos[6].maxsolutions = _nj6;
6733 std::vector<int> vfree(0);
6750 if( bgotonextstatement )
6752 bool bgotonextstatement =
true;
6757 bgotonextstatement=
false;
6762 if( bgotonextstatement )
6773 IkReal j4array[1], cj4array[1], sj4array[1];
6774 bool j4valid[1]={
false};
6780 IkReal x275=x276.
value;
6787 j4array[0]=
IKatan2((x275*(x277.
value)*(((1.0)+(((-1.0)*(new_r02*new_r02)))+(((-1.0)*(cj5*cj5)))))), (new_r02*x275));
6788 sj4array[0]=
IKsin(j4array[0]);
6789 cj4array[0]=
IKcos(j4array[0]);
6790 if( j4array[0] >
IKPI )
6794 else if( j4array[0] < -
IKPI )
6795 { j4array[0]+=
IK2PI;
6798 for(
int ij4 = 0; ij4 < 1; ++ij4)
6804 _ij4[0] = ij4; _ij4[1] = -1;
6805 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
6809 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
6812 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
6815 IkReal x278=
IKcos(j4);
6816 IkReal x279=
IKsin(j4);
6817 IkReal x280=((1.0)*sj5);
6818 IkReal x281=(new_r12*x279);
6819 IkReal x282=(new_r02*x278);
6820 IkReal x283=(sj5*x278);
6821 IkReal x284=(sj5*x279);
6822 evalcond[0]=(new_r02+(((-1.0)*x278*x280)));
6823 evalcond[1]=(new_r12+(((-1.0)*x279*x280)));
6824 evalcond[2]=((((-1.0)*new_r02*x279))+((new_r12*x278)));
6825 evalcond[3]=(x282+x281+(((-1.0)*x280)));
6826 evalcond[4]=(((cj5*new_r20))+((new_r00*x283))+((new_r10*x284)));
6827 evalcond[5]=(((cj5*new_r21))+((new_r11*x284))+((new_r01*x283)));
6828 evalcond[6]=((-1.0)+((cj5*new_r22))+((sj5*x281))+((sj5*x282)));
6829 evalcond[7]=((((-1.0)*new_r22*x280))+((cj5*x281))+((cj5*x282)));
6839 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
6841 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
6847 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
6854 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
6858 bool bgotonextstatement =
true;
6861 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
6862 evalcond[1]=new_r02;
6863 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
6865 bgotonextstatement=
false;
6872 j6eval[1]=
IKsign(new_r12);
6873 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
6874 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
6883 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
6884 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
6892 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
6896 bool bgotonextstatement =
true;
6899 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
6900 evalcond[1]=new_r22;
6901 evalcond[2]=new_r11;
6902 evalcond[3]=new_r10;
6903 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
6905 bgotonextstatement=
false;
6907 IkReal j6array[1], cj6array[1], sj6array[1];
6908 bool j6valid[1]={
false};
6912 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
6913 sj6array[0]=
IKsin(j6array[0]);
6914 cj6array[0]=
IKcos(j6array[0]);
6915 if( j6array[0] >
IKPI )
6919 else if( j6array[0] < -
IKPI )
6920 { j6array[0]+=
IK2PI;
6923 for(
int ij6 = 0; ij6 < 1; ++ij6)
6929 _ij6[0] = ij6; _ij6[1] = -1;
6930 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6934 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6937 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6940 IkReal x285=
IKcos(j6);
6941 IkReal x286=((1.0)*(
IKsin(j6)));
6942 evalcond[0]=(x285+new_r20);
6943 evalcond[1]=((((-1.0)*x286))+new_r21);
6944 evalcond[2]=((((-1.0)*x286))+(((-1.0)*new_r00)));
6945 evalcond[3]=((((-1.0)*x285))+(((-1.0)*new_r01)));
6953 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6954 vinfos[0].jointtype = 17;
6955 vinfos[0].foffset = j0;
6956 vinfos[0].indices[0] = _ij0[0];
6957 vinfos[0].indices[1] = _ij0[1];
6958 vinfos[0].maxsolutions = _nj0;
6959 vinfos[1].jointtype = 1;
6960 vinfos[1].foffset = j1;
6961 vinfos[1].indices[0] = _ij1[0];
6962 vinfos[1].indices[1] = _ij1[1];
6963 vinfos[1].maxsolutions = _nj1;
6964 vinfos[2].jointtype = 1;
6965 vinfos[2].foffset = j2;
6966 vinfos[2].indices[0] = _ij2[0];
6967 vinfos[2].indices[1] = _ij2[1];
6968 vinfos[2].maxsolutions = _nj2;
6969 vinfos[3].jointtype = 1;
6970 vinfos[3].foffset = j3;
6971 vinfos[3].indices[0] = _ij3[0];
6972 vinfos[3].indices[1] = _ij3[1];
6973 vinfos[3].maxsolutions = _nj3;
6974 vinfos[4].jointtype = 1;
6975 vinfos[4].foffset = j4;
6976 vinfos[4].indices[0] = _ij4[0];
6977 vinfos[4].indices[1] = _ij4[1];
6978 vinfos[4].maxsolutions = _nj4;
6979 vinfos[5].jointtype = 1;
6980 vinfos[5].foffset = j5;
6981 vinfos[5].indices[0] = _ij5[0];
6982 vinfos[5].indices[1] = _ij5[1];
6983 vinfos[5].maxsolutions = _nj5;
6984 vinfos[6].jointtype = 1;
6985 vinfos[6].foffset = j6;
6986 vinfos[6].indices[0] = _ij6[0];
6987 vinfos[6].indices[1] = _ij6[1];
6988 vinfos[6].maxsolutions = _nj6;
6989 std::vector<int> vfree(0);
6997 if( bgotonextstatement )
6999 bool bgotonextstatement =
true;
7002 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
7003 evalcond[1]=new_r22;
7004 evalcond[2]=new_r11;
7005 evalcond[3]=new_r10;
7006 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
7008 bgotonextstatement=
false;
7010 IkReal j6array[1], cj6array[1], sj6array[1];
7011 bool j6valid[1]={
false};
7015 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
7016 sj6array[0]=
IKsin(j6array[0]);
7017 cj6array[0]=
IKcos(j6array[0]);
7018 if( j6array[0] >
IKPI )
7022 else if( j6array[0] < -
IKPI )
7023 { j6array[0]+=
IK2PI;
7026 for(
int ij6 = 0; ij6 < 1; ++ij6)
7032 _ij6[0] = ij6; _ij6[1] = -1;
7033 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7037 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7040 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7043 IkReal x287=
IKsin(j6);
7044 IkReal x288=((1.0)*(
IKcos(j6)));
7045 evalcond[0]=(x287+new_r21);
7046 evalcond[1]=((((-1.0)*x288))+new_r20);
7047 evalcond[2]=((((-1.0)*x287))+(((-1.0)*new_r00)));
7048 evalcond[3]=((((-1.0)*x288))+(((-1.0)*new_r01)));
7056 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7057 vinfos[0].jointtype = 17;
7058 vinfos[0].foffset = j0;
7059 vinfos[0].indices[0] = _ij0[0];
7060 vinfos[0].indices[1] = _ij0[1];
7061 vinfos[0].maxsolutions = _nj0;
7062 vinfos[1].jointtype = 1;
7063 vinfos[1].foffset = j1;
7064 vinfos[1].indices[0] = _ij1[0];
7065 vinfos[1].indices[1] = _ij1[1];
7066 vinfos[1].maxsolutions = _nj1;
7067 vinfos[2].jointtype = 1;
7068 vinfos[2].foffset = j2;
7069 vinfos[2].indices[0] = _ij2[0];
7070 vinfos[2].indices[1] = _ij2[1];
7071 vinfos[2].maxsolutions = _nj2;
7072 vinfos[3].jointtype = 1;
7073 vinfos[3].foffset = j3;
7074 vinfos[3].indices[0] = _ij3[0];
7075 vinfos[3].indices[1] = _ij3[1];
7076 vinfos[3].maxsolutions = _nj3;
7077 vinfos[4].jointtype = 1;
7078 vinfos[4].foffset = j4;
7079 vinfos[4].indices[0] = _ij4[0];
7080 vinfos[4].indices[1] = _ij4[1];
7081 vinfos[4].maxsolutions = _nj4;
7082 vinfos[5].jointtype = 1;
7083 vinfos[5].foffset = j5;
7084 vinfos[5].indices[0] = _ij5[0];
7085 vinfos[5].indices[1] = _ij5[1];
7086 vinfos[5].maxsolutions = _nj5;
7087 vinfos[6].jointtype = 1;
7088 vinfos[6].foffset = j6;
7089 vinfos[6].indices[0] = _ij6[0];
7090 vinfos[6].indices[1] = _ij6[1];
7091 vinfos[6].maxsolutions = _nj6;
7092 std::vector<int> vfree(0);
7100 if( bgotonextstatement )
7102 bool bgotonextstatement =
true;
7105 evalcond[0]=
IKabs(new_r12);
7106 evalcond[1]=new_r20;
7107 evalcond[2]=new_r21;
7108 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
7110 bgotonextstatement=
false;
7119 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
7120 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
7129 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
7133 bool bgotonextstatement =
true;
7136 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
7137 evalcond[1]=new_r11;
7138 evalcond[2]=new_r10;
7139 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
7141 bgotonextstatement=
false;
7143 IkReal j6array[1], cj6array[1], sj6array[1];
7144 bool j6valid[1]={
false};
7148 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
7149 sj6array[0]=
IKsin(j6array[0]);
7150 cj6array[0]=
IKcos(j6array[0]);
7151 if( j6array[0] >
IKPI )
7155 else if( j6array[0] < -
IKPI )
7156 { j6array[0]+=
IK2PI;
7159 for(
int ij6 = 0; ij6 < 1; ++ij6)
7165 _ij6[0] = ij6; _ij6[1] = -1;
7166 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7170 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7173 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7176 IkReal x289=
IKsin(j6);
7177 IkReal x290=
IKcos(j6);
7179 evalcond[1]=((-1.0)*x290);
7180 evalcond[2]=((((-1.0)*x289))+(((-1.0)*new_r00)));
7181 evalcond[3]=((((-1.0)*x290))+(((-1.0)*new_r01)));
7189 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7190 vinfos[0].jointtype = 17;
7191 vinfos[0].foffset = j0;
7192 vinfos[0].indices[0] = _ij0[0];
7193 vinfos[0].indices[1] = _ij0[1];
7194 vinfos[0].maxsolutions = _nj0;
7195 vinfos[1].jointtype = 1;
7196 vinfos[1].foffset = j1;
7197 vinfos[1].indices[0] = _ij1[0];
7198 vinfos[1].indices[1] = _ij1[1];
7199 vinfos[1].maxsolutions = _nj1;
7200 vinfos[2].jointtype = 1;
7201 vinfos[2].foffset = j2;
7202 vinfos[2].indices[0] = _ij2[0];
7203 vinfos[2].indices[1] = _ij2[1];
7204 vinfos[2].maxsolutions = _nj2;
7205 vinfos[3].jointtype = 1;
7206 vinfos[3].foffset = j3;
7207 vinfos[3].indices[0] = _ij3[0];
7208 vinfos[3].indices[1] = _ij3[1];
7209 vinfos[3].maxsolutions = _nj3;
7210 vinfos[4].jointtype = 1;
7211 vinfos[4].foffset = j4;
7212 vinfos[4].indices[0] = _ij4[0];
7213 vinfos[4].indices[1] = _ij4[1];
7214 vinfos[4].maxsolutions = _nj4;
7215 vinfos[5].jointtype = 1;
7216 vinfos[5].foffset = j5;
7217 vinfos[5].indices[0] = _ij5[0];
7218 vinfos[5].indices[1] = _ij5[1];
7219 vinfos[5].maxsolutions = _nj5;
7220 vinfos[6].jointtype = 1;
7221 vinfos[6].foffset = j6;
7222 vinfos[6].indices[0] = _ij6[0];
7223 vinfos[6].indices[1] = _ij6[1];
7224 vinfos[6].maxsolutions = _nj6;
7225 std::vector<int> vfree(0);
7233 if( bgotonextstatement )
7235 bool bgotonextstatement =
true;
7238 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
7239 evalcond[1]=new_r11;
7240 evalcond[2]=new_r10;
7241 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
7243 bgotonextstatement=
false;
7245 IkReal j6array[1], cj6array[1], sj6array[1];
7246 bool j6valid[1]={
false};
7250 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
7251 sj6array[0]=
IKsin(j6array[0]);
7252 cj6array[0]=
IKcos(j6array[0]);
7253 if( j6array[0] >
IKPI )
7257 else if( j6array[0] < -
IKPI )
7258 { j6array[0]+=
IK2PI;
7261 for(
int ij6 = 0; ij6 < 1; ++ij6)
7267 _ij6[0] = ij6; _ij6[1] = -1;
7268 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7272 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7275 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7278 IkReal x291=
IKsin(j6);
7279 IkReal x292=
IKcos(j6);
7281 evalcond[1]=((-1.0)*x292);
7282 evalcond[2]=((((-1.0)*x291))+(((-1.0)*new_r00)));
7283 evalcond[3]=((((-1.0)*x292))+(((-1.0)*new_r01)));
7291 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7292 vinfos[0].jointtype = 17;
7293 vinfos[0].foffset = j0;
7294 vinfos[0].indices[0] = _ij0[0];
7295 vinfos[0].indices[1] = _ij0[1];
7296 vinfos[0].maxsolutions = _nj0;
7297 vinfos[1].jointtype = 1;
7298 vinfos[1].foffset = j1;
7299 vinfos[1].indices[0] = _ij1[0];
7300 vinfos[1].indices[1] = _ij1[1];
7301 vinfos[1].maxsolutions = _nj1;
7302 vinfos[2].jointtype = 1;
7303 vinfos[2].foffset = j2;
7304 vinfos[2].indices[0] = _ij2[0];
7305 vinfos[2].indices[1] = _ij2[1];
7306 vinfos[2].maxsolutions = _nj2;
7307 vinfos[3].jointtype = 1;
7308 vinfos[3].foffset = j3;
7309 vinfos[3].indices[0] = _ij3[0];
7310 vinfos[3].indices[1] = _ij3[1];
7311 vinfos[3].maxsolutions = _nj3;
7312 vinfos[4].jointtype = 1;
7313 vinfos[4].foffset = j4;
7314 vinfos[4].indices[0] = _ij4[0];
7315 vinfos[4].indices[1] = _ij4[1];
7316 vinfos[4].maxsolutions = _nj4;
7317 vinfos[5].jointtype = 1;
7318 vinfos[5].foffset = j5;
7319 vinfos[5].indices[0] = _ij5[0];
7320 vinfos[5].indices[1] = _ij5[1];
7321 vinfos[5].maxsolutions = _nj5;
7322 vinfos[6].jointtype = 1;
7323 vinfos[6].foffset = j6;
7324 vinfos[6].indices[0] = _ij6[0];
7325 vinfos[6].indices[1] = _ij6[1];
7326 vinfos[6].maxsolutions = _nj6;
7327 std::vector<int> vfree(0);
7335 if( bgotonextstatement )
7337 bool bgotonextstatement =
true;
7340 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
7341 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7343 bgotonextstatement=
false;
7345 IkReal j6array[1], cj6array[1], sj6array[1];
7346 bool j6valid[1]={
false};
7350 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
7351 sj6array[0]=
IKsin(j6array[0]);
7352 cj6array[0]=
IKcos(j6array[0]);
7353 if( j6array[0] >
IKPI )
7357 else if( j6array[0] < -
IKPI )
7358 { j6array[0]+=
IK2PI;
7361 for(
int ij6 = 0; ij6 < 1; ++ij6)
7367 _ij6[0] = ij6; _ij6[1] = -1;
7368 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7372 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7375 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7378 IkReal x293=
IKsin(j6);
7379 IkReal x294=
IKcos(j6);
7380 IkReal x295=((-1.0)*x294);
7382 evalcond[1]=(cj5*x293);
7384 evalcond[3]=(cj5*x295);
7385 evalcond[4]=((((-1.0)*x293))+(((-1.0)*new_r00)));
7386 evalcond[5]=((((-1.0)*x294))+(((-1.0)*new_r01)));
7394 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7395 vinfos[0].jointtype = 17;
7396 vinfos[0].foffset = j0;
7397 vinfos[0].indices[0] = _ij0[0];
7398 vinfos[0].indices[1] = _ij0[1];
7399 vinfos[0].maxsolutions = _nj0;
7400 vinfos[1].jointtype = 1;
7401 vinfos[1].foffset = j1;
7402 vinfos[1].indices[0] = _ij1[0];
7403 vinfos[1].indices[1] = _ij1[1];
7404 vinfos[1].maxsolutions = _nj1;
7405 vinfos[2].jointtype = 1;
7406 vinfos[2].foffset = j2;
7407 vinfos[2].indices[0] = _ij2[0];
7408 vinfos[2].indices[1] = _ij2[1];
7409 vinfos[2].maxsolutions = _nj2;
7410 vinfos[3].jointtype = 1;
7411 vinfos[3].foffset = j3;
7412 vinfos[3].indices[0] = _ij3[0];
7413 vinfos[3].indices[1] = _ij3[1];
7414 vinfos[3].maxsolutions = _nj3;
7415 vinfos[4].jointtype = 1;
7416 vinfos[4].foffset = j4;
7417 vinfos[4].indices[0] = _ij4[0];
7418 vinfos[4].indices[1] = _ij4[1];
7419 vinfos[4].maxsolutions = _nj4;
7420 vinfos[5].jointtype = 1;
7421 vinfos[5].foffset = j5;
7422 vinfos[5].indices[0] = _ij5[0];
7423 vinfos[5].indices[1] = _ij5[1];
7424 vinfos[5].maxsolutions = _nj5;
7425 vinfos[6].jointtype = 1;
7426 vinfos[6].foffset = j6;
7427 vinfos[6].indices[0] = _ij6[0];
7428 vinfos[6].indices[1] = _ij6[1];
7429 vinfos[6].maxsolutions = _nj6;
7430 std::vector<int> vfree(0);
7438 if( bgotonextstatement )
7440 bool bgotonextstatement =
true;
7445 bgotonextstatement=
false;
7450 if( bgotonextstatement )
7461 IkReal j6array[1], cj6array[1], sj6array[1];
7462 bool j6valid[1]={
false};
7470 j6array[0]=
IKatan2(((-1.0)*cj5*new_r11), (new_r10*(x296.
value)));
7471 sj6array[0]=
IKsin(j6array[0]);
7472 cj6array[0]=
IKcos(j6array[0]);
7473 if( j6array[0] >
IKPI )
7477 else if( j6array[0] < -
IKPI )
7478 { j6array[0]+=
IK2PI;
7481 for(
int ij6 = 0; ij6 < 1; ++ij6)
7487 _ij6[0] = ij6; _ij6[1] = -1;
7488 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7492 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7495 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7498 IkReal x297=
IKsin(j6);
7499 IkReal x298=
IKcos(j6);
7500 IkReal x299=((1.0)*x298);
7501 evalcond[0]=(((cj5*x297))+new_r11);
7502 evalcond[1]=(((cj5*new_r11))+x297);
7503 evalcond[2]=((((-1.0)*cj5*x299))+new_r10);
7504 evalcond[3]=((((-1.0)*x297))+(((-1.0)*new_r00)));
7505 evalcond[4]=((((-1.0)*x299))+(((-1.0)*new_r01)));
7506 evalcond[5]=(((cj5*new_r10))+(((-1.0)*x299)));
7514 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7515 vinfos[0].jointtype = 17;
7516 vinfos[0].foffset = j0;
7517 vinfos[0].indices[0] = _ij0[0];
7518 vinfos[0].indices[1] = _ij0[1];
7519 vinfos[0].maxsolutions = _nj0;
7520 vinfos[1].jointtype = 1;
7521 vinfos[1].foffset = j1;
7522 vinfos[1].indices[0] = _ij1[0];
7523 vinfos[1].indices[1] = _ij1[1];
7524 vinfos[1].maxsolutions = _nj1;
7525 vinfos[2].jointtype = 1;
7526 vinfos[2].foffset = j2;
7527 vinfos[2].indices[0] = _ij2[0];
7528 vinfos[2].indices[1] = _ij2[1];
7529 vinfos[2].maxsolutions = _nj2;
7530 vinfos[3].jointtype = 1;
7531 vinfos[3].foffset = j3;
7532 vinfos[3].indices[0] = _ij3[0];
7533 vinfos[3].indices[1] = _ij3[1];
7534 vinfos[3].maxsolutions = _nj3;
7535 vinfos[4].jointtype = 1;
7536 vinfos[4].foffset = j4;
7537 vinfos[4].indices[0] = _ij4[0];
7538 vinfos[4].indices[1] = _ij4[1];
7539 vinfos[4].maxsolutions = _nj4;
7540 vinfos[5].jointtype = 1;
7541 vinfos[5].foffset = j5;
7542 vinfos[5].indices[0] = _ij5[0];
7543 vinfos[5].indices[1] = _ij5[1];
7544 vinfos[5].maxsolutions = _nj5;
7545 vinfos[6].jointtype = 1;
7546 vinfos[6].foffset = j6;
7547 vinfos[6].indices[0] = _ij6[0];
7548 vinfos[6].indices[1] = _ij6[1];
7549 vinfos[6].maxsolutions = _nj6;
7550 std::vector<int> vfree(0);
7563 IkReal j6array[1], cj6array[1], sj6array[1];
7564 bool j6valid[1]={
false};
7574 j6array[0]=((-1.5707963267949)+(x300.
value)+(((1.5707963267949)*(x301.
value))));
7575 sj6array[0]=
IKsin(j6array[0]);
7576 cj6array[0]=
IKcos(j6array[0]);
7577 if( j6array[0] >
IKPI )
7581 else if( j6array[0] < -
IKPI )
7582 { j6array[0]+=
IK2PI;
7585 for(
int ij6 = 0; ij6 < 1; ++ij6)
7591 _ij6[0] = ij6; _ij6[1] = -1;
7592 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7596 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7599 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7602 IkReal x302=
IKsin(j6);
7603 IkReal x303=
IKcos(j6);
7604 IkReal x304=((1.0)*x303);
7605 evalcond[0]=(new_r11+((cj5*x302)));
7606 evalcond[1]=(((cj5*new_r11))+x302);
7607 evalcond[2]=((((-1.0)*cj5*x304))+new_r10);
7608 evalcond[3]=((((-1.0)*x302))+(((-1.0)*new_r00)));
7609 evalcond[4]=((((-1.0)*new_r01))+(((-1.0)*x304)));
7610 evalcond[5]=(((cj5*new_r10))+(((-1.0)*x304)));
7618 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7619 vinfos[0].jointtype = 17;
7620 vinfos[0].foffset = j0;
7621 vinfos[0].indices[0] = _ij0[0];
7622 vinfos[0].indices[1] = _ij0[1];
7623 vinfos[0].maxsolutions = _nj0;
7624 vinfos[1].jointtype = 1;
7625 vinfos[1].foffset = j1;
7626 vinfos[1].indices[0] = _ij1[0];
7627 vinfos[1].indices[1] = _ij1[1];
7628 vinfos[1].maxsolutions = _nj1;
7629 vinfos[2].jointtype = 1;
7630 vinfos[2].foffset = j2;
7631 vinfos[2].indices[0] = _ij2[0];
7632 vinfos[2].indices[1] = _ij2[1];
7633 vinfos[2].maxsolutions = _nj2;
7634 vinfos[3].jointtype = 1;
7635 vinfos[3].foffset = j3;
7636 vinfos[3].indices[0] = _ij3[0];
7637 vinfos[3].indices[1] = _ij3[1];
7638 vinfos[3].maxsolutions = _nj3;
7639 vinfos[4].jointtype = 1;
7640 vinfos[4].foffset = j4;
7641 vinfos[4].indices[0] = _ij4[0];
7642 vinfos[4].indices[1] = _ij4[1];
7643 vinfos[4].maxsolutions = _nj4;
7644 vinfos[5].jointtype = 1;
7645 vinfos[5].foffset = j5;
7646 vinfos[5].indices[0] = _ij5[0];
7647 vinfos[5].indices[1] = _ij5[1];
7648 vinfos[5].maxsolutions = _nj5;
7649 vinfos[6].jointtype = 1;
7650 vinfos[6].foffset = j6;
7651 vinfos[6].indices[0] = _ij6[0];
7652 vinfos[6].indices[1] = _ij6[1];
7653 vinfos[6].maxsolutions = _nj6;
7654 std::vector<int> vfree(0);
7666 if( bgotonextstatement )
7668 bool bgotonextstatement =
true;
7671 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
7672 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7674 bgotonextstatement=
false;
7676 IkReal j6array[1], cj6array[1], sj6array[1];
7677 bool j6valid[1]={
false};
7681 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
7682 sj6array[0]=
IKsin(j6array[0]);
7683 cj6array[0]=
IKcos(j6array[0]);
7684 if( j6array[0] >
IKPI )
7688 else if( j6array[0] < -
IKPI )
7689 { j6array[0]+=
IK2PI;
7692 for(
int ij6 = 0; ij6 < 1; ++ij6)
7698 _ij6[0] = ij6; _ij6[1] = -1;
7699 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7703 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7706 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7709 IkReal x305=
IKsin(j6);
7710 IkReal x306=
IKcos(j6);
7711 IkReal x307=((-1.0)*x306);
7713 evalcond[1]=(new_r22*x305);
7715 evalcond[3]=(new_r22*x307);
7716 evalcond[4]=((((-1.0)*x305))+(((-1.0)*new_r00)));
7717 evalcond[5]=((((-1.0)*x306))+(((-1.0)*new_r01)));
7725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7726 vinfos[0].jointtype = 17;
7727 vinfos[0].foffset = j0;
7728 vinfos[0].indices[0] = _ij0[0];
7729 vinfos[0].indices[1] = _ij0[1];
7730 vinfos[0].maxsolutions = _nj0;
7731 vinfos[1].jointtype = 1;
7732 vinfos[1].foffset = j1;
7733 vinfos[1].indices[0] = _ij1[0];
7734 vinfos[1].indices[1] = _ij1[1];
7735 vinfos[1].maxsolutions = _nj1;
7736 vinfos[2].jointtype = 1;
7737 vinfos[2].foffset = j2;
7738 vinfos[2].indices[0] = _ij2[0];
7739 vinfos[2].indices[1] = _ij2[1];
7740 vinfos[2].maxsolutions = _nj2;
7741 vinfos[3].jointtype = 1;
7742 vinfos[3].foffset = j3;
7743 vinfos[3].indices[0] = _ij3[0];
7744 vinfos[3].indices[1] = _ij3[1];
7745 vinfos[3].maxsolutions = _nj3;
7746 vinfos[4].jointtype = 1;
7747 vinfos[4].foffset = j4;
7748 vinfos[4].indices[0] = _ij4[0];
7749 vinfos[4].indices[1] = _ij4[1];
7750 vinfos[4].maxsolutions = _nj4;
7751 vinfos[5].jointtype = 1;
7752 vinfos[5].foffset = j5;
7753 vinfos[5].indices[0] = _ij5[0];
7754 vinfos[5].indices[1] = _ij5[1];
7755 vinfos[5].maxsolutions = _nj5;
7756 vinfos[6].jointtype = 1;
7757 vinfos[6].foffset = j6;
7758 vinfos[6].indices[0] = _ij6[0];
7759 vinfos[6].indices[1] = _ij6[1];
7760 vinfos[6].maxsolutions = _nj6;
7761 std::vector<int> vfree(0);
7769 if( bgotonextstatement )
7771 bool bgotonextstatement =
true;
7776 bgotonextstatement=
false;
7781 if( bgotonextstatement )
7793 IkReal j6array[1], cj6array[1], sj6array[1];
7794 bool j6valid[1]={
false};
7802 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x308.
value)));
7803 sj6array[0]=
IKsin(j6array[0]);
7804 cj6array[0]=
IKcos(j6array[0]);
7805 if( j6array[0] >
IKPI )
7809 else if( j6array[0] < -
IKPI )
7810 { j6array[0]+=
IK2PI;
7813 for(
int ij6 = 0; ij6 < 1; ++ij6)
7819 _ij6[0] = ij6; _ij6[1] = -1;
7820 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7824 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7827 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7830 IkReal x309=
IKsin(j6);
7831 IkReal x310=
IKcos(j6);
7832 IkReal x311=((1.0)*sj5);
7833 IkReal x312=((1.0)*x310);
7834 IkReal x313=((1.0)*x309);
7835 evalcond[0]=(((new_r12*x310))+new_r20);
7836 evalcond[1]=(new_r11+((cj5*x309)));
7837 evalcond[2]=((((-1.0)*new_r12*x313))+new_r21);
7838 evalcond[3]=((((-1.0)*cj5*x312))+new_r10);
7839 evalcond[4]=((((-1.0)*x313))+(((-1.0)*new_r00)));
7840 evalcond[5]=((((-1.0)*x312))+(((-1.0)*new_r01)));
7841 evalcond[6]=((((-1.0)*new_r21*x311))+((cj5*new_r11))+x309);
7842 evalcond[7]=(((cj5*new_r10))+(((-1.0)*new_r20*x311))+(((-1.0)*x312)));
7850 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7851 vinfos[0].jointtype = 17;
7852 vinfos[0].foffset = j0;
7853 vinfos[0].indices[0] = _ij0[0];
7854 vinfos[0].indices[1] = _ij0[1];
7855 vinfos[0].maxsolutions = _nj0;
7856 vinfos[1].jointtype = 1;
7857 vinfos[1].foffset = j1;
7858 vinfos[1].indices[0] = _ij1[0];
7859 vinfos[1].indices[1] = _ij1[1];
7860 vinfos[1].maxsolutions = _nj1;
7861 vinfos[2].jointtype = 1;
7862 vinfos[2].foffset = j2;
7863 vinfos[2].indices[0] = _ij2[0];
7864 vinfos[2].indices[1] = _ij2[1];
7865 vinfos[2].maxsolutions = _nj2;
7866 vinfos[3].jointtype = 1;
7867 vinfos[3].foffset = j3;
7868 vinfos[3].indices[0] = _ij3[0];
7869 vinfos[3].indices[1] = _ij3[1];
7870 vinfos[3].maxsolutions = _nj3;
7871 vinfos[4].jointtype = 1;
7872 vinfos[4].foffset = j4;
7873 vinfos[4].indices[0] = _ij4[0];
7874 vinfos[4].indices[1] = _ij4[1];
7875 vinfos[4].maxsolutions = _nj4;
7876 vinfos[5].jointtype = 1;
7877 vinfos[5].foffset = j5;
7878 vinfos[5].indices[0] = _ij5[0];
7879 vinfos[5].indices[1] = _ij5[1];
7880 vinfos[5].maxsolutions = _nj5;
7881 vinfos[6].jointtype = 1;
7882 vinfos[6].foffset = j6;
7883 vinfos[6].indices[0] = _ij6[0];
7884 vinfos[6].indices[1] = _ij6[1];
7885 vinfos[6].maxsolutions = _nj6;
7886 std::vector<int> vfree(0);
7899 IkReal j6array[1], cj6array[1], sj6array[1];
7900 bool j6valid[1]={
false};
7910 j6array[0]=((-1.5707963267949)+(x314.
value)+(((1.5707963267949)*(x315.
value))));
7911 sj6array[0]=
IKsin(j6array[0]);
7912 cj6array[0]=
IKcos(j6array[0]);
7913 if( j6array[0] >
IKPI )
7917 else if( j6array[0] < -
IKPI )
7918 { j6array[0]+=
IK2PI;
7921 for(
int ij6 = 0; ij6 < 1; ++ij6)
7927 _ij6[0] = ij6; _ij6[1] = -1;
7928 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7932 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7935 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7938 IkReal x316=
IKsin(j6);
7939 IkReal x317=
IKcos(j6);
7940 IkReal x318=((1.0)*sj5);
7941 IkReal x319=((1.0)*x317);
7942 IkReal x320=((1.0)*x316);
7943 evalcond[0]=(((new_r12*x317))+new_r20);
7944 evalcond[1]=(((cj5*x316))+new_r11);
7945 evalcond[2]=((((-1.0)*new_r12*x320))+new_r21);
7946 evalcond[3]=((((-1.0)*cj5*x319))+new_r10);
7947 evalcond[4]=((((-1.0)*x320))+(((-1.0)*new_r00)));
7948 evalcond[5]=((((-1.0)*x319))+(((-1.0)*new_r01)));
7949 evalcond[6]=((((-1.0)*new_r21*x318))+((cj5*new_r11))+x316);
7950 evalcond[7]=(((cj5*new_r10))+(((-1.0)*new_r20*x318))+(((-1.0)*x319)));
7958 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7959 vinfos[0].jointtype = 17;
7960 vinfos[0].foffset = j0;
7961 vinfos[0].indices[0] = _ij0[0];
7962 vinfos[0].indices[1] = _ij0[1];
7963 vinfos[0].maxsolutions = _nj0;
7964 vinfos[1].jointtype = 1;
7965 vinfos[1].foffset = j1;
7966 vinfos[1].indices[0] = _ij1[0];
7967 vinfos[1].indices[1] = _ij1[1];
7968 vinfos[1].maxsolutions = _nj1;
7969 vinfos[2].jointtype = 1;
7970 vinfos[2].foffset = j2;
7971 vinfos[2].indices[0] = _ij2[0];
7972 vinfos[2].indices[1] = _ij2[1];
7973 vinfos[2].maxsolutions = _nj2;
7974 vinfos[3].jointtype = 1;
7975 vinfos[3].foffset = j3;
7976 vinfos[3].indices[0] = _ij3[0];
7977 vinfos[3].indices[1] = _ij3[1];
7978 vinfos[3].maxsolutions = _nj3;
7979 vinfos[4].jointtype = 1;
7980 vinfos[4].foffset = j4;
7981 vinfos[4].indices[0] = _ij4[0];
7982 vinfos[4].indices[1] = _ij4[1];
7983 vinfos[4].maxsolutions = _nj4;
7984 vinfos[5].jointtype = 1;
7985 vinfos[5].foffset = j5;
7986 vinfos[5].indices[0] = _ij5[0];
7987 vinfos[5].indices[1] = _ij5[1];
7988 vinfos[5].maxsolutions = _nj5;
7989 vinfos[6].jointtype = 1;
7990 vinfos[6].foffset = j6;
7991 vinfos[6].indices[0] = _ij6[0];
7992 vinfos[6].indices[1] = _ij6[1];
7993 vinfos[6].maxsolutions = _nj6;
7994 std::vector<int> vfree(0);
8007 IkReal j6array[1], cj6array[1], sj6array[1];
8008 bool j6valid[1]={
false};
8018 j6array[0]=((-1.5707963267949)+(x321.
value)+(((1.5707963267949)*(x322.
value))));
8019 sj6array[0]=
IKsin(j6array[0]);
8020 cj6array[0]=
IKcos(j6array[0]);
8021 if( j6array[0] >
IKPI )
8025 else if( j6array[0] < -
IKPI )
8026 { j6array[0]+=
IK2PI;
8029 for(
int ij6 = 0; ij6 < 1; ++ij6)
8035 _ij6[0] = ij6; _ij6[1] = -1;
8036 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8040 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8043 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8046 IkReal x323=
IKsin(j6);
8047 IkReal x324=
IKcos(j6);
8048 IkReal x325=((1.0)*sj5);
8049 IkReal x326=((1.0)*x324);
8050 IkReal x327=((1.0)*x323);
8051 evalcond[0]=(((new_r12*x324))+new_r20);
8052 evalcond[1]=(((cj5*x323))+new_r11);
8053 evalcond[2]=((((-1.0)*new_r12*x327))+new_r21);
8054 evalcond[3]=((((-1.0)*cj5*x326))+new_r10);
8055 evalcond[4]=((((-1.0)*x327))+(((-1.0)*new_r00)));
8056 evalcond[5]=((((-1.0)*x326))+(((-1.0)*new_r01)));
8057 evalcond[6]=(((cj5*new_r11))+x323+(((-1.0)*new_r21*x325)));
8058 evalcond[7]=(((cj5*new_r10))+(((-1.0)*x326))+(((-1.0)*new_r20*x325)));
8066 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8067 vinfos[0].jointtype = 17;
8068 vinfos[0].foffset = j0;
8069 vinfos[0].indices[0] = _ij0[0];
8070 vinfos[0].indices[1] = _ij0[1];
8071 vinfos[0].maxsolutions = _nj0;
8072 vinfos[1].jointtype = 1;
8073 vinfos[1].foffset = j1;
8074 vinfos[1].indices[0] = _ij1[0];
8075 vinfos[1].indices[1] = _ij1[1];
8076 vinfos[1].maxsolutions = _nj1;
8077 vinfos[2].jointtype = 1;
8078 vinfos[2].foffset = j2;
8079 vinfos[2].indices[0] = _ij2[0];
8080 vinfos[2].indices[1] = _ij2[1];
8081 vinfos[2].maxsolutions = _nj2;
8082 vinfos[3].jointtype = 1;
8083 vinfos[3].foffset = j3;
8084 vinfos[3].indices[0] = _ij3[0];
8085 vinfos[3].indices[1] = _ij3[1];
8086 vinfos[3].maxsolutions = _nj3;
8087 vinfos[4].jointtype = 1;
8088 vinfos[4].foffset = j4;
8089 vinfos[4].indices[0] = _ij4[0];
8090 vinfos[4].indices[1] = _ij4[1];
8091 vinfos[4].maxsolutions = _nj4;
8092 vinfos[5].jointtype = 1;
8093 vinfos[5].foffset = j5;
8094 vinfos[5].indices[0] = _ij5[0];
8095 vinfos[5].indices[1] = _ij5[1];
8096 vinfos[5].maxsolutions = _nj5;
8097 vinfos[6].jointtype = 1;
8098 vinfos[6].foffset = j6;
8099 vinfos[6].indices[0] = _ij6[0];
8100 vinfos[6].indices[1] = _ij6[1];
8101 vinfos[6].maxsolutions = _nj6;
8102 std::vector<int> vfree(0);
8114 if( bgotonextstatement )
8116 bool bgotonextstatement =
true;
8119 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
8120 evalcond[1]=new_r02;
8121 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8123 bgotonextstatement=
false;
8125 IkReal j6array[1], cj6array[1], sj6array[1];
8126 bool j6valid[1]={
false};
8130 j6array[0]=
IKatan2(new_r00, new_r01);
8131 sj6array[0]=
IKsin(j6array[0]);
8132 cj6array[0]=
IKcos(j6array[0]);
8133 if( j6array[0] >
IKPI )
8137 else if( j6array[0] < -
IKPI )
8138 { j6array[0]+=
IK2PI;
8141 for(
int ij6 = 0; ij6 < 1; ++ij6)
8147 _ij6[0] = ij6; _ij6[1] = -1;
8148 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8152 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8155 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8158 IkReal x328=
IKsin(j6);
8159 IkReal x329=
IKcos(j6);
8164 IkReal x330=x334.
value;
8165 IkReal x331=new_r22*new_r22;
8166 IkReal x332=((1.0)*x329);
8167 IkReal x333=(x330*x331);
8168 evalcond[0]=(((new_r12*x328))+new_r21);
8169 evalcond[1]=((((-1.0)*x328))+new_r00);
8170 evalcond[2]=((((-1.0)*x332))+new_r01);
8171 evalcond[3]=((((-1.0)*new_r12*x332))+new_r20);
8172 evalcond[4]=(((cj5*x328))+(((-1.0)*new_r11)));
8173 evalcond[5]=((((-1.0)*cj5*x332))+(((-1.0)*new_r10)));
8174 evalcond[6]=(((new_r21*x333))+x328+((new_r12*new_r21)));
8175 evalcond[7]=(((new_r12*new_r20))+(((-1.0)*x332))+((new_r20*x333)));
8183 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8184 vinfos[0].jointtype = 17;
8185 vinfos[0].foffset = j0;
8186 vinfos[0].indices[0] = _ij0[0];
8187 vinfos[0].indices[1] = _ij0[1];
8188 vinfos[0].maxsolutions = _nj0;
8189 vinfos[1].jointtype = 1;
8190 vinfos[1].foffset = j1;
8191 vinfos[1].indices[0] = _ij1[0];
8192 vinfos[1].indices[1] = _ij1[1];
8193 vinfos[1].maxsolutions = _nj1;
8194 vinfos[2].jointtype = 1;
8195 vinfos[2].foffset = j2;
8196 vinfos[2].indices[0] = _ij2[0];
8197 vinfos[2].indices[1] = _ij2[1];
8198 vinfos[2].maxsolutions = _nj2;
8199 vinfos[3].jointtype = 1;
8200 vinfos[3].foffset = j3;
8201 vinfos[3].indices[0] = _ij3[0];
8202 vinfos[3].indices[1] = _ij3[1];
8203 vinfos[3].maxsolutions = _nj3;
8204 vinfos[4].jointtype = 1;
8205 vinfos[4].foffset = j4;
8206 vinfos[4].indices[0] = _ij4[0];
8207 vinfos[4].indices[1] = _ij4[1];
8208 vinfos[4].maxsolutions = _nj4;
8209 vinfos[5].jointtype = 1;
8210 vinfos[5].foffset = j5;
8211 vinfos[5].indices[0] = _ij5[0];
8212 vinfos[5].indices[1] = _ij5[1];
8213 vinfos[5].maxsolutions = _nj5;
8214 vinfos[6].jointtype = 1;
8215 vinfos[6].foffset = j6;
8216 vinfos[6].indices[0] = _ij6[0];
8217 vinfos[6].indices[1] = _ij6[1];
8218 vinfos[6].maxsolutions = _nj6;
8219 std::vector<int> vfree(0);
8227 if( bgotonextstatement )
8229 bool bgotonextstatement =
true;
8232 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
8233 evalcond[1]=new_r22;
8234 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8236 bgotonextstatement=
false;
8238 IkReal j6array[1], cj6array[1], sj6array[1];
8239 bool j6valid[1]={
false};
8243 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
8244 sj6array[0]=
IKsin(j6array[0]);
8245 cj6array[0]=
IKcos(j6array[0]);
8246 if( j6array[0] >
IKPI )
8250 else if( j6array[0] < -
IKPI )
8251 { j6array[0]+=
IK2PI;
8254 for(
int ij6 = 0; ij6 < 1; ++ij6)
8260 _ij6[0] = ij6; _ij6[1] = -1;
8261 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8265 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8268 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8271 IkReal x335=
IKcos(j6);
8272 IkReal x336=
IKsin(j6);
8273 IkReal x337=((1.0)*sj4);
8274 IkReal x338=((1.0)*x336);
8275 IkReal x339=((1.0)*x335);
8276 evalcond[0]=(x335+new_r20);
8277 evalcond[1]=((((-1.0)*x338))+new_r21);
8278 evalcond[2]=(((sj4*x335))+new_r01);
8279 evalcond[3]=(((sj4*x336))+new_r00);
8280 evalcond[4]=((((-1.0)*cj4*x339))+new_r11);
8281 evalcond[5]=((((-1.0)*cj4*x338))+new_r10);
8282 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x337))+(((-1.0)*x338)));
8283 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x339))+(((-1.0)*new_r01*x337)));
8291 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8292 vinfos[0].jointtype = 17;
8293 vinfos[0].foffset = j0;
8294 vinfos[0].indices[0] = _ij0[0];
8295 vinfos[0].indices[1] = _ij0[1];
8296 vinfos[0].maxsolutions = _nj0;
8297 vinfos[1].jointtype = 1;
8298 vinfos[1].foffset = j1;
8299 vinfos[1].indices[0] = _ij1[0];
8300 vinfos[1].indices[1] = _ij1[1];
8301 vinfos[1].maxsolutions = _nj1;
8302 vinfos[2].jointtype = 1;
8303 vinfos[2].foffset = j2;
8304 vinfos[2].indices[0] = _ij2[0];
8305 vinfos[2].indices[1] = _ij2[1];
8306 vinfos[2].maxsolutions = _nj2;
8307 vinfos[3].jointtype = 1;
8308 vinfos[3].foffset = j3;
8309 vinfos[3].indices[0] = _ij3[0];
8310 vinfos[3].indices[1] = _ij3[1];
8311 vinfos[3].maxsolutions = _nj3;
8312 vinfos[4].jointtype = 1;
8313 vinfos[4].foffset = j4;
8314 vinfos[4].indices[0] = _ij4[0];
8315 vinfos[4].indices[1] = _ij4[1];
8316 vinfos[4].maxsolutions = _nj4;
8317 vinfos[5].jointtype = 1;
8318 vinfos[5].foffset = j5;
8319 vinfos[5].indices[0] = _ij5[0];
8320 vinfos[5].indices[1] = _ij5[1];
8321 vinfos[5].maxsolutions = _nj5;
8322 vinfos[6].jointtype = 1;
8323 vinfos[6].foffset = j6;
8324 vinfos[6].indices[0] = _ij6[0];
8325 vinfos[6].indices[1] = _ij6[1];
8326 vinfos[6].maxsolutions = _nj6;
8327 std::vector<int> vfree(0);
8335 if( bgotonextstatement )
8337 bool bgotonextstatement =
true;
8340 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
8341 evalcond[1]=new_r22;
8342 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8344 bgotonextstatement=
false;
8346 IkReal j6array[1], cj6array[1], sj6array[1];
8347 bool j6valid[1]={
false};
8351 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
8352 sj6array[0]=
IKsin(j6array[0]);
8353 cj6array[0]=
IKcos(j6array[0]);
8354 if( j6array[0] >
IKPI )
8358 else if( j6array[0] < -
IKPI )
8359 { j6array[0]+=
IK2PI;
8362 for(
int ij6 = 0; ij6 < 1; ++ij6)
8368 _ij6[0] = ij6; _ij6[1] = -1;
8369 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8373 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8376 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8379 IkReal x340=
IKcos(j6);
8380 IkReal x341=
IKsin(j6);
8381 IkReal x342=((1.0)*sj4);
8382 IkReal x343=((1.0)*x340);
8383 IkReal x344=((1.0)*x341);
8384 evalcond[0]=(x341+new_r21);
8385 evalcond[1]=(new_r20+(((-1.0)*x343)));
8386 evalcond[2]=(((sj4*x340))+new_r01);
8387 evalcond[3]=(((sj4*x341))+new_r00);
8388 evalcond[4]=((((-1.0)*cj4*x343))+new_r11);
8389 evalcond[5]=((((-1.0)*cj4*x344))+new_r10);
8390 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x342))+(((-1.0)*x344)));
8391 evalcond[7]=(((cj4*new_r11))+(((-1.0)*new_r01*x342))+(((-1.0)*x343)));
8399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8400 vinfos[0].jointtype = 17;
8401 vinfos[0].foffset = j0;
8402 vinfos[0].indices[0] = _ij0[0];
8403 vinfos[0].indices[1] = _ij0[1];
8404 vinfos[0].maxsolutions = _nj0;
8405 vinfos[1].jointtype = 1;
8406 vinfos[1].foffset = j1;
8407 vinfos[1].indices[0] = _ij1[0];
8408 vinfos[1].indices[1] = _ij1[1];
8409 vinfos[1].maxsolutions = _nj1;
8410 vinfos[2].jointtype = 1;
8411 vinfos[2].foffset = j2;
8412 vinfos[2].indices[0] = _ij2[0];
8413 vinfos[2].indices[1] = _ij2[1];
8414 vinfos[2].maxsolutions = _nj2;
8415 vinfos[3].jointtype = 1;
8416 vinfos[3].foffset = j3;
8417 vinfos[3].indices[0] = _ij3[0];
8418 vinfos[3].indices[1] = _ij3[1];
8419 vinfos[3].maxsolutions = _nj3;
8420 vinfos[4].jointtype = 1;
8421 vinfos[4].foffset = j4;
8422 vinfos[4].indices[0] = _ij4[0];
8423 vinfos[4].indices[1] = _ij4[1];
8424 vinfos[4].maxsolutions = _nj4;
8425 vinfos[5].jointtype = 1;
8426 vinfos[5].foffset = j5;
8427 vinfos[5].indices[0] = _ij5[0];
8428 vinfos[5].indices[1] = _ij5[1];
8429 vinfos[5].maxsolutions = _nj5;
8430 vinfos[6].jointtype = 1;
8431 vinfos[6].foffset = j6;
8432 vinfos[6].indices[0] = _ij6[0];
8433 vinfos[6].indices[1] = _ij6[1];
8434 vinfos[6].maxsolutions = _nj6;
8435 std::vector<int> vfree(0);
8443 if( bgotonextstatement )
8445 bool bgotonextstatement =
true;
8448 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
8449 evalcond[1]=new_r20;
8450 evalcond[2]=new_r02;
8451 evalcond[3]=new_r12;
8452 evalcond[4]=new_r21;
8453 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 )
8455 bgotonextstatement=
false;
8457 IkReal j6array[1], cj6array[1], sj6array[1];
8458 bool j6valid[1]={
false};
8460 IkReal x345=((1.0)*new_r01);
8463 j6array[0]=
IKatan2(((((-1.0)*cj4*x345))+(((-1.0)*new_r00*sj4))), (((cj4*new_r00))+(((-1.0)*sj4*x345))));
8464 sj6array[0]=
IKsin(j6array[0]);
8465 cj6array[0]=
IKcos(j6array[0]);
8466 if( j6array[0] >
IKPI )
8470 else if( j6array[0] < -
IKPI )
8471 { j6array[0]+=
IK2PI;
8474 for(
int ij6 = 0; ij6 < 1; ++ij6)
8480 _ij6[0] = ij6; _ij6[1] = -1;
8481 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8485 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8488 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8491 IkReal x346=
IKsin(j6);
8492 IkReal x347=
IKcos(j6);
8493 IkReal x348=((1.0)*sj4);
8494 IkReal x349=((1.0)*x347);
8495 IkReal x350=(sj4*x346);
8496 IkReal x351=(sj4*x347);
8497 IkReal x352=(cj4*x346);
8498 IkReal x353=(cj4*x349);
8499 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x346);
8500 evalcond[1]=(x351+x352+new_r01);
8501 evalcond[2]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x349)));
8502 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x348))+(((-1.0)*x346)));
8503 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x348))+(((-1.0)*x349)));
8504 evalcond[5]=((((-1.0)*x353))+x350+new_r00);
8505 evalcond[6]=((((-1.0)*x353))+x350+new_r11);
8506 evalcond[7]=((((-1.0)*x352))+(((-1.0)*x347*x348))+new_r10);
8514 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8515 vinfos[0].jointtype = 17;
8516 vinfos[0].foffset = j0;
8517 vinfos[0].indices[0] = _ij0[0];
8518 vinfos[0].indices[1] = _ij0[1];
8519 vinfos[0].maxsolutions = _nj0;
8520 vinfos[1].jointtype = 1;
8521 vinfos[1].foffset = j1;
8522 vinfos[1].indices[0] = _ij1[0];
8523 vinfos[1].indices[1] = _ij1[1];
8524 vinfos[1].maxsolutions = _nj1;
8525 vinfos[2].jointtype = 1;
8526 vinfos[2].foffset = j2;
8527 vinfos[2].indices[0] = _ij2[0];
8528 vinfos[2].indices[1] = _ij2[1];
8529 vinfos[2].maxsolutions = _nj2;
8530 vinfos[3].jointtype = 1;
8531 vinfos[3].foffset = j3;
8532 vinfos[3].indices[0] = _ij3[0];
8533 vinfos[3].indices[1] = _ij3[1];
8534 vinfos[3].maxsolutions = _nj3;
8535 vinfos[4].jointtype = 1;
8536 vinfos[4].foffset = j4;
8537 vinfos[4].indices[0] = _ij4[0];
8538 vinfos[4].indices[1] = _ij4[1];
8539 vinfos[4].maxsolutions = _nj4;
8540 vinfos[5].jointtype = 1;
8541 vinfos[5].foffset = j5;
8542 vinfos[5].indices[0] = _ij5[0];
8543 vinfos[5].indices[1] = _ij5[1];
8544 vinfos[5].maxsolutions = _nj5;
8545 vinfos[6].jointtype = 1;
8546 vinfos[6].foffset = j6;
8547 vinfos[6].indices[0] = _ij6[0];
8548 vinfos[6].indices[1] = _ij6[1];
8549 vinfos[6].maxsolutions = _nj6;
8550 std::vector<int> vfree(0);
8558 if( bgotonextstatement )
8560 bool bgotonextstatement =
true;
8563 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8564 evalcond[1]=new_r20;
8565 evalcond[2]=new_r02;
8566 evalcond[3]=new_r12;
8567 evalcond[4]=new_r21;
8568 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 )
8570 bgotonextstatement=
false;
8572 IkReal j6array[1], cj6array[1], sj6array[1];
8573 bool j6valid[1]={
false};
8575 IkReal x354=((1.0)*new_r00);
8578 j6array[0]=
IKatan2((((cj4*new_r01))+(((-1.0)*sj4*x354))), ((((-1.0)*new_r01*sj4))+(((-1.0)*cj4*x354))));
8579 sj6array[0]=
IKsin(j6array[0]);
8580 cj6array[0]=
IKcos(j6array[0]);
8581 if( j6array[0] >
IKPI )
8585 else if( j6array[0] < -
IKPI )
8586 { j6array[0]+=
IK2PI;
8589 for(
int ij6 = 0; ij6 < 1; ++ij6)
8595 _ij6[0] = ij6; _ij6[1] = -1;
8596 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8600 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8603 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8606 IkReal x355=
IKcos(j6);
8607 IkReal x356=
IKsin(j6);
8608 IkReal x357=((1.0)*sj4);
8609 IkReal x358=((1.0)*x356);
8610 IkReal x359=(sj4*x355);
8611 IkReal x360=((1.0)*x355);
8612 IkReal x361=(cj4*x358);
8613 evalcond[0]=(((cj4*new_r00))+((new_r10*sj4))+x355);
8614 evalcond[1]=(((cj4*new_r01))+((new_r11*sj4))+(((-1.0)*x358)));
8615 evalcond[2]=(((sj4*x356))+((cj4*x355))+new_r00);
8616 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x358))+(((-1.0)*new_r00*x357)));
8617 evalcond[4]=((((-1.0)*x360))+((cj4*new_r11))+(((-1.0)*new_r01*x357)));
8618 evalcond[5]=((((-1.0)*x361))+x359+new_r01);
8619 evalcond[6]=((((-1.0)*x361))+x359+new_r10);
8620 evalcond[7]=((((-1.0)*cj4*x360))+(((-1.0)*x356*x357))+new_r11);
8628 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8629 vinfos[0].jointtype = 17;
8630 vinfos[0].foffset = j0;
8631 vinfos[0].indices[0] = _ij0[0];
8632 vinfos[0].indices[1] = _ij0[1];
8633 vinfos[0].maxsolutions = _nj0;
8634 vinfos[1].jointtype = 1;
8635 vinfos[1].foffset = j1;
8636 vinfos[1].indices[0] = _ij1[0];
8637 vinfos[1].indices[1] = _ij1[1];
8638 vinfos[1].maxsolutions = _nj1;
8639 vinfos[2].jointtype = 1;
8640 vinfos[2].foffset = j2;
8641 vinfos[2].indices[0] = _ij2[0];
8642 vinfos[2].indices[1] = _ij2[1];
8643 vinfos[2].maxsolutions = _nj2;
8644 vinfos[3].jointtype = 1;
8645 vinfos[3].foffset = j3;
8646 vinfos[3].indices[0] = _ij3[0];
8647 vinfos[3].indices[1] = _ij3[1];
8648 vinfos[3].maxsolutions = _nj3;
8649 vinfos[4].jointtype = 1;
8650 vinfos[4].foffset = j4;
8651 vinfos[4].indices[0] = _ij4[0];
8652 vinfos[4].indices[1] = _ij4[1];
8653 vinfos[4].maxsolutions = _nj4;
8654 vinfos[5].jointtype = 1;
8655 vinfos[5].foffset = j5;
8656 vinfos[5].indices[0] = _ij5[0];
8657 vinfos[5].indices[1] = _ij5[1];
8658 vinfos[5].maxsolutions = _nj5;
8659 vinfos[6].jointtype = 1;
8660 vinfos[6].foffset = j6;
8661 vinfos[6].indices[0] = _ij6[0];
8662 vinfos[6].indices[1] = _ij6[1];
8663 vinfos[6].maxsolutions = _nj6;
8664 std::vector<int> vfree(0);
8672 if( bgotonextstatement )
8674 bool bgotonextstatement =
true;
8677 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
8678 evalcond[1]=new_r12;
8679 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8681 bgotonextstatement=
false;
8683 IkReal j6array[1], cj6array[1], sj6array[1];
8684 bool j6valid[1]={
false};
8688 j6array[0]=
IKatan2(new_r10, new_r11);
8689 sj6array[0]=
IKsin(j6array[0]);
8690 cj6array[0]=
IKcos(j6array[0]);
8691 if( j6array[0] >
IKPI )
8695 else if( j6array[0] < -
IKPI )
8696 { j6array[0]+=
IK2PI;
8699 for(
int ij6 = 0; ij6 < 1; ++ij6)
8705 _ij6[0] = ij6; _ij6[1] = -1;
8706 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8710 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8713 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8716 IkReal x362=
IKcos(j6);
8717 IkReal x363=
IKsin(j6);
8718 IkReal x364=((1.0)*sj5);
8719 IkReal x365=((1.0)*x362);
8720 IkReal x366=((1.0)*x363);
8721 evalcond[0]=(new_r20+((new_r02*x362)));
8722 evalcond[1]=((((-1.0)*x366))+new_r10);
8723 evalcond[2]=((((-1.0)*x365))+new_r11);
8724 evalcond[3]=(((cj5*x363))+new_r01);
8725 evalcond[4]=(new_r21+(((-1.0)*new_r02*x366)));
8726 evalcond[5]=((((-1.0)*cj5*x365))+new_r00);
8727 evalcond[6]=(((cj5*new_r01))+x363+(((-1.0)*new_r21*x364)));
8728 evalcond[7]=((((-1.0)*new_r20*x364))+(((-1.0)*x365))+((cj5*new_r00)));
8736 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8737 vinfos[0].jointtype = 17;
8738 vinfos[0].foffset = j0;
8739 vinfos[0].indices[0] = _ij0[0];
8740 vinfos[0].indices[1] = _ij0[1];
8741 vinfos[0].maxsolutions = _nj0;
8742 vinfos[1].jointtype = 1;
8743 vinfos[1].foffset = j1;
8744 vinfos[1].indices[0] = _ij1[0];
8745 vinfos[1].indices[1] = _ij1[1];
8746 vinfos[1].maxsolutions = _nj1;
8747 vinfos[2].jointtype = 1;
8748 vinfos[2].foffset = j2;
8749 vinfos[2].indices[0] = _ij2[0];
8750 vinfos[2].indices[1] = _ij2[1];
8751 vinfos[2].maxsolutions = _nj2;
8752 vinfos[3].jointtype = 1;
8753 vinfos[3].foffset = j3;
8754 vinfos[3].indices[0] = _ij3[0];
8755 vinfos[3].indices[1] = _ij3[1];
8756 vinfos[3].maxsolutions = _nj3;
8757 vinfos[4].jointtype = 1;
8758 vinfos[4].foffset = j4;
8759 vinfos[4].indices[0] = _ij4[0];
8760 vinfos[4].indices[1] = _ij4[1];
8761 vinfos[4].maxsolutions = _nj4;
8762 vinfos[5].jointtype = 1;
8763 vinfos[5].foffset = j5;
8764 vinfos[5].indices[0] = _ij5[0];
8765 vinfos[5].indices[1] = _ij5[1];
8766 vinfos[5].maxsolutions = _nj5;
8767 vinfos[6].jointtype = 1;
8768 vinfos[6].foffset = j6;
8769 vinfos[6].indices[0] = _ij6[0];
8770 vinfos[6].indices[1] = _ij6[1];
8771 vinfos[6].maxsolutions = _nj6;
8772 std::vector<int> vfree(0);
8780 if( bgotonextstatement )
8782 bool bgotonextstatement =
true;
8785 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
8786 evalcond[1]=new_r12;
8787 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8789 bgotonextstatement=
false;
8794 j4=3.14159265358979;
8796 j6eval[1]=
IKsign(new_r02);
8797 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
8798 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
8804 j4=3.14159265358979;
8806 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
8812 j4=3.14159265358979;
8815 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
8819 bool bgotonextstatement =
true;
8822 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
8823 evalcond[1]=new_r22;
8824 evalcond[2]=new_r01;
8825 evalcond[3]=new_r00;
8826 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
8828 bgotonextstatement=
false;
8830 IkReal j6array[1], cj6array[1], sj6array[1];
8831 bool j6valid[1]={
false};
8835 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
8836 sj6array[0]=
IKsin(j6array[0]);
8837 cj6array[0]=
IKcos(j6array[0]);
8838 if( j6array[0] >
IKPI )
8842 else if( j6array[0] < -
IKPI )
8843 { j6array[0]+=
IK2PI;
8846 for(
int ij6 = 0; ij6 < 1; ++ij6)
8852 _ij6[0] = ij6; _ij6[1] = -1;
8853 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8857 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8860 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8863 IkReal x367=
IKcos(j6);
8864 IkReal x368=((1.0)*(
IKsin(j6)));
8865 evalcond[0]=(x367+new_r20);
8866 evalcond[1]=((((-1.0)*x368))+new_r21);
8867 evalcond[2]=((((-1.0)*x368))+(((-1.0)*new_r10)));
8868 evalcond[3]=((((-1.0)*x367))+(((-1.0)*new_r11)));
8876 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8877 vinfos[0].jointtype = 17;
8878 vinfos[0].foffset = j0;
8879 vinfos[0].indices[0] = _ij0[0];
8880 vinfos[0].indices[1] = _ij0[1];
8881 vinfos[0].maxsolutions = _nj0;
8882 vinfos[1].jointtype = 1;
8883 vinfos[1].foffset = j1;
8884 vinfos[1].indices[0] = _ij1[0];
8885 vinfos[1].indices[1] = _ij1[1];
8886 vinfos[1].maxsolutions = _nj1;
8887 vinfos[2].jointtype = 1;
8888 vinfos[2].foffset = j2;
8889 vinfos[2].indices[0] = _ij2[0];
8890 vinfos[2].indices[1] = _ij2[1];
8891 vinfos[2].maxsolutions = _nj2;
8892 vinfos[3].jointtype = 1;
8893 vinfos[3].foffset = j3;
8894 vinfos[3].indices[0] = _ij3[0];
8895 vinfos[3].indices[1] = _ij3[1];
8896 vinfos[3].maxsolutions = _nj3;
8897 vinfos[4].jointtype = 1;
8898 vinfos[4].foffset = j4;
8899 vinfos[4].indices[0] = _ij4[0];
8900 vinfos[4].indices[1] = _ij4[1];
8901 vinfos[4].maxsolutions = _nj4;
8902 vinfos[5].jointtype = 1;
8903 vinfos[5].foffset = j5;
8904 vinfos[5].indices[0] = _ij5[0];
8905 vinfos[5].indices[1] = _ij5[1];
8906 vinfos[5].maxsolutions = _nj5;
8907 vinfos[6].jointtype = 1;
8908 vinfos[6].foffset = j6;
8909 vinfos[6].indices[0] = _ij6[0];
8910 vinfos[6].indices[1] = _ij6[1];
8911 vinfos[6].maxsolutions = _nj6;
8912 std::vector<int> vfree(0);
8920 if( bgotonextstatement )
8922 bool bgotonextstatement =
true;
8925 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
8926 evalcond[1]=new_r22;
8927 evalcond[2]=new_r01;
8928 evalcond[3]=new_r00;
8929 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
8931 bgotonextstatement=
false;
8933 IkReal j6array[1], cj6array[1], sj6array[1];
8934 bool j6valid[1]={
false};
8938 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
8939 sj6array[0]=
IKsin(j6array[0]);
8940 cj6array[0]=
IKcos(j6array[0]);
8941 if( j6array[0] >
IKPI )
8945 else if( j6array[0] < -
IKPI )
8946 { j6array[0]+=
IK2PI;
8949 for(
int ij6 = 0; ij6 < 1; ++ij6)
8955 _ij6[0] = ij6; _ij6[1] = -1;
8956 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8960 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8963 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8966 IkReal x369=
IKsin(j6);
8967 IkReal x370=((1.0)*(
IKcos(j6)));
8968 evalcond[0]=(x369+new_r21);
8969 evalcond[1]=((((-1.0)*x370))+new_r20);
8970 evalcond[2]=((((-1.0)*x369))+(((-1.0)*new_r10)));
8971 evalcond[3]=((((-1.0)*x370))+(((-1.0)*new_r11)));
8979 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8980 vinfos[0].jointtype = 17;
8981 vinfos[0].foffset = j0;
8982 vinfos[0].indices[0] = _ij0[0];
8983 vinfos[0].indices[1] = _ij0[1];
8984 vinfos[0].maxsolutions = _nj0;
8985 vinfos[1].jointtype = 1;
8986 vinfos[1].foffset = j1;
8987 vinfos[1].indices[0] = _ij1[0];
8988 vinfos[1].indices[1] = _ij1[1];
8989 vinfos[1].maxsolutions = _nj1;
8990 vinfos[2].jointtype = 1;
8991 vinfos[2].foffset = j2;
8992 vinfos[2].indices[0] = _ij2[0];
8993 vinfos[2].indices[1] = _ij2[1];
8994 vinfos[2].maxsolutions = _nj2;
8995 vinfos[3].jointtype = 1;
8996 vinfos[3].foffset = j3;
8997 vinfos[3].indices[0] = _ij3[0];
8998 vinfos[3].indices[1] = _ij3[1];
8999 vinfos[3].maxsolutions = _nj3;
9000 vinfos[4].jointtype = 1;
9001 vinfos[4].foffset = j4;
9002 vinfos[4].indices[0] = _ij4[0];
9003 vinfos[4].indices[1] = _ij4[1];
9004 vinfos[4].maxsolutions = _nj4;
9005 vinfos[5].jointtype = 1;
9006 vinfos[5].foffset = j5;
9007 vinfos[5].indices[0] = _ij5[0];
9008 vinfos[5].indices[1] = _ij5[1];
9009 vinfos[5].maxsolutions = _nj5;
9010 vinfos[6].jointtype = 1;
9011 vinfos[6].foffset = j6;
9012 vinfos[6].indices[0] = _ij6[0];
9013 vinfos[6].indices[1] = _ij6[1];
9014 vinfos[6].maxsolutions = _nj6;
9015 std::vector<int> vfree(0);
9023 if( bgotonextstatement )
9025 bool bgotonextstatement =
true;
9028 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
9029 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9031 bgotonextstatement=
false;
9033 IkReal j6array[1], cj6array[1], sj6array[1];
9034 bool j6valid[1]={
false};
9038 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
9039 sj6array[0]=
IKsin(j6array[0]);
9040 cj6array[0]=
IKcos(j6array[0]);
9041 if( j6array[0] >
IKPI )
9045 else if( j6array[0] < -
IKPI )
9046 { j6array[0]+=
IK2PI;
9049 for(
int ij6 = 0; ij6 < 1; ++ij6)
9055 _ij6[0] = ij6; _ij6[1] = -1;
9056 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9060 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9063 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9066 IkReal x371=
IKsin(j6);
9067 IkReal x372=
IKcos(j6);
9068 IkReal x373=((-1.0)*x372);
9070 evalcond[1]=(new_r22*x371);
9072 evalcond[3]=(new_r22*x373);
9073 evalcond[4]=((((-1.0)*x371))+(((-1.0)*new_r10)));
9074 evalcond[5]=((((-1.0)*x372))+(((-1.0)*new_r11)));
9082 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9083 vinfos[0].jointtype = 17;
9084 vinfos[0].foffset = j0;
9085 vinfos[0].indices[0] = _ij0[0];
9086 vinfos[0].indices[1] = _ij0[1];
9087 vinfos[0].maxsolutions = _nj0;
9088 vinfos[1].jointtype = 1;
9089 vinfos[1].foffset = j1;
9090 vinfos[1].indices[0] = _ij1[0];
9091 vinfos[1].indices[1] = _ij1[1];
9092 vinfos[1].maxsolutions = _nj1;
9093 vinfos[2].jointtype = 1;
9094 vinfos[2].foffset = j2;
9095 vinfos[2].indices[0] = _ij2[0];
9096 vinfos[2].indices[1] = _ij2[1];
9097 vinfos[2].maxsolutions = _nj2;
9098 vinfos[3].jointtype = 1;
9099 vinfos[3].foffset = j3;
9100 vinfos[3].indices[0] = _ij3[0];
9101 vinfos[3].indices[1] = _ij3[1];
9102 vinfos[3].maxsolutions = _nj3;
9103 vinfos[4].jointtype = 1;
9104 vinfos[4].foffset = j4;
9105 vinfos[4].indices[0] = _ij4[0];
9106 vinfos[4].indices[1] = _ij4[1];
9107 vinfos[4].maxsolutions = _nj4;
9108 vinfos[5].jointtype = 1;
9109 vinfos[5].foffset = j5;
9110 vinfos[5].indices[0] = _ij5[0];
9111 vinfos[5].indices[1] = _ij5[1];
9112 vinfos[5].maxsolutions = _nj5;
9113 vinfos[6].jointtype = 1;
9114 vinfos[6].foffset = j6;
9115 vinfos[6].indices[0] = _ij6[0];
9116 vinfos[6].indices[1] = _ij6[1];
9117 vinfos[6].maxsolutions = _nj6;
9118 std::vector<int> vfree(0);
9126 if( bgotonextstatement )
9128 bool bgotonextstatement =
true;
9133 bgotonextstatement=
false;
9138 if( bgotonextstatement )
9149 IkReal j6array[1], cj6array[1], sj6array[1];
9150 bool j6valid[1]={
false};
9162 j6array[0]=
IKatan2(((-1.0)*new_r21*(x374.
value)), ((-1.0)*new_r00*(x375.
value)));
9163 sj6array[0]=
IKsin(j6array[0]);
9164 cj6array[0]=
IKcos(j6array[0]);
9165 if( j6array[0] >
IKPI )
9169 else if( j6array[0] < -
IKPI )
9170 { j6array[0]+=
IK2PI;
9173 for(
int ij6 = 0; ij6 < 1; ++ij6)
9179 _ij6[0] = ij6; _ij6[1] = -1;
9180 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9184 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9187 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9190 IkReal x376=
IKsin(j6);
9191 IkReal x377=
IKcos(j6);
9196 IkReal x378=x382.
value;
9197 IkReal x379=new_r22*new_r22;
9198 IkReal x380=((1.0)*x377);
9199 IkReal x381=(x378*x379);
9200 evalcond[0]=(new_r21+((new_r02*x376)));
9201 evalcond[1]=((((-1.0)*new_r02*x380))+new_r20);
9202 evalcond[2]=((((-1.0)*x376))+(((-1.0)*new_r10)));
9203 evalcond[3]=((((-1.0)*x380))+(((-1.0)*new_r11)));
9204 evalcond[4]=(((cj5*x376))+(((-1.0)*new_r01)));
9205 evalcond[5]=((((-1.0)*cj5*x380))+(((-1.0)*new_r00)));
9206 evalcond[6]=(x376+((new_r02*new_r21))+((new_r21*x381)));
9207 evalcond[7]=((((-1.0)*x380))+((new_r02*new_r20))+((new_r20*x381)));
9215 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9216 vinfos[0].jointtype = 17;
9217 vinfos[0].foffset = j0;
9218 vinfos[0].indices[0] = _ij0[0];
9219 vinfos[0].indices[1] = _ij0[1];
9220 vinfos[0].maxsolutions = _nj0;
9221 vinfos[1].jointtype = 1;
9222 vinfos[1].foffset = j1;
9223 vinfos[1].indices[0] = _ij1[0];
9224 vinfos[1].indices[1] = _ij1[1];
9225 vinfos[1].maxsolutions = _nj1;
9226 vinfos[2].jointtype = 1;
9227 vinfos[2].foffset = j2;
9228 vinfos[2].indices[0] = _ij2[0];
9229 vinfos[2].indices[1] = _ij2[1];
9230 vinfos[2].maxsolutions = _nj2;
9231 vinfos[3].jointtype = 1;
9232 vinfos[3].foffset = j3;
9233 vinfos[3].indices[0] = _ij3[0];
9234 vinfos[3].indices[1] = _ij3[1];
9235 vinfos[3].maxsolutions = _nj3;
9236 vinfos[4].jointtype = 1;
9237 vinfos[4].foffset = j4;
9238 vinfos[4].indices[0] = _ij4[0];
9239 vinfos[4].indices[1] = _ij4[1];
9240 vinfos[4].maxsolutions = _nj4;
9241 vinfos[5].jointtype = 1;
9242 vinfos[5].foffset = j5;
9243 vinfos[5].indices[0] = _ij5[0];
9244 vinfos[5].indices[1] = _ij5[1];
9245 vinfos[5].maxsolutions = _nj5;
9246 vinfos[6].jointtype = 1;
9247 vinfos[6].foffset = j6;
9248 vinfos[6].indices[0] = _ij6[0];
9249 vinfos[6].indices[1] = _ij6[1];
9250 vinfos[6].maxsolutions = _nj6;
9251 std::vector<int> vfree(0);
9264 IkReal j6array[1], cj6array[1], sj6array[1];
9265 bool j6valid[1]={
false};
9273 j6array[0]=
IKatan2(((-1.0)*new_r10), (new_r20*(x383.
value)));
9274 sj6array[0]=
IKsin(j6array[0]);
9275 cj6array[0]=
IKcos(j6array[0]);
9276 if( j6array[0] >
IKPI )
9280 else if( j6array[0] < -
IKPI )
9281 { j6array[0]+=
IK2PI;
9284 for(
int ij6 = 0; ij6 < 1; ++ij6)
9290 _ij6[0] = ij6; _ij6[1] = -1;
9291 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9295 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9298 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9301 IkReal x384=
IKsin(j6);
9302 IkReal x385=
IKcos(j6);
9307 IkReal x386=x390.
value;
9308 IkReal x387=new_r22*new_r22;
9309 IkReal x388=((1.0)*x385);
9310 IkReal x389=(x386*x387);
9311 evalcond[0]=(((new_r02*x384))+new_r21);
9312 evalcond[1]=((((-1.0)*new_r02*x388))+new_r20);
9313 evalcond[2]=((((-1.0)*x384))+(((-1.0)*new_r10)));
9314 evalcond[3]=((((-1.0)*x388))+(((-1.0)*new_r11)));
9315 evalcond[4]=(((cj5*x384))+(((-1.0)*new_r01)));
9316 evalcond[5]=((((-1.0)*cj5*x388))+(((-1.0)*new_r00)));
9317 evalcond[6]=(x384+((new_r02*new_r21))+((new_r21*x389)));
9318 evalcond[7]=((((-1.0)*x388))+((new_r02*new_r20))+((new_r20*x389)));
9326 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9327 vinfos[0].jointtype = 17;
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 vinfos[6].jointtype = 1;
9358 vinfos[6].foffset = j6;
9359 vinfos[6].indices[0] = _ij6[0];
9360 vinfos[6].indices[1] = _ij6[1];
9361 vinfos[6].maxsolutions = _nj6;
9362 std::vector<int> vfree(0);
9375 IkReal j6array[1], cj6array[1], sj6array[1];
9376 bool j6valid[1]={
false};
9386 j6array[0]=((-1.5707963267949)+(x391.
value)+(((1.5707963267949)*(x392.
value))));
9387 sj6array[0]=
IKsin(j6array[0]);
9388 cj6array[0]=
IKcos(j6array[0]);
9389 if( j6array[0] >
IKPI )
9393 else if( j6array[0] < -
IKPI )
9394 { j6array[0]+=
IK2PI;
9397 for(
int ij6 = 0; ij6 < 1; ++ij6)
9403 _ij6[0] = ij6; _ij6[1] = -1;
9404 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9408 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9411 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9414 IkReal x393=
IKsin(j6);
9415 IkReal x394=
IKcos(j6);
9420 IkReal x395=x399.
value;
9421 IkReal x396=new_r22*new_r22;
9422 IkReal x397=((1.0)*x394);
9423 IkReal x398=(x395*x396);
9424 evalcond[0]=(new_r21+((new_r02*x393)));
9425 evalcond[1]=(new_r20+(((-1.0)*new_r02*x397)));
9426 evalcond[2]=((((-1.0)*x393))+(((-1.0)*new_r10)));
9427 evalcond[3]=((((-1.0)*x397))+(((-1.0)*new_r11)));
9428 evalcond[4]=(((cj5*x393))+(((-1.0)*new_r01)));
9429 evalcond[5]=((((-1.0)*cj5*x397))+(((-1.0)*new_r00)));
9430 evalcond[6]=(x393+((new_r21*x398))+((new_r02*new_r21)));
9431 evalcond[7]=((((-1.0)*x397))+((new_r20*x398))+((new_r02*new_r20)));
9439 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9440 vinfos[0].jointtype = 17;
9441 vinfos[0].foffset = j0;
9442 vinfos[0].indices[0] = _ij0[0];
9443 vinfos[0].indices[1] = _ij0[1];
9444 vinfos[0].maxsolutions = _nj0;
9445 vinfos[1].jointtype = 1;
9446 vinfos[1].foffset = j1;
9447 vinfos[1].indices[0] = _ij1[0];
9448 vinfos[1].indices[1] = _ij1[1];
9449 vinfos[1].maxsolutions = _nj1;
9450 vinfos[2].jointtype = 1;
9451 vinfos[2].foffset = j2;
9452 vinfos[2].indices[0] = _ij2[0];
9453 vinfos[2].indices[1] = _ij2[1];
9454 vinfos[2].maxsolutions = _nj2;
9455 vinfos[3].jointtype = 1;
9456 vinfos[3].foffset = j3;
9457 vinfos[3].indices[0] = _ij3[0];
9458 vinfos[3].indices[1] = _ij3[1];
9459 vinfos[3].maxsolutions = _nj3;
9460 vinfos[4].jointtype = 1;
9461 vinfos[4].foffset = j4;
9462 vinfos[4].indices[0] = _ij4[0];
9463 vinfos[4].indices[1] = _ij4[1];
9464 vinfos[4].maxsolutions = _nj4;
9465 vinfos[5].jointtype = 1;
9466 vinfos[5].foffset = j5;
9467 vinfos[5].indices[0] = _ij5[0];
9468 vinfos[5].indices[1] = _ij5[1];
9469 vinfos[5].maxsolutions = _nj5;
9470 vinfos[6].jointtype = 1;
9471 vinfos[6].foffset = j6;
9472 vinfos[6].indices[0] = _ij6[0];
9473 vinfos[6].indices[1] = _ij6[1];
9474 vinfos[6].maxsolutions = _nj6;
9475 std::vector<int> vfree(0);
9487 if( bgotonextstatement )
9489 bool bgotonextstatement =
true;
9492 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
9493 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9495 bgotonextstatement=
false;
9503 if(
IKabs(j6eval[0]) < 0.0000000100000000 )
9509 IkReal op[2+1], zeror[2];
9514 polyroots2(op,zeror,numroots);
9515 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
9516 int numsolutions = 0;
9517 for(
int ij6 = 0; ij6 < numroots; ++ij6)
9519 IkReal htj6 = zeror[ij6];
9520 tempj6array[0]=((2.0)*(
atan(htj6)));
9521 for(
int kj6 = 0; kj6 < 1; ++kj6)
9523 j6array[numsolutions] = tempj6array[kj6];
9524 if( j6array[numsolutions] >
IKPI )
9526 j6array[numsolutions]-=
IK2PI;
9528 else if( j6array[numsolutions] < -
IKPI )
9530 j6array[numsolutions]+=
IK2PI;
9532 sj6array[numsolutions] =
IKsin(j6array[numsolutions]);
9533 cj6array[numsolutions] =
IKcos(j6array[numsolutions]);
9537 bool j6valid[2]={
true,
true};
9539 for(
int ij6 = 0; ij6 < numsolutions; ++ij6)
9545 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9548 _ij6[0] = ij6; _ij6[1] = -1;
9549 for(
int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
9553 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9558 vinfos[0].jointtype = 17;
9559 vinfos[0].foffset = j0;
9560 vinfos[0].indices[0] = _ij0[0];
9561 vinfos[0].indices[1] = _ij0[1];
9562 vinfos[0].maxsolutions = _nj0;
9563 vinfos[1].jointtype = 1;
9564 vinfos[1].foffset = j1;
9565 vinfos[1].indices[0] = _ij1[0];
9566 vinfos[1].indices[1] = _ij1[1];
9567 vinfos[1].maxsolutions = _nj1;
9568 vinfos[2].jointtype = 1;
9569 vinfos[2].foffset = j2;
9570 vinfos[2].indices[0] = _ij2[0];
9571 vinfos[2].indices[1] = _ij2[1];
9572 vinfos[2].maxsolutions = _nj2;
9573 vinfos[3].jointtype = 1;
9574 vinfos[3].foffset = j3;
9575 vinfos[3].indices[0] = _ij3[0];
9576 vinfos[3].indices[1] = _ij3[1];
9577 vinfos[3].maxsolutions = _nj3;
9578 vinfos[4].jointtype = 1;
9579 vinfos[4].foffset = j4;
9580 vinfos[4].indices[0] = _ij4[0];
9581 vinfos[4].indices[1] = _ij4[1];
9582 vinfos[4].maxsolutions = _nj4;
9583 vinfos[5].jointtype = 1;
9584 vinfos[5].foffset = j5;
9585 vinfos[5].indices[0] = _ij5[0];
9586 vinfos[5].indices[1] = _ij5[1];
9587 vinfos[5].maxsolutions = _nj5;
9588 vinfos[6].jointtype = 1;
9589 vinfos[6].foffset = j6;
9590 vinfos[6].indices[0] = _ij6[0];
9591 vinfos[6].indices[1] = _ij6[1];
9592 vinfos[6].maxsolutions = _nj6;
9593 std::vector<int> vfree(0);
9604 if( bgotonextstatement )
9606 bool bgotonextstatement =
true;
9611 bgotonextstatement=
false;
9616 if( bgotonextstatement )
9633 IkReal j6array[1], cj6array[1], sj6array[1];
9634 bool j6valid[1]={
false};
9640 IkReal x400=x401.
value;
9651 j6array[0]=
IKatan2((x400*(x402.
value)*(x403.
value)*((((new_r20*sj4))+(((-1.0)*new_r01*sj5))))), ((-1.0)*new_r20*x400));
9652 sj6array[0]=
IKsin(j6array[0]);
9653 cj6array[0]=
IKcos(j6array[0]);
9654 if( j6array[0] >
IKPI )
9658 else if( j6array[0] < -
IKPI )
9659 { j6array[0]+=
IK2PI;
9662 for(
int ij6 = 0; ij6 < 1; ++ij6)
9668 _ij6[0] = ij6; _ij6[1] = -1;
9669 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9673 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9676 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9678 IkReal evalcond[12];
9679 IkReal x404=
IKsin(j6);
9680 IkReal x405=
IKcos(j6);
9681 IkReal x406=((1.0)*sj5);
9682 IkReal x407=((1.0)*sj4);
9683 IkReal x408=(cj5*sj4);
9684 IkReal x409=(cj4*new_r01);
9685 IkReal x410=(cj4*new_r00);
9686 IkReal x411=((1.0)*x405);
9687 IkReal x412=(cj5*x404);
9688 IkReal x413=((1.0)*x404);
9689 evalcond[0]=(((sj5*x405))+new_r20);
9690 evalcond[1]=((((-1.0)*x404*x406))+new_r21);
9691 evalcond[2]=(((new_r11*sj4))+x409+x412);
9692 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x407))+(((-1.0)*x413)));
9693 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x411))+(((-1.0)*new_r01*x407)));
9694 evalcond[5]=(((cj4*x412))+new_r01+((sj4*x405)));
9695 evalcond[6]=(((new_r10*sj4))+x410+(((-1.0)*cj5*x411)));
9696 evalcond[7]=((((-1.0)*cj4*cj5*x411))+new_r00+((sj4*x404)));
9697 evalcond[8]=(((x404*x408))+(((-1.0)*cj4*x411))+new_r11);
9698 evalcond[9]=((((-1.0)*cj4*x413))+new_r10+(((-1.0)*cj5*x405*x407)));
9699 evalcond[10]=(((cj5*x409))+x404+(((-1.0)*new_r21*x406))+((new_r11*x408)));
9700 evalcond[11]=((((-1.0)*new_r20*x406))+((cj5*x410))+((new_r10*x408))+(((-1.0)*x411)));
9708 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9709 vinfos[0].jointtype = 17;
9710 vinfos[0].foffset = j0;
9711 vinfos[0].indices[0] = _ij0[0];
9712 vinfos[0].indices[1] = _ij0[1];
9713 vinfos[0].maxsolutions = _nj0;
9714 vinfos[1].jointtype = 1;
9715 vinfos[1].foffset = j1;
9716 vinfos[1].indices[0] = _ij1[0];
9717 vinfos[1].indices[1] = _ij1[1];
9718 vinfos[1].maxsolutions = _nj1;
9719 vinfos[2].jointtype = 1;
9720 vinfos[2].foffset = j2;
9721 vinfos[2].indices[0] = _ij2[0];
9722 vinfos[2].indices[1] = _ij2[1];
9723 vinfos[2].maxsolutions = _nj2;
9724 vinfos[3].jointtype = 1;
9725 vinfos[3].foffset = j3;
9726 vinfos[3].indices[0] = _ij3[0];
9727 vinfos[3].indices[1] = _ij3[1];
9728 vinfos[3].maxsolutions = _nj3;
9729 vinfos[4].jointtype = 1;
9730 vinfos[4].foffset = j4;
9731 vinfos[4].indices[0] = _ij4[0];
9732 vinfos[4].indices[1] = _ij4[1];
9733 vinfos[4].maxsolutions = _nj4;
9734 vinfos[5].jointtype = 1;
9735 vinfos[5].foffset = j5;
9736 vinfos[5].indices[0] = _ij5[0];
9737 vinfos[5].indices[1] = _ij5[1];
9738 vinfos[5].maxsolutions = _nj5;
9739 vinfos[6].jointtype = 1;
9740 vinfos[6].foffset = j6;
9741 vinfos[6].indices[0] = _ij6[0];
9742 vinfos[6].indices[1] = _ij6[1];
9743 vinfos[6].maxsolutions = _nj6;
9744 std::vector<int> vfree(0);
9757 IkReal j6array[1], cj6array[1], sj6array[1];
9758 bool j6valid[1]={
false};
9764 IkReal x414=x415.
value;
9771 j6array[0]=
IKatan2((x414*(x416.
value)*(((((-1.0)*new_r00*sj5))+(((-1.0)*cj4*cj5*new_r20))))), ((-1.0)*new_r20*x414));
9772 sj6array[0]=
IKsin(j6array[0]);
9773 cj6array[0]=
IKcos(j6array[0]);
9774 if( j6array[0] >
IKPI )
9778 else if( j6array[0] < -
IKPI )
9779 { j6array[0]+=
IK2PI;
9782 for(
int ij6 = 0; ij6 < 1; ++ij6)
9788 _ij6[0] = ij6; _ij6[1] = -1;
9789 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9793 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9796 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9798 IkReal evalcond[12];
9799 IkReal x417=
IKsin(j6);
9800 IkReal x418=
IKcos(j6);
9801 IkReal x419=((1.0)*sj5);
9802 IkReal x420=((1.0)*sj4);
9803 IkReal x421=(cj5*sj4);
9804 IkReal x422=(cj4*new_r01);
9805 IkReal x423=(cj4*new_r00);
9806 IkReal x424=((1.0)*x418);
9807 IkReal x425=(cj5*x417);
9808 IkReal x426=((1.0)*x417);
9809 evalcond[0]=(((sj5*x418))+new_r20);
9810 evalcond[1]=((((-1.0)*x417*x419))+new_r21);
9811 evalcond[2]=(((new_r11*sj4))+x425+x422);
9812 evalcond[3]=((((-1.0)*new_r00*x420))+((cj4*new_r10))+(((-1.0)*x426)));
9813 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x424))+(((-1.0)*new_r01*x420)));
9814 evalcond[5]=(((cj4*x425))+new_r01+((sj4*x418)));
9815 evalcond[6]=(((new_r10*sj4))+x423+(((-1.0)*cj5*x424)));
9816 evalcond[7]=((((-1.0)*cj4*cj5*x424))+new_r00+((sj4*x417)));
9817 evalcond[8]=((((-1.0)*cj4*x424))+new_r11+((x417*x421)));
9818 evalcond[9]=((((-1.0)*cj4*x426))+new_r10+(((-1.0)*cj5*x418*x420)));
9819 evalcond[10]=(((cj5*x422))+((new_r11*x421))+x417+(((-1.0)*new_r21*x419)));
9820 evalcond[11]=((((-1.0)*x424))+((cj5*x423))+((new_r10*x421))+(((-1.0)*new_r20*x419)));
9828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9829 vinfos[0].jointtype = 17;
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 vinfos[6].jointtype = 1;
9860 vinfos[6].foffset = j6;
9861 vinfos[6].indices[0] = _ij6[0];
9862 vinfos[6].indices[1] = _ij6[1];
9863 vinfos[6].maxsolutions = _nj6;
9864 std::vector<int> vfree(0);
9877 IkReal j6array[1], cj6array[1], sj6array[1];
9878 bool j6valid[1]={
false};
9888 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x427.
value)))+(x428.
value));
9889 sj6array[0]=
IKsin(j6array[0]);
9890 cj6array[0]=
IKcos(j6array[0]);
9891 if( j6array[0] >
IKPI )
9895 else if( j6array[0] < -
IKPI )
9896 { j6array[0]+=
IK2PI;
9899 for(
int ij6 = 0; ij6 < 1; ++ij6)
9905 _ij6[0] = ij6; _ij6[1] = -1;
9906 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9910 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9913 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9915 IkReal evalcond[12];
9916 IkReal x429=
IKsin(j6);
9917 IkReal x430=
IKcos(j6);
9918 IkReal x431=((1.0)*sj5);
9919 IkReal x432=((1.0)*sj4);
9920 IkReal x433=(cj5*sj4);
9921 IkReal x434=(cj4*new_r01);
9922 IkReal x435=(cj4*new_r00);
9923 IkReal x436=((1.0)*x430);
9924 IkReal x437=(cj5*x429);
9925 IkReal x438=((1.0)*x429);
9926 evalcond[0]=(new_r20+((sj5*x430)));
9927 evalcond[1]=((((-1.0)*x429*x431))+new_r21);
9928 evalcond[2]=(((new_r11*sj4))+x434+x437);
9929 evalcond[3]=((((-1.0)*new_r00*x432))+(((-1.0)*x438))+((cj4*new_r10)));
9930 evalcond[4]=((((-1.0)*x436))+(((-1.0)*new_r01*x432))+((cj4*new_r11)));
9931 evalcond[5]=(((sj4*x430))+((cj4*x437))+new_r01);
9932 evalcond[6]=(((new_r10*sj4))+(((-1.0)*cj5*x436))+x435);
9933 evalcond[7]=(((sj4*x429))+new_r00+(((-1.0)*cj4*cj5*x436)));
9934 evalcond[8]=(((x429*x433))+(((-1.0)*cj4*x436))+new_r11);
9935 evalcond[9]=((((-1.0)*cj4*x438))+(((-1.0)*cj5*x430*x432))+new_r10);
9936 evalcond[10]=(((cj5*x434))+((new_r11*x433))+x429+(((-1.0)*new_r21*x431)));
9937 evalcond[11]=((((-1.0)*x436))+((cj5*x435))+((new_r10*x433))+(((-1.0)*new_r20*x431)));
9945 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9946 vinfos[0].jointtype = 17;
9947 vinfos[0].foffset = j0;
9948 vinfos[0].indices[0] = _ij0[0];
9949 vinfos[0].indices[1] = _ij0[1];
9950 vinfos[0].maxsolutions = _nj0;
9951 vinfos[1].jointtype = 1;
9952 vinfos[1].foffset = j1;
9953 vinfos[1].indices[0] = _ij1[0];
9954 vinfos[1].indices[1] = _ij1[1];
9955 vinfos[1].maxsolutions = _nj1;
9956 vinfos[2].jointtype = 1;
9957 vinfos[2].foffset = j2;
9958 vinfos[2].indices[0] = _ij2[0];
9959 vinfos[2].indices[1] = _ij2[1];
9960 vinfos[2].maxsolutions = _nj2;
9961 vinfos[3].jointtype = 1;
9962 vinfos[3].foffset = j3;
9963 vinfos[3].indices[0] = _ij3[0];
9964 vinfos[3].indices[1] = _ij3[1];
9965 vinfos[3].maxsolutions = _nj3;
9966 vinfos[4].jointtype = 1;
9967 vinfos[4].foffset = j4;
9968 vinfos[4].indices[0] = _ij4[0];
9969 vinfos[4].indices[1] = _ij4[1];
9970 vinfos[4].maxsolutions = _nj4;
9971 vinfos[5].jointtype = 1;
9972 vinfos[5].foffset = j5;
9973 vinfos[5].indices[0] = _ij5[0];
9974 vinfos[5].indices[1] = _ij5[1];
9975 vinfos[5].maxsolutions = _nj5;
9976 vinfos[6].jointtype = 1;
9977 vinfos[6].foffset = j6;
9978 vinfos[6].indices[0] = _ij6[0];
9979 vinfos[6].indices[1] = _ij6[1];
9980 vinfos[6].maxsolutions = _nj6;
9981 std::vector<int> vfree(0);
10000 IkReal j6array[1], cj6array[1], sj6array[1];
10001 bool j6valid[1]={
false};
10011 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x439.
value)))+(x440.
value));
10012 sj6array[0]=
IKsin(j6array[0]);
10013 cj6array[0]=
IKcos(j6array[0]);
10014 if( j6array[0] >
IKPI )
10018 else if( j6array[0] < -
IKPI )
10019 { j6array[0]+=
IK2PI;
10022 for(
int ij6 = 0; ij6 < 1; ++ij6)
10024 if( !j6valid[ij6] )
10028 _ij6[0] = ij6; _ij6[1] = -1;
10029 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10033 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10036 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10038 IkReal evalcond[2];
10039 evalcond[0]=(new_r20+((sj5*(
IKcos(j6)))));
10040 evalcond[1]=((((-1.0)*sj5*(
IKsin(j6))))+new_r21);
10050 j4eval[1]=((
IKabs(new_r12))+(
IKabs(new_r02)));
10052 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 ||
IKabs(j4eval[2]) < 0.0000010000000000 )
10058 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 )
10061 IkReal evalcond[5];
10062 bool bgotonextstatement =
true;
10065 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j6)))), 6.28318530717959)));
10066 evalcond[1]=new_r20;
10067 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
10069 bgotonextstatement=
false;
10071 IkReal j4array[1], cj4array[1], sj4array[1];
10072 bool j4valid[1]={
false};
10076 j4array[0]=
IKatan2(((-1.0)*new_r00), new_r10);
10077 sj4array[0]=
IKsin(j4array[0]);
10078 cj4array[0]=
IKcos(j4array[0]);
10079 if( j4array[0] >
IKPI )
10083 else if( j4array[0] < -
IKPI )
10084 { j4array[0]+=
IK2PI;
10087 for(
int ij4 = 0; ij4 < 1; ++ij4)
10089 if( !j4valid[ij4] )
10093 _ij4[0] = ij4; _ij4[1] = -1;
10094 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
10098 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10101 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10103 IkReal evalcond[18];
10104 IkReal x441=
IKsin(j4);
10105 IkReal x442=
IKcos(j4);
10106 IkReal x443=((1.0)*sj5);
10107 IkReal x444=(new_r22*x441);
10108 IkReal x445=(new_r11*x441);
10109 IkReal x446=(new_r02*x442);
10110 IkReal x447=(new_r22*x442);
10111 IkReal x448=(new_r12*x441);
10112 IkReal x449=(sj5*x442);
10113 IkReal x450=((1.0)*x441);
10114 IkReal x451=(new_r10*x441);
10115 evalcond[0]=(x441+new_r00);
10116 evalcond[1]=(x447+new_r01);
10117 evalcond[2]=(x444+new_r11);
10118 evalcond[3]=((((-1.0)*x442))+new_r10);
10119 evalcond[4]=(new_r02+(((-1.0)*x442*x443)));
10120 evalcond[5]=(new_r12+(((-1.0)*x441*x443)));
10121 evalcond[6]=(((new_r00*x442))+x451);
10122 evalcond[7]=(((new_r12*x442))+(((-1.0)*new_r02*x450)));
10123 evalcond[8]=((((-1.0)*new_r01*x450))+((new_r11*x442)));
10124 evalcond[9]=(((new_r01*x442))+x445+new_r22);
10125 evalcond[10]=((-1.0)+(((-1.0)*new_r00*x450))+((new_r10*x442)));
10126 evalcond[11]=(((new_r00*x449))+((sj5*x451)));
10127 evalcond[12]=(((new_r00*x447))+((new_r10*x444)));
10128 evalcond[13]=((((-1.0)*x443))+x448+x446);
10129 evalcond[14]=(((new_r01*x449))+((cj5*new_r21))+((sj5*x445)));
10130 evalcond[15]=((-1.0)+(new_r22*new_r22)+((sj5*x448))+((sj5*x446)));
10131 evalcond[16]=(((new_r22*x446))+((new_r12*x444))+(((-1.0)*new_r22*x443)));
10132 evalcond[17]=((1.0)+((new_r01*x447))+(((-1.0)*sj5*x443))+((new_r11*x444)));
10140 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10141 vinfos[0].jointtype = 17;
10142 vinfos[0].foffset = j0;
10143 vinfos[0].indices[0] = _ij0[0];
10144 vinfos[0].indices[1] = _ij0[1];
10145 vinfos[0].maxsolutions = _nj0;
10146 vinfos[1].jointtype = 1;
10147 vinfos[1].foffset = j1;
10148 vinfos[1].indices[0] = _ij1[0];
10149 vinfos[1].indices[1] = _ij1[1];
10150 vinfos[1].maxsolutions = _nj1;
10151 vinfos[2].jointtype = 1;
10152 vinfos[2].foffset = j2;
10153 vinfos[2].indices[0] = _ij2[0];
10154 vinfos[2].indices[1] = _ij2[1];
10155 vinfos[2].maxsolutions = _nj2;
10156 vinfos[3].jointtype = 1;
10157 vinfos[3].foffset = j3;
10158 vinfos[3].indices[0] = _ij3[0];
10159 vinfos[3].indices[1] = _ij3[1];
10160 vinfos[3].maxsolutions = _nj3;
10161 vinfos[4].jointtype = 1;
10162 vinfos[4].foffset = j4;
10163 vinfos[4].indices[0] = _ij4[0];
10164 vinfos[4].indices[1] = _ij4[1];
10165 vinfos[4].maxsolutions = _nj4;
10166 vinfos[5].jointtype = 1;
10167 vinfos[5].foffset = j5;
10168 vinfos[5].indices[0] = _ij5[0];
10169 vinfos[5].indices[1] = _ij5[1];
10170 vinfos[5].maxsolutions = _nj5;
10171 vinfos[6].jointtype = 1;
10172 vinfos[6].foffset = j6;
10173 vinfos[6].indices[0] = _ij6[0];
10174 vinfos[6].indices[1] = _ij6[1];
10175 vinfos[6].maxsolutions = _nj6;
10176 std::vector<int> vfree(0);
10184 if( bgotonextstatement )
10186 bool bgotonextstatement =
true;
10189 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j6)))), 6.28318530717959)));
10190 evalcond[1]=new_r20;
10191 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
10193 bgotonextstatement=
false;
10195 IkReal j4array[1], cj4array[1], sj4array[1];
10196 bool j4valid[1]={
false};
10200 j4array[0]=
IKatan2(new_r00, ((-1.0)*new_r10));
10201 sj4array[0]=
IKsin(j4array[0]);
10202 cj4array[0]=
IKcos(j4array[0]);
10203 if( j4array[0] >
IKPI )
10207 else if( j4array[0] < -
IKPI )
10208 { j4array[0]+=
IK2PI;
10211 for(
int ij4 = 0; ij4 < 1; ++ij4)
10213 if( !j4valid[ij4] )
10217 _ij4[0] = ij4; _ij4[1] = -1;
10218 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
10222 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10225 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10227 IkReal evalcond[18];
10228 IkReal x452=
IKcos(j4);
10229 IkReal x453=
IKsin(j4);
10230 IkReal x454=((1.0)*sj5);
10231 IkReal x455=((1.0)*x452);
10232 IkReal x456=(new_r22*x453);
10233 IkReal x457=(new_r11*x453);
10234 IkReal x458=(new_r02*x452);
10235 IkReal x459=(new_r01*x452);
10236 IkReal x460=(new_r12*x453);
10237 IkReal x461=(new_r00*x452);
10238 IkReal x462=((1.0)*x453);
10239 IkReal x463=(new_r10*x453);
10240 evalcond[0]=(x452+new_r10);
10241 evalcond[1]=((((-1.0)*x462))+new_r00);
10242 evalcond[2]=((((-1.0)*x452*x454))+new_r02);
10243 evalcond[3]=((((-1.0)*x453*x454))+new_r12);
10244 evalcond[4]=((((-1.0)*new_r22*x455))+new_r01);
10245 evalcond[5]=((((-1.0)*x456))+new_r11);
10246 evalcond[6]=(x463+x461);
10247 evalcond[7]=(((new_r12*x452))+(((-1.0)*new_r02*x462)));
10248 evalcond[8]=(((new_r11*x452))+(((-1.0)*new_r01*x462)));
10249 evalcond[9]=((1.0)+((new_r10*x452))+(((-1.0)*new_r00*x462)));
10250 evalcond[10]=(((sj5*x463))+((sj5*x461)));
10251 evalcond[11]=(((new_r10*x456))+((new_r22*x461)));
10252 evalcond[12]=((((-1.0)*x454))+x458+x460);
10253 evalcond[13]=(x459+x457+(((-1.0)*new_r22)));
10254 evalcond[14]=(((cj5*new_r21))+((sj5*x459))+((sj5*x457)));
10255 evalcond[15]=((-1.0)+(new_r22*new_r22)+((sj5*x460))+((sj5*x458)));
10256 evalcond[16]=(((new_r12*x456))+(((-1.0)*new_r22*x454))+((new_r22*x458)));
10257 evalcond[17]=((-1.0)+((new_r11*x456))+((new_r22*x459))+(sj5*sj5));
10265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10266 vinfos[0].jointtype = 17;
10267 vinfos[0].foffset = j0;
10268 vinfos[0].indices[0] = _ij0[0];
10269 vinfos[0].indices[1] = _ij0[1];
10270 vinfos[0].maxsolutions = _nj0;
10271 vinfos[1].jointtype = 1;
10272 vinfos[1].foffset = j1;
10273 vinfos[1].indices[0] = _ij1[0];
10274 vinfos[1].indices[1] = _ij1[1];
10275 vinfos[1].maxsolutions = _nj1;
10276 vinfos[2].jointtype = 1;
10277 vinfos[2].foffset = j2;
10278 vinfos[2].indices[0] = _ij2[0];
10279 vinfos[2].indices[1] = _ij2[1];
10280 vinfos[2].maxsolutions = _nj2;
10281 vinfos[3].jointtype = 1;
10282 vinfos[3].foffset = j3;
10283 vinfos[3].indices[0] = _ij3[0];
10284 vinfos[3].indices[1] = _ij3[1];
10285 vinfos[3].maxsolutions = _nj3;
10286 vinfos[4].jointtype = 1;
10287 vinfos[4].foffset = j4;
10288 vinfos[4].indices[0] = _ij4[0];
10289 vinfos[4].indices[1] = _ij4[1];
10290 vinfos[4].maxsolutions = _nj4;
10291 vinfos[5].jointtype = 1;
10292 vinfos[5].foffset = j5;
10293 vinfos[5].indices[0] = _ij5[0];
10294 vinfos[5].indices[1] = _ij5[1];
10295 vinfos[5].maxsolutions = _nj5;
10296 vinfos[6].jointtype = 1;
10297 vinfos[6].foffset = j6;
10298 vinfos[6].indices[0] = _ij6[0];
10299 vinfos[6].indices[1] = _ij6[1];
10300 vinfos[6].maxsolutions = _nj6;
10301 std::vector<int> vfree(0);
10309 if( bgotonextstatement )
10311 bool bgotonextstatement =
true;
10314 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
10315 evalcond[1]=new_r20;
10316 evalcond[2]=new_r02;
10317 evalcond[3]=new_r12;
10318 evalcond[4]=new_r21;
10319 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 )
10321 bgotonextstatement=
false;
10323 IkReal j4array[1], cj4array[1], sj4array[1];
10324 bool j4valid[1]={
false};
10326 IkReal x464=((1.0)*sj6);
10329 j4array[0]=
IKatan2(((((-1.0)*cj6*new_r01))+(((-1.0)*new_r00*x464))), ((((-1.0)*new_r01*x464))+((cj6*new_r00))));
10330 sj4array[0]=
IKsin(j4array[0]);
10331 cj4array[0]=
IKcos(j4array[0]);
10332 if( j4array[0] >
IKPI )
10336 else if( j4array[0] < -
IKPI )
10337 { j4array[0]+=
IK2PI;
10340 for(
int ij4 = 0; ij4 < 1; ++ij4)
10342 if( !j4valid[ij4] )
10346 _ij4[0] = ij4; _ij4[1] = -1;
10347 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
10351 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10354 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10356 IkReal evalcond[8];
10357 IkReal x465=
IKcos(j4);
10358 IkReal x466=
IKsin(j4);
10359 IkReal x467=((1.0)*cj6);
10360 IkReal x468=(sj6*x466);
10361 IkReal x469=(cj6*x466);
10362 IkReal x470=(sj6*x465);
10363 IkReal x471=((1.0)*x466);
10364 IkReal x472=(x465*x467);
10365 evalcond[0]=(((new_r11*x466))+sj6+((new_r01*x465)));
10366 evalcond[1]=(x469+x470+new_r01);
10367 evalcond[2]=((((-1.0)*x472))+x468+new_r00);
10368 evalcond[3]=((((-1.0)*x472))+x468+new_r11);
10369 evalcond[4]=(((new_r10*x466))+((new_r00*x465))+(((-1.0)*x467)));
10370 evalcond[5]=((((-1.0)*x466*x467))+(((-1.0)*x470))+new_r10);
10371 evalcond[6]=((((-1.0)*sj6))+((new_r10*x465))+(((-1.0)*new_r00*x471)));
10372 evalcond[7]=(((new_r11*x465))+(((-1.0)*x467))+(((-1.0)*new_r01*x471)));
10380 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10381 vinfos[0].jointtype = 17;
10382 vinfos[0].foffset = j0;
10383 vinfos[0].indices[0] = _ij0[0];
10384 vinfos[0].indices[1] = _ij0[1];
10385 vinfos[0].maxsolutions = _nj0;
10386 vinfos[1].jointtype = 1;
10387 vinfos[1].foffset = j1;
10388 vinfos[1].indices[0] = _ij1[0];
10389 vinfos[1].indices[1] = _ij1[1];
10390 vinfos[1].maxsolutions = _nj1;
10391 vinfos[2].jointtype = 1;
10392 vinfos[2].foffset = j2;
10393 vinfos[2].indices[0] = _ij2[0];
10394 vinfos[2].indices[1] = _ij2[1];
10395 vinfos[2].maxsolutions = _nj2;
10396 vinfos[3].jointtype = 1;
10397 vinfos[3].foffset = j3;
10398 vinfos[3].indices[0] = _ij3[0];
10399 vinfos[3].indices[1] = _ij3[1];
10400 vinfos[3].maxsolutions = _nj3;
10401 vinfos[4].jointtype = 1;
10402 vinfos[4].foffset = j4;
10403 vinfos[4].indices[0] = _ij4[0];
10404 vinfos[4].indices[1] = _ij4[1];
10405 vinfos[4].maxsolutions = _nj4;
10406 vinfos[5].jointtype = 1;
10407 vinfos[5].foffset = j5;
10408 vinfos[5].indices[0] = _ij5[0];
10409 vinfos[5].indices[1] = _ij5[1];
10410 vinfos[5].maxsolutions = _nj5;
10411 vinfos[6].jointtype = 1;
10412 vinfos[6].foffset = j6;
10413 vinfos[6].indices[0] = _ij6[0];
10414 vinfos[6].indices[1] = _ij6[1];
10415 vinfos[6].maxsolutions = _nj6;
10416 std::vector<int> vfree(0);
10424 if( bgotonextstatement )
10426 bool bgotonextstatement =
true;
10429 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
10430 evalcond[1]=new_r20;
10431 evalcond[2]=new_r02;
10432 evalcond[3]=new_r12;
10433 evalcond[4]=new_r21;
10434 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 )
10436 bgotonextstatement=
false;
10438 IkReal j4array[1], cj4array[1], sj4array[1];
10439 bool j4valid[1]={
false};
10441 IkReal x473=((1.0)*new_r00);
10444 j4array[0]=
IKatan2(((((-1.0)*cj6*new_r01))+(((-1.0)*sj6*x473))), (((new_r01*sj6))+(((-1.0)*cj6*x473))));
10445 sj4array[0]=
IKsin(j4array[0]);
10446 cj4array[0]=
IKcos(j4array[0]);
10447 if( j4array[0] >
IKPI )
10451 else if( j4array[0] < -
IKPI )
10452 { j4array[0]+=
IK2PI;
10455 for(
int ij4 = 0; ij4 < 1; ++ij4)
10457 if( !j4valid[ij4] )
10461 _ij4[0] = ij4; _ij4[1] = -1;
10462 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
10466 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10469 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10471 IkReal evalcond[8];
10472 IkReal x474=
IKsin(j4);
10473 IkReal x475=
IKcos(j4);
10474 IkReal x476=((1.0)*sj6);
10475 IkReal x477=(cj6*x474);
10476 IkReal x478=(cj6*x475);
10477 IkReal x479=((1.0)*x474);
10478 IkReal x480=(x475*x476);
10479 evalcond[0]=(cj6+((new_r10*x474))+((new_r00*x475)));
10480 evalcond[1]=(((sj6*x474))+x478+new_r00);
10481 evalcond[2]=((((-1.0)*x480))+x477+new_r01);
10482 evalcond[3]=((((-1.0)*x480))+x477+new_r10);
10483 evalcond[4]=(((new_r01*x475))+((new_r11*x474))+(((-1.0)*x476)));
10484 evalcond[5]=((((-1.0)*x478))+new_r11+(((-1.0)*x474*x476)));
10485 evalcond[6]=(((new_r10*x475))+(((-1.0)*new_r00*x479))+(((-1.0)*x476)));
10486 evalcond[7]=(((new_r11*x475))+(((-1.0)*new_r01*x479))+(((-1.0)*cj6)));
10494 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10495 vinfos[0].jointtype = 17;
10496 vinfos[0].foffset = j0;
10497 vinfos[0].indices[0] = _ij0[0];
10498 vinfos[0].indices[1] = _ij0[1];
10499 vinfos[0].maxsolutions = _nj0;
10500 vinfos[1].jointtype = 1;
10501 vinfos[1].foffset = j1;
10502 vinfos[1].indices[0] = _ij1[0];
10503 vinfos[1].indices[1] = _ij1[1];
10504 vinfos[1].maxsolutions = _nj1;
10505 vinfos[2].jointtype = 1;
10506 vinfos[2].foffset = j2;
10507 vinfos[2].indices[0] = _ij2[0];
10508 vinfos[2].indices[1] = _ij2[1];
10509 vinfos[2].maxsolutions = _nj2;
10510 vinfos[3].jointtype = 1;
10511 vinfos[3].foffset = j3;
10512 vinfos[3].indices[0] = _ij3[0];
10513 vinfos[3].indices[1] = _ij3[1];
10514 vinfos[3].maxsolutions = _nj3;
10515 vinfos[4].jointtype = 1;
10516 vinfos[4].foffset = j4;
10517 vinfos[4].indices[0] = _ij4[0];
10518 vinfos[4].indices[1] = _ij4[1];
10519 vinfos[4].maxsolutions = _nj4;
10520 vinfos[5].jointtype = 1;
10521 vinfos[5].foffset = j5;
10522 vinfos[5].indices[0] = _ij5[0];
10523 vinfos[5].indices[1] = _ij5[1];
10524 vinfos[5].maxsolutions = _nj5;
10525 vinfos[6].jointtype = 1;
10526 vinfos[6].foffset = j6;
10527 vinfos[6].indices[0] = _ij6[0];
10528 vinfos[6].indices[1] = _ij6[1];
10529 vinfos[6].maxsolutions = _nj6;
10530 std::vector<int> vfree(0);
10538 if( bgotonextstatement )
10540 bool bgotonextstatement =
true;
10543 evalcond[0]=((
IKabs(new_r12))+(
IKabs(new_r02)));
10544 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
10546 bgotonextstatement=
false;
10553 j4eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
10554 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
10562 j4eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
10563 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
10571 j4eval[0]=((
IKabs((new_r11*new_r22)))+(
IKabs((new_r01*new_r22))));
10572 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
10579 IkReal j4array[2], cj4array[2], sj4array[2];
10580 bool j4valid[2]={
false};
10586 IkReal x481=x482.
value;
10587 j4array[0]=((-1.0)*x481);
10588 sj4array[0]=
IKsin(j4array[0]);
10589 cj4array[0]=
IKcos(j4array[0]);
10590 j4array[1]=((3.14159265358979)+(((-1.0)*x481)));
10591 sj4array[1]=
IKsin(j4array[1]);
10592 cj4array[1]=
IKcos(j4array[1]);
10593 if( j4array[0] >
IKPI )
10597 else if( j4array[0] < -
IKPI )
10598 { j4array[0]+=
IK2PI;
10601 if( j4array[1] >
IKPI )
10605 else if( j4array[1] < -
IKPI )
10606 { j4array[1]+=
IK2PI;
10609 for(
int ij4 = 0; ij4 < 2; ++ij4)
10611 if( !j4valid[ij4] )
10615 _ij4[0] = ij4; _ij4[1] = -1;
10616 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
10620 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10623 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10625 IkReal evalcond[5];
10626 IkReal x483=
IKcos(j4);
10627 IkReal x484=
IKsin(j4);
10628 IkReal x485=(new_r00*x483);
10629 IkReal x486=(new_r10*x484);
10630 IkReal x487=((1.0)*x484);
10631 evalcond[0]=(((new_r11*x484))+((new_r01*x483)));
10632 evalcond[1]=(x485+x486);
10633 evalcond[2]=((((-1.0)*new_r00*x487))+((new_r10*x483)));
10634 evalcond[3]=((((-1.0)*new_r01*x487))+((new_r11*x483)));
10635 evalcond[4]=(((new_r22*x485))+((new_r22*x486)));
10643 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10644 vinfos[0].jointtype = 17;
10645 vinfos[0].foffset = j0;
10646 vinfos[0].indices[0] = _ij0[0];
10647 vinfos[0].indices[1] = _ij0[1];
10648 vinfos[0].maxsolutions = _nj0;
10649 vinfos[1].jointtype = 1;
10650 vinfos[1].foffset = j1;
10651 vinfos[1].indices[0] = _ij1[0];
10652 vinfos[1].indices[1] = _ij1[1];
10653 vinfos[1].maxsolutions = _nj1;
10654 vinfos[2].jointtype = 1;
10655 vinfos[2].foffset = j2;
10656 vinfos[2].indices[0] = _ij2[0];
10657 vinfos[2].indices[1] = _ij2[1];
10658 vinfos[2].maxsolutions = _nj2;
10659 vinfos[3].jointtype = 1;
10660 vinfos[3].foffset = j3;
10661 vinfos[3].indices[0] = _ij3[0];
10662 vinfos[3].indices[1] = _ij3[1];
10663 vinfos[3].maxsolutions = _nj3;
10664 vinfos[4].jointtype = 1;
10665 vinfos[4].foffset = j4;
10666 vinfos[4].indices[0] = _ij4[0];
10667 vinfos[4].indices[1] = _ij4[1];
10668 vinfos[4].maxsolutions = _nj4;
10669 vinfos[5].jointtype = 1;
10670 vinfos[5].foffset = j5;
10671 vinfos[5].indices[0] = _ij5[0];
10672 vinfos[5].indices[1] = _ij5[1];
10673 vinfos[5].maxsolutions = _nj5;
10674 vinfos[6].jointtype = 1;
10675 vinfos[6].foffset = j6;
10676 vinfos[6].indices[0] = _ij6[0];
10677 vinfos[6].indices[1] = _ij6[1];
10678 vinfos[6].maxsolutions = _nj6;
10679 std::vector<int> vfree(0);
10692 IkReal j4array[2], cj4array[2], sj4array[2];
10693 bool j4valid[2]={
false};
10699 IkReal x488=x489.
value;
10700 j4array[0]=((-1.0)*x488);
10701 sj4array[0]=
IKsin(j4array[0]);
10702 cj4array[0]=
IKcos(j4array[0]);
10703 j4array[1]=((3.14159265358979)+(((-1.0)*x488)));
10704 sj4array[1]=
IKsin(j4array[1]);
10705 cj4array[1]=
IKcos(j4array[1]);
10706 if( j4array[0] >
IKPI )
10710 else if( j4array[0] < -
IKPI )
10711 { j4array[0]+=
IK2PI;
10714 if( j4array[1] >
IKPI )
10718 else if( j4array[1] < -
IKPI )
10719 { j4array[1]+=
IK2PI;
10722 for(
int ij4 = 0; ij4 < 2; ++ij4)
10724 if( !j4valid[ij4] )
10728 _ij4[0] = ij4; _ij4[1] = -1;
10729 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
10733 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10736 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10738 IkReal evalcond[5];
10739 IkReal x490=
IKcos(j4);
10740 IkReal x491=
IKsin(j4);
10741 IkReal x492=(new_r01*x490);
10742 IkReal x493=(new_r11*x491);
10743 IkReal x494=((1.0)*x491);
10744 evalcond[0]=(x492+x493);
10745 evalcond[1]=((((-1.0)*new_r00*x494))+((new_r10*x490)));
10746 evalcond[2]=((((-1.0)*new_r01*x494))+((new_r11*x490)));
10747 evalcond[3]=(((new_r22*x492))+((new_r22*x493)));
10748 evalcond[4]=(((new_r00*new_r22*x490))+((new_r10*new_r22*x491)));
10756 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10757 vinfos[0].jointtype = 17;
10758 vinfos[0].foffset = j0;
10759 vinfos[0].indices[0] = _ij0[0];
10760 vinfos[0].indices[1] = _ij0[1];
10761 vinfos[0].maxsolutions = _nj0;
10762 vinfos[1].jointtype = 1;
10763 vinfos[1].foffset = j1;
10764 vinfos[1].indices[0] = _ij1[0];
10765 vinfos[1].indices[1] = _ij1[1];
10766 vinfos[1].maxsolutions = _nj1;
10767 vinfos[2].jointtype = 1;
10768 vinfos[2].foffset = j2;
10769 vinfos[2].indices[0] = _ij2[0];
10770 vinfos[2].indices[1] = _ij2[1];
10771 vinfos[2].maxsolutions = _nj2;
10772 vinfos[3].jointtype = 1;
10773 vinfos[3].foffset = j3;
10774 vinfos[3].indices[0] = _ij3[0];
10775 vinfos[3].indices[1] = _ij3[1];
10776 vinfos[3].maxsolutions = _nj3;
10777 vinfos[4].jointtype = 1;
10778 vinfos[4].foffset = j4;
10779 vinfos[4].indices[0] = _ij4[0];
10780 vinfos[4].indices[1] = _ij4[1];
10781 vinfos[4].maxsolutions = _nj4;
10782 vinfos[5].jointtype = 1;
10783 vinfos[5].foffset = j5;
10784 vinfos[5].indices[0] = _ij5[0];
10785 vinfos[5].indices[1] = _ij5[1];
10786 vinfos[5].maxsolutions = _nj5;
10787 vinfos[6].jointtype = 1;
10788 vinfos[6].foffset = j6;
10789 vinfos[6].indices[0] = _ij6[0];
10790 vinfos[6].indices[1] = _ij6[1];
10791 vinfos[6].maxsolutions = _nj6;
10792 std::vector<int> vfree(0);
10805 IkReal j4array[2], cj4array[2], sj4array[2];
10806 bool j4valid[2]={
false};
10812 IkReal x495=x496.
value;
10813 j4array[0]=((-1.0)*x495);
10814 sj4array[0]=
IKsin(j4array[0]);
10815 cj4array[0]=
IKcos(j4array[0]);
10816 j4array[1]=((3.14159265358979)+(((-1.0)*x495)));
10817 sj4array[1]=
IKsin(j4array[1]);
10818 cj4array[1]=
IKcos(j4array[1]);
10819 if( j4array[0] >
IKPI )
10823 else if( j4array[0] < -
IKPI )
10824 { j4array[0]+=
IK2PI;
10827 if( j4array[1] >
IKPI )
10831 else if( j4array[1] < -
IKPI )
10832 { j4array[1]+=
IK2PI;
10835 for(
int ij4 = 0; ij4 < 2; ++ij4)
10837 if( !j4valid[ij4] )
10841 _ij4[0] = ij4; _ij4[1] = -1;
10842 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
10846 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10849 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10851 IkReal evalcond[5];
10852 IkReal x497=
IKcos(j4);
10853 IkReal x498=
IKsin(j4);
10854 IkReal x499=(new_r00*x497);
10855 IkReal x500=(new_r22*x498);
10856 IkReal x501=((1.0)*x498);
10857 evalcond[0]=(((new_r10*x498))+x499);
10858 evalcond[1]=((((-1.0)*new_r00*x501))+((new_r10*x497)));
10859 evalcond[2]=((((-1.0)*new_r01*x501))+((new_r11*x497)));
10860 evalcond[3]=(((new_r01*new_r22*x497))+((new_r11*x500)));
10861 evalcond[4]=(((new_r10*x500))+((new_r22*x499)));
10869 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10870 vinfos[0].jointtype = 17;
10871 vinfos[0].foffset = j0;
10872 vinfos[0].indices[0] = _ij0[0];
10873 vinfos[0].indices[1] = _ij0[1];
10874 vinfos[0].maxsolutions = _nj0;
10875 vinfos[1].jointtype = 1;
10876 vinfos[1].foffset = j1;
10877 vinfos[1].indices[0] = _ij1[0];
10878 vinfos[1].indices[1] = _ij1[1];
10879 vinfos[1].maxsolutions = _nj1;
10880 vinfos[2].jointtype = 1;
10881 vinfos[2].foffset = j2;
10882 vinfos[2].indices[0] = _ij2[0];
10883 vinfos[2].indices[1] = _ij2[1];
10884 vinfos[2].maxsolutions = _nj2;
10885 vinfos[3].jointtype = 1;
10886 vinfos[3].foffset = j3;
10887 vinfos[3].indices[0] = _ij3[0];
10888 vinfos[3].indices[1] = _ij3[1];
10889 vinfos[3].maxsolutions = _nj3;
10890 vinfos[4].jointtype = 1;
10891 vinfos[4].foffset = j4;
10892 vinfos[4].indices[0] = _ij4[0];
10893 vinfos[4].indices[1] = _ij4[1];
10894 vinfos[4].maxsolutions = _nj4;
10895 vinfos[5].jointtype = 1;
10896 vinfos[5].foffset = j5;
10897 vinfos[5].indices[0] = _ij5[0];
10898 vinfos[5].indices[1] = _ij5[1];
10899 vinfos[5].maxsolutions = _nj5;
10900 vinfos[6].jointtype = 1;
10901 vinfos[6].foffset = j6;
10902 vinfos[6].indices[0] = _ij6[0];
10903 vinfos[6].indices[1] = _ij6[1];
10904 vinfos[6].maxsolutions = _nj6;
10905 std::vector<int> vfree(0);
10917 if( bgotonextstatement )
10919 bool bgotonextstatement =
true;
10924 bgotonextstatement=
false;
10929 if( bgotonextstatement )
10942 IkReal j4array[1], cj4array[1], sj4array[1];
10943 bool j4valid[1]={
false};
10949 IkReal x502=x503.
value;
10956 j4array[0]=
IKatan2((x502*(x504.
value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*new_r02*sj6))))), (new_r02*x502));
10957 sj4array[0]=
IKsin(j4array[0]);
10958 cj4array[0]=
IKcos(j4array[0]);
10959 if( j4array[0] >
IKPI )
10963 else if( j4array[0] < -
IKPI )
10964 { j4array[0]+=
IK2PI;
10967 for(
int ij4 = 0; ij4 < 1; ++ij4)
10969 if( !j4valid[ij4] )
10973 _ij4[0] = ij4; _ij4[1] = -1;
10974 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
10978 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
10981 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10983 IkReal evalcond[18];
10984 IkReal x505=
IKcos(j4);
10985 IkReal x506=
IKsin(j4);
10986 IkReal x507=((1.0)*cj6);
10987 IkReal x508=(cj5*sj6);
10988 IkReal x509=((1.0)*sj5);
10989 IkReal x510=((1.0)*sj6);
10990 IkReal x511=(cj5*x506);
10991 IkReal x512=(new_r11*x506);
10992 IkReal x513=(cj5*x505);
10993 IkReal x514=(new_r02*x505);
10994 IkReal x515=(new_r12*x506);
10995 IkReal x516=(sj5*x505);
10996 IkReal x517=((1.0)*x506);
10997 IkReal x518=(new_r10*x506);
10998 evalcond[0]=((((-1.0)*x505*x509))+new_r02);
10999 evalcond[1]=((((-1.0)*x506*x509))+new_r12);
11000 evalcond[2]=(((new_r12*x505))+(((-1.0)*new_r02*x517)));
11001 evalcond[3]=(((cj6*x506))+((x505*x508))+new_r01);
11002 evalcond[4]=((((-1.0)*x509))+x515+x514);
11003 evalcond[5]=(x508+x512+((new_r01*x505)));
11004 evalcond[6]=(((sj6*x506))+new_r00+(((-1.0)*x507*x513)));
11005 evalcond[7]=((((-1.0)*x505*x507))+((x506*x508))+new_r11);
11006 evalcond[8]=(((new_r10*x505))+(((-1.0)*new_r00*x517))+(((-1.0)*x510)));
11007 evalcond[9]=((((-1.0)*x507))+((new_r11*x505))+(((-1.0)*new_r01*x517)));
11008 evalcond[10]=((((-1.0)*cj5*x507))+((new_r00*x505))+x518);
11009 evalcond[11]=((((-1.0)*x505*x510))+new_r10+(((-1.0)*x507*x511)));
11010 evalcond[12]=(((new_r00*x516))+((cj5*new_r20))+((sj5*x518)));
11011 evalcond[13]=(((new_r01*x516))+((cj5*new_r21))+((sj5*x512)));
11012 evalcond[14]=((-1.0)+((cj5*new_r22))+((sj5*x515))+((sj5*x514)));
11013 evalcond[15]=(((new_r02*x513))+((new_r12*x511))+(((-1.0)*new_r22*x509)));
11014 evalcond[16]=((((-1.0)*new_r21*x509))+((new_r01*x513))+((new_r11*x511))+sj6);
11015 evalcond[17]=(((new_r00*x513))+((new_r10*x511))+(((-1.0)*new_r20*x509))+(((-1.0)*x507)));
11023 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11024 vinfos[0].jointtype = 17;
11025 vinfos[0].foffset = j0;
11026 vinfos[0].indices[0] = _ij0[0];
11027 vinfos[0].indices[1] = _ij0[1];
11028 vinfos[0].maxsolutions = _nj0;
11029 vinfos[1].jointtype = 1;
11030 vinfos[1].foffset = j1;
11031 vinfos[1].indices[0] = _ij1[0];
11032 vinfos[1].indices[1] = _ij1[1];
11033 vinfos[1].maxsolutions = _nj1;
11034 vinfos[2].jointtype = 1;
11035 vinfos[2].foffset = j2;
11036 vinfos[2].indices[0] = _ij2[0];
11037 vinfos[2].indices[1] = _ij2[1];
11038 vinfos[2].maxsolutions = _nj2;
11039 vinfos[3].jointtype = 1;
11040 vinfos[3].foffset = j3;
11041 vinfos[3].indices[0] = _ij3[0];
11042 vinfos[3].indices[1] = _ij3[1];
11043 vinfos[3].maxsolutions = _nj3;
11044 vinfos[4].jointtype = 1;
11045 vinfos[4].foffset = j4;
11046 vinfos[4].indices[0] = _ij4[0];
11047 vinfos[4].indices[1] = _ij4[1];
11048 vinfos[4].maxsolutions = _nj4;
11049 vinfos[5].jointtype = 1;
11050 vinfos[5].foffset = j5;
11051 vinfos[5].indices[0] = _ij5[0];
11052 vinfos[5].indices[1] = _ij5[1];
11053 vinfos[5].maxsolutions = _nj5;
11054 vinfos[6].jointtype = 1;
11055 vinfos[6].foffset = j6;
11056 vinfos[6].indices[0] = _ij6[0];
11057 vinfos[6].indices[1] = _ij6[1];
11058 vinfos[6].maxsolutions = _nj6;
11059 std::vector<int> vfree(0);
11072 IkReal j4array[1], cj4array[1], sj4array[1];
11073 bool j4valid[1]={
false};
11083 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x519.
value)))+(x520.
value));
11084 sj4array[0]=
IKsin(j4array[0]);
11085 cj4array[0]=
IKcos(j4array[0]);
11086 if( j4array[0] >
IKPI )
11090 else if( j4array[0] < -
IKPI )
11091 { j4array[0]+=
IK2PI;
11094 for(
int ij4 = 0; ij4 < 1; ++ij4)
11096 if( !j4valid[ij4] )
11100 _ij4[0] = ij4; _ij4[1] = -1;
11101 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
11105 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
11108 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11110 IkReal evalcond[18];
11111 IkReal x521=
IKcos(j4);
11112 IkReal x522=
IKsin(j4);
11113 IkReal x523=((1.0)*cj6);
11114 IkReal x524=(cj5*sj6);
11115 IkReal x525=((1.0)*sj5);
11116 IkReal x526=((1.0)*sj6);
11117 IkReal x527=(cj5*x522);
11118 IkReal x528=(new_r11*x522);
11119 IkReal x529=(cj5*x521);
11120 IkReal x530=(new_r02*x521);
11121 IkReal x531=(new_r12*x522);
11122 IkReal x532=(sj5*x521);
11123 IkReal x533=((1.0)*x522);
11124 IkReal x534=(new_r10*x522);
11125 evalcond[0]=((((-1.0)*x521*x525))+new_r02);
11126 evalcond[1]=((((-1.0)*x522*x525))+new_r12);
11127 evalcond[2]=(((new_r12*x521))+(((-1.0)*new_r02*x533)));
11128 evalcond[3]=(((x521*x524))+new_r01+((cj6*x522)));
11129 evalcond[4]=((((-1.0)*x525))+x531+x530);
11130 evalcond[5]=(((new_r01*x521))+x524+x528);
11131 evalcond[6]=((((-1.0)*x523*x529))+((sj6*x522))+new_r00);
11132 evalcond[7]=(((x522*x524))+(((-1.0)*x521*x523))+new_r11);
11133 evalcond[8]=((((-1.0)*new_r00*x533))+(((-1.0)*x526))+((new_r10*x521)));
11134 evalcond[9]=(((new_r11*x521))+(((-1.0)*x523))+(((-1.0)*new_r01*x533)));
11135 evalcond[10]=(x534+(((-1.0)*cj5*x523))+((new_r00*x521)));
11136 evalcond[11]=((((-1.0)*x523*x527))+(((-1.0)*x521*x526))+new_r10);
11137 evalcond[12]=(((cj5*new_r20))+((sj5*x534))+((new_r00*x532)));
11138 evalcond[13]=(((new_r01*x532))+((sj5*x528))+((cj5*new_r21)));
11139 evalcond[14]=((-1.0)+((cj5*new_r22))+((sj5*x531))+((sj5*x530)));
11140 evalcond[15]=(((new_r02*x529))+(((-1.0)*new_r22*x525))+((new_r12*x527)));
11141 evalcond[16]=(((new_r01*x529))+sj6+((new_r11*x527))+(((-1.0)*new_r21*x525)));
11142 evalcond[17]=((((-1.0)*new_r20*x525))+(((-1.0)*x523))+((new_r10*x527))+((new_r00*x529)));
11150 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11151 vinfos[0].jointtype = 17;
11152 vinfos[0].foffset = j0;
11153 vinfos[0].indices[0] = _ij0[0];
11154 vinfos[0].indices[1] = _ij0[1];
11155 vinfos[0].maxsolutions = _nj0;
11156 vinfos[1].jointtype = 1;
11157 vinfos[1].foffset = j1;
11158 vinfos[1].indices[0] = _ij1[0];
11159 vinfos[1].indices[1] = _ij1[1];
11160 vinfos[1].maxsolutions = _nj1;
11161 vinfos[2].jointtype = 1;
11162 vinfos[2].foffset = j2;
11163 vinfos[2].indices[0] = _ij2[0];
11164 vinfos[2].indices[1] = _ij2[1];
11165 vinfos[2].maxsolutions = _nj2;
11166 vinfos[3].jointtype = 1;
11167 vinfos[3].foffset = j3;
11168 vinfos[3].indices[0] = _ij3[0];
11169 vinfos[3].indices[1] = _ij3[1];
11170 vinfos[3].maxsolutions = _nj3;
11171 vinfos[4].jointtype = 1;
11172 vinfos[4].foffset = j4;
11173 vinfos[4].indices[0] = _ij4[0];
11174 vinfos[4].indices[1] = _ij4[1];
11175 vinfos[4].maxsolutions = _nj4;
11176 vinfos[5].jointtype = 1;
11177 vinfos[5].foffset = j5;
11178 vinfos[5].indices[0] = _ij5[0];
11179 vinfos[5].indices[1] = _ij5[1];
11180 vinfos[5].maxsolutions = _nj5;
11181 vinfos[6].jointtype = 1;
11182 vinfos[6].foffset = j6;
11183 vinfos[6].indices[0] = _ij6[0];
11184 vinfos[6].indices[1] = _ij6[1];
11185 vinfos[6].maxsolutions = _nj6;
11186 std::vector<int> vfree(0);
11205 IkReal j4array[1], cj4array[1], sj4array[1];
11206 bool j4valid[1]={
false};
11216 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x535.
value)))+(x536.
value));
11217 sj4array[0]=
IKsin(j4array[0]);
11218 cj4array[0]=
IKcos(j4array[0]);
11219 if( j4array[0] >
IKPI )
11223 else if( j4array[0] < -
IKPI )
11224 { j4array[0]+=
IK2PI;
11227 for(
int ij4 = 0; ij4 < 1; ++ij4)
11229 if( !j4valid[ij4] )
11233 _ij4[0] = ij4; _ij4[1] = -1;
11234 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
11238 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
11241 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11243 IkReal evalcond[8];
11244 IkReal x537=
IKcos(j4);
11245 IkReal x538=
IKsin(j4);
11246 IkReal x539=((1.0)*sj5);
11247 IkReal x540=(new_r12*x538);
11248 IkReal x541=(new_r02*x537);
11249 IkReal x542=(sj5*x537);
11250 IkReal x543=(sj5*x538);
11251 evalcond[0]=((((-1.0)*x537*x539))+new_r02);
11252 evalcond[1]=((((-1.0)*x538*x539))+new_r12);
11253 evalcond[2]=(((new_r12*x537))+(((-1.0)*new_r02*x538)));
11254 evalcond[3]=((((-1.0)*x539))+x540+x541);
11255 evalcond[4]=(((new_r00*x542))+((new_r10*x543))+((cj5*new_r20)));
11256 evalcond[5]=(((new_r11*x543))+((cj5*new_r21))+((new_r01*x542)));
11257 evalcond[6]=((-1.0)+((cj5*new_r22))+((sj5*x540))+((sj5*x541)));
11258 evalcond[7]=(((cj5*x540))+((cj5*x541))+(((-1.0)*new_r22*x539)));
11268 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
11270 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11276 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
11283 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11286 IkReal evalcond[5];
11287 bool bgotonextstatement =
true;
11290 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
11291 evalcond[1]=new_r02;
11292 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11294 bgotonextstatement=
false;
11299 j4=1.5707963267949;
11301 j6eval[1]=
IKsign(new_r12);
11302 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
11303 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11309 j4=1.5707963267949;
11312 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
11313 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11319 j4=1.5707963267949;
11321 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
11324 IkReal evalcond[4];
11325 bool bgotonextstatement =
true;
11328 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
11329 evalcond[1]=new_r22;
11330 evalcond[2]=new_r11;
11331 evalcond[3]=new_r10;
11332 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
11334 bgotonextstatement=
false;
11336 IkReal j6array[1], cj6array[1], sj6array[1];
11337 bool j6valid[1]={
false};
11341 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
11342 sj6array[0]=
IKsin(j6array[0]);
11343 cj6array[0]=
IKcos(j6array[0]);
11344 if( j6array[0] >
IKPI )
11348 else if( j6array[0] < -
IKPI )
11349 { j6array[0]+=
IK2PI;
11352 for(
int ij6 = 0; ij6 < 1; ++ij6)
11354 if( !j6valid[ij6] )
11358 _ij6[0] = ij6; _ij6[1] = -1;
11359 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11363 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11366 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11368 IkReal evalcond[4];
11369 IkReal x544=
IKcos(j6);
11370 IkReal x545=((1.0)*(
IKsin(j6)));
11371 evalcond[0]=(x544+new_r20);
11372 evalcond[1]=((((-1.0)*x545))+new_r21);
11373 evalcond[2]=((((-1.0)*x545))+(((-1.0)*new_r00)));
11374 evalcond[3]=((((-1.0)*x544))+(((-1.0)*new_r01)));
11382 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11383 vinfos[0].jointtype = 17;
11384 vinfos[0].foffset = j0;
11385 vinfos[0].indices[0] = _ij0[0];
11386 vinfos[0].indices[1] = _ij0[1];
11387 vinfos[0].maxsolutions = _nj0;
11388 vinfos[1].jointtype = 1;
11389 vinfos[1].foffset = j1;
11390 vinfos[1].indices[0] = _ij1[0];
11391 vinfos[1].indices[1] = _ij1[1];
11392 vinfos[1].maxsolutions = _nj1;
11393 vinfos[2].jointtype = 1;
11394 vinfos[2].foffset = j2;
11395 vinfos[2].indices[0] = _ij2[0];
11396 vinfos[2].indices[1] = _ij2[1];
11397 vinfos[2].maxsolutions = _nj2;
11398 vinfos[3].jointtype = 1;
11399 vinfos[3].foffset = j3;
11400 vinfos[3].indices[0] = _ij3[0];
11401 vinfos[3].indices[1] = _ij3[1];
11402 vinfos[3].maxsolutions = _nj3;
11403 vinfos[4].jointtype = 1;
11404 vinfos[4].foffset = j4;
11405 vinfos[4].indices[0] = _ij4[0];
11406 vinfos[4].indices[1] = _ij4[1];
11407 vinfos[4].maxsolutions = _nj4;
11408 vinfos[5].jointtype = 1;
11409 vinfos[5].foffset = j5;
11410 vinfos[5].indices[0] = _ij5[0];
11411 vinfos[5].indices[1] = _ij5[1];
11412 vinfos[5].maxsolutions = _nj5;
11413 vinfos[6].jointtype = 1;
11414 vinfos[6].foffset = j6;
11415 vinfos[6].indices[0] = _ij6[0];
11416 vinfos[6].indices[1] = _ij6[1];
11417 vinfos[6].maxsolutions = _nj6;
11418 std::vector<int> vfree(0);
11426 if( bgotonextstatement )
11428 bool bgotonextstatement =
true;
11431 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
11432 evalcond[1]=new_r22;
11433 evalcond[2]=new_r11;
11434 evalcond[3]=new_r10;
11435 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
11437 bgotonextstatement=
false;
11439 IkReal j6array[1], cj6array[1], sj6array[1];
11440 bool j6valid[1]={
false};
11444 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
11445 sj6array[0]=
IKsin(j6array[0]);
11446 cj6array[0]=
IKcos(j6array[0]);
11447 if( j6array[0] >
IKPI )
11451 else if( j6array[0] < -
IKPI )
11452 { j6array[0]+=
IK2PI;
11455 for(
int ij6 = 0; ij6 < 1; ++ij6)
11457 if( !j6valid[ij6] )
11461 _ij6[0] = ij6; _ij6[1] = -1;
11462 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11466 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11469 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11471 IkReal evalcond[4];
11472 IkReal x546=
IKsin(j6);
11473 IkReal x547=((1.0)*(
IKcos(j6)));
11474 evalcond[0]=(x546+new_r21);
11475 evalcond[1]=((((-1.0)*x547))+new_r20);
11476 evalcond[2]=((((-1.0)*x546))+(((-1.0)*new_r00)));
11477 evalcond[3]=((((-1.0)*x547))+(((-1.0)*new_r01)));
11485 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11486 vinfos[0].jointtype = 17;
11487 vinfos[0].foffset = j0;
11488 vinfos[0].indices[0] = _ij0[0];
11489 vinfos[0].indices[1] = _ij0[1];
11490 vinfos[0].maxsolutions = _nj0;
11491 vinfos[1].jointtype = 1;
11492 vinfos[1].foffset = j1;
11493 vinfos[1].indices[0] = _ij1[0];
11494 vinfos[1].indices[1] = _ij1[1];
11495 vinfos[1].maxsolutions = _nj1;
11496 vinfos[2].jointtype = 1;
11497 vinfos[2].foffset = j2;
11498 vinfos[2].indices[0] = _ij2[0];
11499 vinfos[2].indices[1] = _ij2[1];
11500 vinfos[2].maxsolutions = _nj2;
11501 vinfos[3].jointtype = 1;
11502 vinfos[3].foffset = j3;
11503 vinfos[3].indices[0] = _ij3[0];
11504 vinfos[3].indices[1] = _ij3[1];
11505 vinfos[3].maxsolutions = _nj3;
11506 vinfos[4].jointtype = 1;
11507 vinfos[4].foffset = j4;
11508 vinfos[4].indices[0] = _ij4[0];
11509 vinfos[4].indices[1] = _ij4[1];
11510 vinfos[4].maxsolutions = _nj4;
11511 vinfos[5].jointtype = 1;
11512 vinfos[5].foffset = j5;
11513 vinfos[5].indices[0] = _ij5[0];
11514 vinfos[5].indices[1] = _ij5[1];
11515 vinfos[5].maxsolutions = _nj5;
11516 vinfos[6].jointtype = 1;
11517 vinfos[6].foffset = j6;
11518 vinfos[6].indices[0] = _ij6[0];
11519 vinfos[6].indices[1] = _ij6[1];
11520 vinfos[6].maxsolutions = _nj6;
11521 std::vector<int> vfree(0);
11529 if( bgotonextstatement )
11531 bool bgotonextstatement =
true;
11534 evalcond[0]=
IKabs(new_r12);
11535 evalcond[1]=new_r20;
11536 evalcond[2]=new_r21;
11537 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
11539 bgotonextstatement=
false;
11544 j4=1.5707963267949;
11548 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
11549 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11555 j4=1.5707963267949;
11558 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
11561 IkReal evalcond[3];
11562 bool bgotonextstatement =
true;
11565 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
11566 evalcond[1]=new_r11;
11567 evalcond[2]=new_r10;
11568 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
11570 bgotonextstatement=
false;
11572 IkReal j6array[1], cj6array[1], sj6array[1];
11573 bool j6valid[1]={
false};
11577 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
11578 sj6array[0]=
IKsin(j6array[0]);
11579 cj6array[0]=
IKcos(j6array[0]);
11580 if( j6array[0] >
IKPI )
11584 else if( j6array[0] < -
IKPI )
11585 { j6array[0]+=
IK2PI;
11588 for(
int ij6 = 0; ij6 < 1; ++ij6)
11590 if( !j6valid[ij6] )
11594 _ij6[0] = ij6; _ij6[1] = -1;
11595 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11599 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11602 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11604 IkReal evalcond[4];
11605 IkReal x548=
IKsin(j6);
11606 IkReal x549=
IKcos(j6);
11608 evalcond[1]=((-1.0)*x549);
11609 evalcond[2]=((((-1.0)*x548))+(((-1.0)*new_r00)));
11610 evalcond[3]=((((-1.0)*x549))+(((-1.0)*new_r01)));
11618 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11619 vinfos[0].jointtype = 17;
11620 vinfos[0].foffset = j0;
11621 vinfos[0].indices[0] = _ij0[0];
11622 vinfos[0].indices[1] = _ij0[1];
11623 vinfos[0].maxsolutions = _nj0;
11624 vinfos[1].jointtype = 1;
11625 vinfos[1].foffset = j1;
11626 vinfos[1].indices[0] = _ij1[0];
11627 vinfos[1].indices[1] = _ij1[1];
11628 vinfos[1].maxsolutions = _nj1;
11629 vinfos[2].jointtype = 1;
11630 vinfos[2].foffset = j2;
11631 vinfos[2].indices[0] = _ij2[0];
11632 vinfos[2].indices[1] = _ij2[1];
11633 vinfos[2].maxsolutions = _nj2;
11634 vinfos[3].jointtype = 1;
11635 vinfos[3].foffset = j3;
11636 vinfos[3].indices[0] = _ij3[0];
11637 vinfos[3].indices[1] = _ij3[1];
11638 vinfos[3].maxsolutions = _nj3;
11639 vinfos[4].jointtype = 1;
11640 vinfos[4].foffset = j4;
11641 vinfos[4].indices[0] = _ij4[0];
11642 vinfos[4].indices[1] = _ij4[1];
11643 vinfos[4].maxsolutions = _nj4;
11644 vinfos[5].jointtype = 1;
11645 vinfos[5].foffset = j5;
11646 vinfos[5].indices[0] = _ij5[0];
11647 vinfos[5].indices[1] = _ij5[1];
11648 vinfos[5].maxsolutions = _nj5;
11649 vinfos[6].jointtype = 1;
11650 vinfos[6].foffset = j6;
11651 vinfos[6].indices[0] = _ij6[0];
11652 vinfos[6].indices[1] = _ij6[1];
11653 vinfos[6].maxsolutions = _nj6;
11654 std::vector<int> vfree(0);
11662 if( bgotonextstatement )
11664 bool bgotonextstatement =
true;
11667 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
11668 evalcond[1]=new_r11;
11669 evalcond[2]=new_r10;
11670 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
11672 bgotonextstatement=
false;
11674 IkReal j6array[1], cj6array[1], sj6array[1];
11675 bool j6valid[1]={
false};
11679 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
11680 sj6array[0]=
IKsin(j6array[0]);
11681 cj6array[0]=
IKcos(j6array[0]);
11682 if( j6array[0] >
IKPI )
11686 else if( j6array[0] < -
IKPI )
11687 { j6array[0]+=
IK2PI;
11690 for(
int ij6 = 0; ij6 < 1; ++ij6)
11692 if( !j6valid[ij6] )
11696 _ij6[0] = ij6; _ij6[1] = -1;
11697 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11701 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11704 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11706 IkReal evalcond[4];
11707 IkReal x550=
IKsin(j6);
11708 IkReal x551=
IKcos(j6);
11710 evalcond[1]=((-1.0)*x551);
11711 evalcond[2]=((((-1.0)*x550))+(((-1.0)*new_r00)));
11712 evalcond[3]=((((-1.0)*x551))+(((-1.0)*new_r01)));
11720 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11721 vinfos[0].jointtype = 17;
11722 vinfos[0].foffset = j0;
11723 vinfos[0].indices[0] = _ij0[0];
11724 vinfos[0].indices[1] = _ij0[1];
11725 vinfos[0].maxsolutions = _nj0;
11726 vinfos[1].jointtype = 1;
11727 vinfos[1].foffset = j1;
11728 vinfos[1].indices[0] = _ij1[0];
11729 vinfos[1].indices[1] = _ij1[1];
11730 vinfos[1].maxsolutions = _nj1;
11731 vinfos[2].jointtype = 1;
11732 vinfos[2].foffset = j2;
11733 vinfos[2].indices[0] = _ij2[0];
11734 vinfos[2].indices[1] = _ij2[1];
11735 vinfos[2].maxsolutions = _nj2;
11736 vinfos[3].jointtype = 1;
11737 vinfos[3].foffset = j3;
11738 vinfos[3].indices[0] = _ij3[0];
11739 vinfos[3].indices[1] = _ij3[1];
11740 vinfos[3].maxsolutions = _nj3;
11741 vinfos[4].jointtype = 1;
11742 vinfos[4].foffset = j4;
11743 vinfos[4].indices[0] = _ij4[0];
11744 vinfos[4].indices[1] = _ij4[1];
11745 vinfos[4].maxsolutions = _nj4;
11746 vinfos[5].jointtype = 1;
11747 vinfos[5].foffset = j5;
11748 vinfos[5].indices[0] = _ij5[0];
11749 vinfos[5].indices[1] = _ij5[1];
11750 vinfos[5].maxsolutions = _nj5;
11751 vinfos[6].jointtype = 1;
11752 vinfos[6].foffset = j6;
11753 vinfos[6].indices[0] = _ij6[0];
11754 vinfos[6].indices[1] = _ij6[1];
11755 vinfos[6].maxsolutions = _nj6;
11756 std::vector<int> vfree(0);
11764 if( bgotonextstatement )
11766 bool bgotonextstatement =
true;
11769 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
11770 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
11772 bgotonextstatement=
false;
11774 IkReal j6array[1], cj6array[1], sj6array[1];
11775 bool j6valid[1]={
false};
11779 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
11780 sj6array[0]=
IKsin(j6array[0]);
11781 cj6array[0]=
IKcos(j6array[0]);
11782 if( j6array[0] >
IKPI )
11786 else if( j6array[0] < -
IKPI )
11787 { j6array[0]+=
IK2PI;
11790 for(
int ij6 = 0; ij6 < 1; ++ij6)
11792 if( !j6valid[ij6] )
11796 _ij6[0] = ij6; _ij6[1] = -1;
11797 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11801 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11804 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11806 IkReal evalcond[6];
11807 IkReal x552=
IKsin(j6);
11808 IkReal x553=
IKcos(j6);
11809 IkReal x554=((-1.0)*x553);
11811 evalcond[1]=(cj5*x552);
11813 evalcond[3]=(cj5*x554);
11814 evalcond[4]=((((-1.0)*x552))+(((-1.0)*new_r00)));
11815 evalcond[5]=((((-1.0)*x553))+(((-1.0)*new_r01)));
11823 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11824 vinfos[0].jointtype = 17;
11825 vinfos[0].foffset = j0;
11826 vinfos[0].indices[0] = _ij0[0];
11827 vinfos[0].indices[1] = _ij0[1];
11828 vinfos[0].maxsolutions = _nj0;
11829 vinfos[1].jointtype = 1;
11830 vinfos[1].foffset = j1;
11831 vinfos[1].indices[0] = _ij1[0];
11832 vinfos[1].indices[1] = _ij1[1];
11833 vinfos[1].maxsolutions = _nj1;
11834 vinfos[2].jointtype = 1;
11835 vinfos[2].foffset = j2;
11836 vinfos[2].indices[0] = _ij2[0];
11837 vinfos[2].indices[1] = _ij2[1];
11838 vinfos[2].maxsolutions = _nj2;
11839 vinfos[3].jointtype = 1;
11840 vinfos[3].foffset = j3;
11841 vinfos[3].indices[0] = _ij3[0];
11842 vinfos[3].indices[1] = _ij3[1];
11843 vinfos[3].maxsolutions = _nj3;
11844 vinfos[4].jointtype = 1;
11845 vinfos[4].foffset = j4;
11846 vinfos[4].indices[0] = _ij4[0];
11847 vinfos[4].indices[1] = _ij4[1];
11848 vinfos[4].maxsolutions = _nj4;
11849 vinfos[5].jointtype = 1;
11850 vinfos[5].foffset = j5;
11851 vinfos[5].indices[0] = _ij5[0];
11852 vinfos[5].indices[1] = _ij5[1];
11853 vinfos[5].maxsolutions = _nj5;
11854 vinfos[6].jointtype = 1;
11855 vinfos[6].foffset = j6;
11856 vinfos[6].indices[0] = _ij6[0];
11857 vinfos[6].indices[1] = _ij6[1];
11858 vinfos[6].maxsolutions = _nj6;
11859 std::vector<int> vfree(0);
11867 if( bgotonextstatement )
11869 bool bgotonextstatement =
true;
11874 bgotonextstatement=
false;
11879 if( bgotonextstatement )
11890 IkReal j6array[1], cj6array[1], sj6array[1];
11891 bool j6valid[1]={
false};
11899 j6array[0]=
IKatan2(((-1.0)*cj5*new_r11), (new_r10*(x555.
value)));
11900 sj6array[0]=
IKsin(j6array[0]);
11901 cj6array[0]=
IKcos(j6array[0]);
11902 if( j6array[0] >
IKPI )
11906 else if( j6array[0] < -
IKPI )
11907 { j6array[0]+=
IK2PI;
11910 for(
int ij6 = 0; ij6 < 1; ++ij6)
11912 if( !j6valid[ij6] )
11916 _ij6[0] = ij6; _ij6[1] = -1;
11917 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11921 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11924 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11926 IkReal evalcond[6];
11927 IkReal x556=
IKsin(j6);
11928 IkReal x557=
IKcos(j6);
11929 IkReal x558=((1.0)*x557);
11930 evalcond[0]=(((cj5*x556))+new_r11);
11931 evalcond[1]=(((cj5*new_r11))+x556);
11932 evalcond[2]=((((-1.0)*cj5*x558))+new_r10);
11933 evalcond[3]=((((-1.0)*x556))+(((-1.0)*new_r00)));
11934 evalcond[4]=((((-1.0)*x558))+(((-1.0)*new_r01)));
11935 evalcond[5]=(((cj5*new_r10))+(((-1.0)*x558)));
11943 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11944 vinfos[0].jointtype = 17;
11945 vinfos[0].foffset = j0;
11946 vinfos[0].indices[0] = _ij0[0];
11947 vinfos[0].indices[1] = _ij0[1];
11948 vinfos[0].maxsolutions = _nj0;
11949 vinfos[1].jointtype = 1;
11950 vinfos[1].foffset = j1;
11951 vinfos[1].indices[0] = _ij1[0];
11952 vinfos[1].indices[1] = _ij1[1];
11953 vinfos[1].maxsolutions = _nj1;
11954 vinfos[2].jointtype = 1;
11955 vinfos[2].foffset = j2;
11956 vinfos[2].indices[0] = _ij2[0];
11957 vinfos[2].indices[1] = _ij2[1];
11958 vinfos[2].maxsolutions = _nj2;
11959 vinfos[3].jointtype = 1;
11960 vinfos[3].foffset = j3;
11961 vinfos[3].indices[0] = _ij3[0];
11962 vinfos[3].indices[1] = _ij3[1];
11963 vinfos[3].maxsolutions = _nj3;
11964 vinfos[4].jointtype = 1;
11965 vinfos[4].foffset = j4;
11966 vinfos[4].indices[0] = _ij4[0];
11967 vinfos[4].indices[1] = _ij4[1];
11968 vinfos[4].maxsolutions = _nj4;
11969 vinfos[5].jointtype = 1;
11970 vinfos[5].foffset = j5;
11971 vinfos[5].indices[0] = _ij5[0];
11972 vinfos[5].indices[1] = _ij5[1];
11973 vinfos[5].maxsolutions = _nj5;
11974 vinfos[6].jointtype = 1;
11975 vinfos[6].foffset = j6;
11976 vinfos[6].indices[0] = _ij6[0];
11977 vinfos[6].indices[1] = _ij6[1];
11978 vinfos[6].maxsolutions = _nj6;
11979 std::vector<int> vfree(0);
11992 IkReal j6array[1], cj6array[1], sj6array[1];
11993 bool j6valid[1]={
false};
12003 j6array[0]=((-1.5707963267949)+(x559.
value)+(((1.5707963267949)*(x560.
value))));
12004 sj6array[0]=
IKsin(j6array[0]);
12005 cj6array[0]=
IKcos(j6array[0]);
12006 if( j6array[0] >
IKPI )
12010 else if( j6array[0] < -
IKPI )
12011 { j6array[0]+=
IK2PI;
12014 for(
int ij6 = 0; ij6 < 1; ++ij6)
12016 if( !j6valid[ij6] )
12020 _ij6[0] = ij6; _ij6[1] = -1;
12021 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12025 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12028 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12030 IkReal evalcond[6];
12031 IkReal x561=
IKsin(j6);
12032 IkReal x562=
IKcos(j6);
12033 IkReal x563=((1.0)*x562);
12034 evalcond[0]=(new_r11+((cj5*x561)));
12035 evalcond[1]=(((cj5*new_r11))+x561);
12036 evalcond[2]=((((-1.0)*cj5*x563))+new_r10);
12037 evalcond[3]=((((-1.0)*x561))+(((-1.0)*new_r00)));
12038 evalcond[4]=((((-1.0)*new_r01))+(((-1.0)*x563)));
12039 evalcond[5]=(((cj5*new_r10))+(((-1.0)*x563)));
12047 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12048 vinfos[0].jointtype = 17;
12049 vinfos[0].foffset = j0;
12050 vinfos[0].indices[0] = _ij0[0];
12051 vinfos[0].indices[1] = _ij0[1];
12052 vinfos[0].maxsolutions = _nj0;
12053 vinfos[1].jointtype = 1;
12054 vinfos[1].foffset = j1;
12055 vinfos[1].indices[0] = _ij1[0];
12056 vinfos[1].indices[1] = _ij1[1];
12057 vinfos[1].maxsolutions = _nj1;
12058 vinfos[2].jointtype = 1;
12059 vinfos[2].foffset = j2;
12060 vinfos[2].indices[0] = _ij2[0];
12061 vinfos[2].indices[1] = _ij2[1];
12062 vinfos[2].maxsolutions = _nj2;
12063 vinfos[3].jointtype = 1;
12064 vinfos[3].foffset = j3;
12065 vinfos[3].indices[0] = _ij3[0];
12066 vinfos[3].indices[1] = _ij3[1];
12067 vinfos[3].maxsolutions = _nj3;
12068 vinfos[4].jointtype = 1;
12069 vinfos[4].foffset = j4;
12070 vinfos[4].indices[0] = _ij4[0];
12071 vinfos[4].indices[1] = _ij4[1];
12072 vinfos[4].maxsolutions = _nj4;
12073 vinfos[5].jointtype = 1;
12074 vinfos[5].foffset = j5;
12075 vinfos[5].indices[0] = _ij5[0];
12076 vinfos[5].indices[1] = _ij5[1];
12077 vinfos[5].maxsolutions = _nj5;
12078 vinfos[6].jointtype = 1;
12079 vinfos[6].foffset = j6;
12080 vinfos[6].indices[0] = _ij6[0];
12081 vinfos[6].indices[1] = _ij6[1];
12082 vinfos[6].maxsolutions = _nj6;
12083 std::vector<int> vfree(0);
12095 if( bgotonextstatement )
12097 bool bgotonextstatement =
true;
12100 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
12101 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12103 bgotonextstatement=
false;
12105 IkReal j6array[1], cj6array[1], sj6array[1];
12106 bool j6valid[1]={
false};
12110 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
12111 sj6array[0]=
IKsin(j6array[0]);
12112 cj6array[0]=
IKcos(j6array[0]);
12113 if( j6array[0] >
IKPI )
12117 else if( j6array[0] < -
IKPI )
12118 { j6array[0]+=
IK2PI;
12121 for(
int ij6 = 0; ij6 < 1; ++ij6)
12123 if( !j6valid[ij6] )
12127 _ij6[0] = ij6; _ij6[1] = -1;
12128 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12132 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12135 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12137 IkReal evalcond[6];
12138 IkReal x564=
IKsin(j6);
12139 IkReal x565=
IKcos(j6);
12140 IkReal x566=((-1.0)*x565);
12142 evalcond[1]=(new_r22*x564);
12144 evalcond[3]=(new_r22*x566);
12145 evalcond[4]=((((-1.0)*x564))+(((-1.0)*new_r00)));
12146 evalcond[5]=((((-1.0)*x565))+(((-1.0)*new_r01)));
12154 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12155 vinfos[0].jointtype = 17;
12156 vinfos[0].foffset = j0;
12157 vinfos[0].indices[0] = _ij0[0];
12158 vinfos[0].indices[1] = _ij0[1];
12159 vinfos[0].maxsolutions = _nj0;
12160 vinfos[1].jointtype = 1;
12161 vinfos[1].foffset = j1;
12162 vinfos[1].indices[0] = _ij1[0];
12163 vinfos[1].indices[1] = _ij1[1];
12164 vinfos[1].maxsolutions = _nj1;
12165 vinfos[2].jointtype = 1;
12166 vinfos[2].foffset = j2;
12167 vinfos[2].indices[0] = _ij2[0];
12168 vinfos[2].indices[1] = _ij2[1];
12169 vinfos[2].maxsolutions = _nj2;
12170 vinfos[3].jointtype = 1;
12171 vinfos[3].foffset = j3;
12172 vinfos[3].indices[0] = _ij3[0];
12173 vinfos[3].indices[1] = _ij3[1];
12174 vinfos[3].maxsolutions = _nj3;
12175 vinfos[4].jointtype = 1;
12176 vinfos[4].foffset = j4;
12177 vinfos[4].indices[0] = _ij4[0];
12178 vinfos[4].indices[1] = _ij4[1];
12179 vinfos[4].maxsolutions = _nj4;
12180 vinfos[5].jointtype = 1;
12181 vinfos[5].foffset = j5;
12182 vinfos[5].indices[0] = _ij5[0];
12183 vinfos[5].indices[1] = _ij5[1];
12184 vinfos[5].maxsolutions = _nj5;
12185 vinfos[6].jointtype = 1;
12186 vinfos[6].foffset = j6;
12187 vinfos[6].indices[0] = _ij6[0];
12188 vinfos[6].indices[1] = _ij6[1];
12189 vinfos[6].maxsolutions = _nj6;
12190 std::vector<int> vfree(0);
12198 if( bgotonextstatement )
12200 bool bgotonextstatement =
true;
12205 bgotonextstatement=
false;
12210 if( bgotonextstatement )
12222 IkReal j6array[1], cj6array[1], sj6array[1];
12223 bool j6valid[1]={
false};
12231 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x567.
value)));
12232 sj6array[0]=
IKsin(j6array[0]);
12233 cj6array[0]=
IKcos(j6array[0]);
12234 if( j6array[0] >
IKPI )
12238 else if( j6array[0] < -
IKPI )
12239 { j6array[0]+=
IK2PI;
12242 for(
int ij6 = 0; ij6 < 1; ++ij6)
12244 if( !j6valid[ij6] )
12248 _ij6[0] = ij6; _ij6[1] = -1;
12249 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12253 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12256 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12258 IkReal evalcond[8];
12259 IkReal x568=
IKsin(j6);
12260 IkReal x569=
IKcos(j6);
12261 IkReal x570=((1.0)*sj5);
12262 IkReal x571=((1.0)*x569);
12263 IkReal x572=((1.0)*x568);
12264 evalcond[0]=(new_r20+((new_r12*x569)));
12265 evalcond[1]=(new_r11+((cj5*x568)));
12266 evalcond[2]=((((-1.0)*new_r12*x572))+new_r21);
12267 evalcond[3]=((((-1.0)*cj5*x571))+new_r10);
12268 evalcond[4]=((((-1.0)*x572))+(((-1.0)*new_r00)));
12269 evalcond[5]=((((-1.0)*x571))+(((-1.0)*new_r01)));
12270 evalcond[6]=(((cj5*new_r11))+x568+(((-1.0)*new_r21*x570)));
12271 evalcond[7]=(((cj5*new_r10))+(((-1.0)*x571))+(((-1.0)*new_r20*x570)));
12279 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12280 vinfos[0].jointtype = 17;
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 vinfos[6].jointtype = 1;
12311 vinfos[6].foffset = j6;
12312 vinfos[6].indices[0] = _ij6[0];
12313 vinfos[6].indices[1] = _ij6[1];
12314 vinfos[6].maxsolutions = _nj6;
12315 std::vector<int> vfree(0);
12328 IkReal j6array[1], cj6array[1], sj6array[1];
12329 bool j6valid[1]={
false};
12339 j6array[0]=((-1.5707963267949)+(x573.
value)+(((1.5707963267949)*(x574.
value))));
12340 sj6array[0]=
IKsin(j6array[0]);
12341 cj6array[0]=
IKcos(j6array[0]);
12342 if( j6array[0] >
IKPI )
12346 else if( j6array[0] < -
IKPI )
12347 { j6array[0]+=
IK2PI;
12350 for(
int ij6 = 0; ij6 < 1; ++ij6)
12352 if( !j6valid[ij6] )
12356 _ij6[0] = ij6; _ij6[1] = -1;
12357 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12361 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12364 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12366 IkReal evalcond[8];
12367 IkReal x575=
IKsin(j6);
12368 IkReal x576=
IKcos(j6);
12369 IkReal x577=((1.0)*sj5);
12370 IkReal x578=((1.0)*x576);
12371 IkReal x579=((1.0)*x575);
12372 evalcond[0]=(((new_r12*x576))+new_r20);
12373 evalcond[1]=(((cj5*x575))+new_r11);
12374 evalcond[2]=((((-1.0)*new_r12*x579))+new_r21);
12375 evalcond[3]=((((-1.0)*cj5*x578))+new_r10);
12376 evalcond[4]=((((-1.0)*x579))+(((-1.0)*new_r00)));
12377 evalcond[5]=((((-1.0)*x578))+(((-1.0)*new_r01)));
12378 evalcond[6]=(((cj5*new_r11))+x575+(((-1.0)*new_r21*x577)));
12379 evalcond[7]=(((cj5*new_r10))+(((-1.0)*x578))+(((-1.0)*new_r20*x577)));
12387 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12388 vinfos[0].jointtype = 17;
12389 vinfos[0].foffset = j0;
12390 vinfos[0].indices[0] = _ij0[0];
12391 vinfos[0].indices[1] = _ij0[1];
12392 vinfos[0].maxsolutions = _nj0;
12393 vinfos[1].jointtype = 1;
12394 vinfos[1].foffset = j1;
12395 vinfos[1].indices[0] = _ij1[0];
12396 vinfos[1].indices[1] = _ij1[1];
12397 vinfos[1].maxsolutions = _nj1;
12398 vinfos[2].jointtype = 1;
12399 vinfos[2].foffset = j2;
12400 vinfos[2].indices[0] = _ij2[0];
12401 vinfos[2].indices[1] = _ij2[1];
12402 vinfos[2].maxsolutions = _nj2;
12403 vinfos[3].jointtype = 1;
12404 vinfos[3].foffset = j3;
12405 vinfos[3].indices[0] = _ij3[0];
12406 vinfos[3].indices[1] = _ij3[1];
12407 vinfos[3].maxsolutions = _nj3;
12408 vinfos[4].jointtype = 1;
12409 vinfos[4].foffset = j4;
12410 vinfos[4].indices[0] = _ij4[0];
12411 vinfos[4].indices[1] = _ij4[1];
12412 vinfos[4].maxsolutions = _nj4;
12413 vinfos[5].jointtype = 1;
12414 vinfos[5].foffset = j5;
12415 vinfos[5].indices[0] = _ij5[0];
12416 vinfos[5].indices[1] = _ij5[1];
12417 vinfos[5].maxsolutions = _nj5;
12418 vinfos[6].jointtype = 1;
12419 vinfos[6].foffset = j6;
12420 vinfos[6].indices[0] = _ij6[0];
12421 vinfos[6].indices[1] = _ij6[1];
12422 vinfos[6].maxsolutions = _nj6;
12423 std::vector<int> vfree(0);
12436 IkReal j6array[1], cj6array[1], sj6array[1];
12437 bool j6valid[1]={
false};
12447 j6array[0]=((-1.5707963267949)+(x580.
value)+(((1.5707963267949)*(x581.
value))));
12448 sj6array[0]=
IKsin(j6array[0]);
12449 cj6array[0]=
IKcos(j6array[0]);
12450 if( j6array[0] >
IKPI )
12454 else if( j6array[0] < -
IKPI )
12455 { j6array[0]+=
IK2PI;
12458 for(
int ij6 = 0; ij6 < 1; ++ij6)
12460 if( !j6valid[ij6] )
12464 _ij6[0] = ij6; _ij6[1] = -1;
12465 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12469 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12472 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12474 IkReal evalcond[8];
12475 IkReal x582=
IKsin(j6);
12476 IkReal x583=
IKcos(j6);
12477 IkReal x584=((1.0)*sj5);
12478 IkReal x585=((1.0)*x583);
12479 IkReal x586=((1.0)*x582);
12480 evalcond[0]=(((new_r12*x583))+new_r20);
12481 evalcond[1]=(((cj5*x582))+new_r11);
12482 evalcond[2]=((((-1.0)*new_r12*x586))+new_r21);
12483 evalcond[3]=((((-1.0)*cj5*x585))+new_r10);
12484 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x586)));
12485 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x585)));
12486 evalcond[6]=(((cj5*new_r11))+(((-1.0)*new_r21*x584))+x582);
12487 evalcond[7]=(((cj5*new_r10))+(((-1.0)*new_r20*x584))+(((-1.0)*x585)));
12495 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12496 vinfos[0].jointtype = 17;
12497 vinfos[0].foffset = j0;
12498 vinfos[0].indices[0] = _ij0[0];
12499 vinfos[0].indices[1] = _ij0[1];
12500 vinfos[0].maxsolutions = _nj0;
12501 vinfos[1].jointtype = 1;
12502 vinfos[1].foffset = j1;
12503 vinfos[1].indices[0] = _ij1[0];
12504 vinfos[1].indices[1] = _ij1[1];
12505 vinfos[1].maxsolutions = _nj1;
12506 vinfos[2].jointtype = 1;
12507 vinfos[2].foffset = j2;
12508 vinfos[2].indices[0] = _ij2[0];
12509 vinfos[2].indices[1] = _ij2[1];
12510 vinfos[2].maxsolutions = _nj2;
12511 vinfos[3].jointtype = 1;
12512 vinfos[3].foffset = j3;
12513 vinfos[3].indices[0] = _ij3[0];
12514 vinfos[3].indices[1] = _ij3[1];
12515 vinfos[3].maxsolutions = _nj3;
12516 vinfos[4].jointtype = 1;
12517 vinfos[4].foffset = j4;
12518 vinfos[4].indices[0] = _ij4[0];
12519 vinfos[4].indices[1] = _ij4[1];
12520 vinfos[4].maxsolutions = _nj4;
12521 vinfos[5].jointtype = 1;
12522 vinfos[5].foffset = j5;
12523 vinfos[5].indices[0] = _ij5[0];
12524 vinfos[5].indices[1] = _ij5[1];
12525 vinfos[5].maxsolutions = _nj5;
12526 vinfos[6].jointtype = 1;
12527 vinfos[6].foffset = j6;
12528 vinfos[6].indices[0] = _ij6[0];
12529 vinfos[6].indices[1] = _ij6[1];
12530 vinfos[6].maxsolutions = _nj6;
12531 std::vector<int> vfree(0);
12543 if( bgotonextstatement )
12545 bool bgotonextstatement =
true;
12548 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
12549 evalcond[1]=new_r02;
12550 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
12552 bgotonextstatement=
false;
12554 IkReal j6array[1], cj6array[1], sj6array[1];
12555 bool j6valid[1]={
false};
12559 j6array[0]=
IKatan2(new_r00, new_r01);
12560 sj6array[0]=
IKsin(j6array[0]);
12561 cj6array[0]=
IKcos(j6array[0]);
12562 if( j6array[0] >
IKPI )
12566 else if( j6array[0] < -
IKPI )
12567 { j6array[0]+=
IK2PI;
12570 for(
int ij6 = 0; ij6 < 1; ++ij6)
12572 if( !j6valid[ij6] )
12576 _ij6[0] = ij6; _ij6[1] = -1;
12577 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12581 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12584 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12586 IkReal evalcond[8];
12587 IkReal x587=
IKsin(j6);
12588 IkReal x588=
IKcos(j6);
12593 IkReal x589=x593.
value;
12594 IkReal x590=new_r22*new_r22;
12595 IkReal x591=((1.0)*x588);
12596 IkReal x592=(x589*x590);
12597 evalcond[0]=(((new_r12*x587))+new_r21);
12598 evalcond[1]=((((-1.0)*x587))+new_r00);
12599 evalcond[2]=((((-1.0)*x591))+new_r01);
12600 evalcond[3]=((((-1.0)*new_r12*x591))+new_r20);
12601 evalcond[4]=(((cj5*x587))+(((-1.0)*new_r11)));
12602 evalcond[5]=((((-1.0)*cj5*x591))+(((-1.0)*new_r10)));
12603 evalcond[6]=(((new_r12*new_r21))+x587+((new_r21*x592)));
12604 evalcond[7]=(((new_r12*new_r20))+(((-1.0)*x591))+((new_r20*x592)));
12612 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12613 vinfos[0].jointtype = 17;
12614 vinfos[0].foffset = j0;
12615 vinfos[0].indices[0] = _ij0[0];
12616 vinfos[0].indices[1] = _ij0[1];
12617 vinfos[0].maxsolutions = _nj0;
12618 vinfos[1].jointtype = 1;
12619 vinfos[1].foffset = j1;
12620 vinfos[1].indices[0] = _ij1[0];
12621 vinfos[1].indices[1] = _ij1[1];
12622 vinfos[1].maxsolutions = _nj1;
12623 vinfos[2].jointtype = 1;
12624 vinfos[2].foffset = j2;
12625 vinfos[2].indices[0] = _ij2[0];
12626 vinfos[2].indices[1] = _ij2[1];
12627 vinfos[2].maxsolutions = _nj2;
12628 vinfos[3].jointtype = 1;
12629 vinfos[3].foffset = j3;
12630 vinfos[3].indices[0] = _ij3[0];
12631 vinfos[3].indices[1] = _ij3[1];
12632 vinfos[3].maxsolutions = _nj3;
12633 vinfos[4].jointtype = 1;
12634 vinfos[4].foffset = j4;
12635 vinfos[4].indices[0] = _ij4[0];
12636 vinfos[4].indices[1] = _ij4[1];
12637 vinfos[4].maxsolutions = _nj4;
12638 vinfos[5].jointtype = 1;
12639 vinfos[5].foffset = j5;
12640 vinfos[5].indices[0] = _ij5[0];
12641 vinfos[5].indices[1] = _ij5[1];
12642 vinfos[5].maxsolutions = _nj5;
12643 vinfos[6].jointtype = 1;
12644 vinfos[6].foffset = j6;
12645 vinfos[6].indices[0] = _ij6[0];
12646 vinfos[6].indices[1] = _ij6[1];
12647 vinfos[6].maxsolutions = _nj6;
12648 std::vector<int> vfree(0);
12656 if( bgotonextstatement )
12658 bool bgotonextstatement =
true;
12661 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
12662 evalcond[1]=new_r22;
12663 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
12665 bgotonextstatement=
false;
12667 IkReal j6array[1], cj6array[1], sj6array[1];
12668 bool j6valid[1]={
false};
12672 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
12673 sj6array[0]=
IKsin(j6array[0]);
12674 cj6array[0]=
IKcos(j6array[0]);
12675 if( j6array[0] >
IKPI )
12679 else if( j6array[0] < -
IKPI )
12680 { j6array[0]+=
IK2PI;
12683 for(
int ij6 = 0; ij6 < 1; ++ij6)
12685 if( !j6valid[ij6] )
12689 _ij6[0] = ij6; _ij6[1] = -1;
12690 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12694 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12697 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12699 IkReal evalcond[8];
12700 IkReal x594=
IKcos(j6);
12701 IkReal x595=
IKsin(j6);
12702 IkReal x596=((1.0)*sj4);
12703 IkReal x597=((1.0)*x595);
12704 IkReal x598=((1.0)*x594);
12705 evalcond[0]=(x594+new_r20);
12706 evalcond[1]=((((-1.0)*x597))+new_r21);
12707 evalcond[2]=(((sj4*x594))+new_r01);
12708 evalcond[3]=(((sj4*x595))+new_r00);
12709 evalcond[4]=((((-1.0)*cj4*x598))+new_r11);
12710 evalcond[5]=((((-1.0)*cj4*x597))+new_r10);
12711 evalcond[6]=((((-1.0)*new_r00*x596))+((cj4*new_r10))+(((-1.0)*x597)));
12712 evalcond[7]=(((cj4*new_r11))+(((-1.0)*new_r01*x596))+(((-1.0)*x598)));
12720 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12721 vinfos[0].jointtype = 17;
12722 vinfos[0].foffset = j0;
12723 vinfos[0].indices[0] = _ij0[0];
12724 vinfos[0].indices[1] = _ij0[1];
12725 vinfos[0].maxsolutions = _nj0;
12726 vinfos[1].jointtype = 1;
12727 vinfos[1].foffset = j1;
12728 vinfos[1].indices[0] = _ij1[0];
12729 vinfos[1].indices[1] = _ij1[1];
12730 vinfos[1].maxsolutions = _nj1;
12731 vinfos[2].jointtype = 1;
12732 vinfos[2].foffset = j2;
12733 vinfos[2].indices[0] = _ij2[0];
12734 vinfos[2].indices[1] = _ij2[1];
12735 vinfos[2].maxsolutions = _nj2;
12736 vinfos[3].jointtype = 1;
12737 vinfos[3].foffset = j3;
12738 vinfos[3].indices[0] = _ij3[0];
12739 vinfos[3].indices[1] = _ij3[1];
12740 vinfos[3].maxsolutions = _nj3;
12741 vinfos[4].jointtype = 1;
12742 vinfos[4].foffset = j4;
12743 vinfos[4].indices[0] = _ij4[0];
12744 vinfos[4].indices[1] = _ij4[1];
12745 vinfos[4].maxsolutions = _nj4;
12746 vinfos[5].jointtype = 1;
12747 vinfos[5].foffset = j5;
12748 vinfos[5].indices[0] = _ij5[0];
12749 vinfos[5].indices[1] = _ij5[1];
12750 vinfos[5].maxsolutions = _nj5;
12751 vinfos[6].jointtype = 1;
12752 vinfos[6].foffset = j6;
12753 vinfos[6].indices[0] = _ij6[0];
12754 vinfos[6].indices[1] = _ij6[1];
12755 vinfos[6].maxsolutions = _nj6;
12756 std::vector<int> vfree(0);
12764 if( bgotonextstatement )
12766 bool bgotonextstatement =
true;
12769 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
12770 evalcond[1]=new_r22;
12771 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
12773 bgotonextstatement=
false;
12775 IkReal j6array[1], cj6array[1], sj6array[1];
12776 bool j6valid[1]={
false};
12780 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
12781 sj6array[0]=
IKsin(j6array[0]);
12782 cj6array[0]=
IKcos(j6array[0]);
12783 if( j6array[0] >
IKPI )
12787 else if( j6array[0] < -
IKPI )
12788 { j6array[0]+=
IK2PI;
12791 for(
int ij6 = 0; ij6 < 1; ++ij6)
12793 if( !j6valid[ij6] )
12797 _ij6[0] = ij6; _ij6[1] = -1;
12798 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12802 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12805 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12807 IkReal evalcond[8];
12808 IkReal x599=
IKcos(j6);
12809 IkReal x600=
IKsin(j6);
12810 IkReal x601=((1.0)*sj4);
12811 IkReal x602=((1.0)*x599);
12812 IkReal x603=((1.0)*x600);
12813 evalcond[0]=(x600+new_r21);
12814 evalcond[1]=((((-1.0)*x602))+new_r20);
12815 evalcond[2]=(((sj4*x599))+new_r01);
12816 evalcond[3]=(((sj4*x600))+new_r00);
12817 evalcond[4]=((((-1.0)*cj4*x602))+new_r11);
12818 evalcond[5]=((((-1.0)*cj4*x603))+new_r10);
12819 evalcond[6]=(((cj4*new_r10))+(((-1.0)*x603))+(((-1.0)*new_r00*x601)));
12820 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x602))+(((-1.0)*new_r01*x601)));
12828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12829 vinfos[0].jointtype = 17;
12830 vinfos[0].foffset = j0;
12831 vinfos[0].indices[0] = _ij0[0];
12832 vinfos[0].indices[1] = _ij0[1];
12833 vinfos[0].maxsolutions = _nj0;
12834 vinfos[1].jointtype = 1;
12835 vinfos[1].foffset = j1;
12836 vinfos[1].indices[0] = _ij1[0];
12837 vinfos[1].indices[1] = _ij1[1];
12838 vinfos[1].maxsolutions = _nj1;
12839 vinfos[2].jointtype = 1;
12840 vinfos[2].foffset = j2;
12841 vinfos[2].indices[0] = _ij2[0];
12842 vinfos[2].indices[1] = _ij2[1];
12843 vinfos[2].maxsolutions = _nj2;
12844 vinfos[3].jointtype = 1;
12845 vinfos[3].foffset = j3;
12846 vinfos[3].indices[0] = _ij3[0];
12847 vinfos[3].indices[1] = _ij3[1];
12848 vinfos[3].maxsolutions = _nj3;
12849 vinfos[4].jointtype = 1;
12850 vinfos[4].foffset = j4;
12851 vinfos[4].indices[0] = _ij4[0];
12852 vinfos[4].indices[1] = _ij4[1];
12853 vinfos[4].maxsolutions = _nj4;
12854 vinfos[5].jointtype = 1;
12855 vinfos[5].foffset = j5;
12856 vinfos[5].indices[0] = _ij5[0];
12857 vinfos[5].indices[1] = _ij5[1];
12858 vinfos[5].maxsolutions = _nj5;
12859 vinfos[6].jointtype = 1;
12860 vinfos[6].foffset = j6;
12861 vinfos[6].indices[0] = _ij6[0];
12862 vinfos[6].indices[1] = _ij6[1];
12863 vinfos[6].maxsolutions = _nj6;
12864 std::vector<int> vfree(0);
12872 if( bgotonextstatement )
12874 bool bgotonextstatement =
true;
12877 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
12878 evalcond[1]=new_r20;
12879 evalcond[2]=new_r02;
12880 evalcond[3]=new_r12;
12881 evalcond[4]=new_r21;
12882 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 )
12884 bgotonextstatement=
false;
12886 IkReal j6array[1], cj6array[1], sj6array[1];
12887 bool j6valid[1]={
false};
12889 IkReal x604=((1.0)*new_r01);
12892 j6array[0]=
IKatan2(((((-1.0)*cj4*x604))+(((-1.0)*new_r00*sj4))), (((cj4*new_r00))+(((-1.0)*sj4*x604))));
12893 sj6array[0]=
IKsin(j6array[0]);
12894 cj6array[0]=
IKcos(j6array[0]);
12895 if( j6array[0] >
IKPI )
12899 else if( j6array[0] < -
IKPI )
12900 { j6array[0]+=
IK2PI;
12903 for(
int ij6 = 0; ij6 < 1; ++ij6)
12905 if( !j6valid[ij6] )
12909 _ij6[0] = ij6; _ij6[1] = -1;
12910 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12914 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12917 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12919 IkReal evalcond[8];
12920 IkReal x605=
IKsin(j6);
12921 IkReal x606=
IKcos(j6);
12922 IkReal x607=((1.0)*sj4);
12923 IkReal x608=((1.0)*x606);
12924 IkReal x609=(sj4*x605);
12925 IkReal x610=(sj4*x606);
12926 IkReal x611=(cj4*x605);
12927 IkReal x612=(cj4*x608);
12928 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x605);
12929 evalcond[1]=(x610+x611+new_r01);
12930 evalcond[2]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x608)));
12931 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x605))+(((-1.0)*new_r00*x607)));
12932 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x608))+(((-1.0)*new_r01*x607)));
12933 evalcond[5]=((((-1.0)*x612))+x609+new_r00);
12934 evalcond[6]=((((-1.0)*x612))+x609+new_r11);
12935 evalcond[7]=((((-1.0)*x611))+new_r10+(((-1.0)*x606*x607)));
12943 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12944 vinfos[0].jointtype = 17;
12945 vinfos[0].foffset = j0;
12946 vinfos[0].indices[0] = _ij0[0];
12947 vinfos[0].indices[1] = _ij0[1];
12948 vinfos[0].maxsolutions = _nj0;
12949 vinfos[1].jointtype = 1;
12950 vinfos[1].foffset = j1;
12951 vinfos[1].indices[0] = _ij1[0];
12952 vinfos[1].indices[1] = _ij1[1];
12953 vinfos[1].maxsolutions = _nj1;
12954 vinfos[2].jointtype = 1;
12955 vinfos[2].foffset = j2;
12956 vinfos[2].indices[0] = _ij2[0];
12957 vinfos[2].indices[1] = _ij2[1];
12958 vinfos[2].maxsolutions = _nj2;
12959 vinfos[3].jointtype = 1;
12960 vinfos[3].foffset = j3;
12961 vinfos[3].indices[0] = _ij3[0];
12962 vinfos[3].indices[1] = _ij3[1];
12963 vinfos[3].maxsolutions = _nj3;
12964 vinfos[4].jointtype = 1;
12965 vinfos[4].foffset = j4;
12966 vinfos[4].indices[0] = _ij4[0];
12967 vinfos[4].indices[1] = _ij4[1];
12968 vinfos[4].maxsolutions = _nj4;
12969 vinfos[5].jointtype = 1;
12970 vinfos[5].foffset = j5;
12971 vinfos[5].indices[0] = _ij5[0];
12972 vinfos[5].indices[1] = _ij5[1];
12973 vinfos[5].maxsolutions = _nj5;
12974 vinfos[6].jointtype = 1;
12975 vinfos[6].foffset = j6;
12976 vinfos[6].indices[0] = _ij6[0];
12977 vinfos[6].indices[1] = _ij6[1];
12978 vinfos[6].maxsolutions = _nj6;
12979 std::vector<int> vfree(0);
12987 if( bgotonextstatement )
12989 bool bgotonextstatement =
true;
12992 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
12993 evalcond[1]=new_r20;
12994 evalcond[2]=new_r02;
12995 evalcond[3]=new_r12;
12996 evalcond[4]=new_r21;
12997 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 )
12999 bgotonextstatement=
false;
13001 IkReal j6array[1], cj6array[1], sj6array[1];
13002 bool j6valid[1]={
false};
13004 IkReal x613=((1.0)*new_r00);
13007 j6array[0]=
IKatan2((((cj4*new_r01))+(((-1.0)*sj4*x613))), ((((-1.0)*new_r01*sj4))+(((-1.0)*cj4*x613))));
13008 sj6array[0]=
IKsin(j6array[0]);
13009 cj6array[0]=
IKcos(j6array[0]);
13010 if( j6array[0] >
IKPI )
13014 else if( j6array[0] < -
IKPI )
13015 { j6array[0]+=
IK2PI;
13018 for(
int ij6 = 0; ij6 < 1; ++ij6)
13020 if( !j6valid[ij6] )
13024 _ij6[0] = ij6; _ij6[1] = -1;
13025 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13029 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13032 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13034 IkReal evalcond[8];
13035 IkReal x614=
IKcos(j6);
13036 IkReal x615=
IKsin(j6);
13037 IkReal x616=((1.0)*sj4);
13038 IkReal x617=((1.0)*x615);
13039 IkReal x618=(sj4*x614);
13040 IkReal x619=((1.0)*x614);
13041 IkReal x620=(cj4*x617);
13042 evalcond[0]=(((cj4*new_r00))+((new_r10*sj4))+x614);
13043 evalcond[1]=(((cj4*new_r01))+((new_r11*sj4))+(((-1.0)*x617)));
13044 evalcond[2]=(((sj4*x615))+new_r00+((cj4*x614)));
13045 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x617))+(((-1.0)*new_r00*x616)));
13046 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x619))+(((-1.0)*new_r01*x616)));
13047 evalcond[5]=((((-1.0)*x620))+x618+new_r01);
13048 evalcond[6]=((((-1.0)*x620))+x618+new_r10);
13049 evalcond[7]=((((-1.0)*cj4*x619))+new_r11+(((-1.0)*x615*x616)));
13057 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13058 vinfos[0].jointtype = 17;
13059 vinfos[0].foffset = j0;
13060 vinfos[0].indices[0] = _ij0[0];
13061 vinfos[0].indices[1] = _ij0[1];
13062 vinfos[0].maxsolutions = _nj0;
13063 vinfos[1].jointtype = 1;
13064 vinfos[1].foffset = j1;
13065 vinfos[1].indices[0] = _ij1[0];
13066 vinfos[1].indices[1] = _ij1[1];
13067 vinfos[1].maxsolutions = _nj1;
13068 vinfos[2].jointtype = 1;
13069 vinfos[2].foffset = j2;
13070 vinfos[2].indices[0] = _ij2[0];
13071 vinfos[2].indices[1] = _ij2[1];
13072 vinfos[2].maxsolutions = _nj2;
13073 vinfos[3].jointtype = 1;
13074 vinfos[3].foffset = j3;
13075 vinfos[3].indices[0] = _ij3[0];
13076 vinfos[3].indices[1] = _ij3[1];
13077 vinfos[3].maxsolutions = _nj3;
13078 vinfos[4].jointtype = 1;
13079 vinfos[4].foffset = j4;
13080 vinfos[4].indices[0] = _ij4[0];
13081 vinfos[4].indices[1] = _ij4[1];
13082 vinfos[4].maxsolutions = _nj4;
13083 vinfos[5].jointtype = 1;
13084 vinfos[5].foffset = j5;
13085 vinfos[5].indices[0] = _ij5[0];
13086 vinfos[5].indices[1] = _ij5[1];
13087 vinfos[5].maxsolutions = _nj5;
13088 vinfos[6].jointtype = 1;
13089 vinfos[6].foffset = j6;
13090 vinfos[6].indices[0] = _ij6[0];
13091 vinfos[6].indices[1] = _ij6[1];
13092 vinfos[6].maxsolutions = _nj6;
13093 std::vector<int> vfree(0);
13101 if( bgotonextstatement )
13103 bool bgotonextstatement =
true;
13106 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
13107 evalcond[1]=new_r12;
13108 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
13110 bgotonextstatement=
false;
13112 IkReal j6array[1], cj6array[1], sj6array[1];
13113 bool j6valid[1]={
false};
13117 j6array[0]=
IKatan2(new_r10, new_r11);
13118 sj6array[0]=
IKsin(j6array[0]);
13119 cj6array[0]=
IKcos(j6array[0]);
13120 if( j6array[0] >
IKPI )
13124 else if( j6array[0] < -
IKPI )
13125 { j6array[0]+=
IK2PI;
13128 for(
int ij6 = 0; ij6 < 1; ++ij6)
13130 if( !j6valid[ij6] )
13134 _ij6[0] = ij6; _ij6[1] = -1;
13135 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13139 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13142 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13144 IkReal evalcond[8];
13145 IkReal x621=
IKcos(j6);
13146 IkReal x622=
IKsin(j6);
13147 IkReal x623=((1.0)*sj5);
13148 IkReal x624=((1.0)*x621);
13149 IkReal x625=((1.0)*x622);
13150 evalcond[0]=(((new_r02*x621))+new_r20);
13151 evalcond[1]=((((-1.0)*x625))+new_r10);
13152 evalcond[2]=((((-1.0)*x624))+new_r11);
13153 evalcond[3]=(((cj5*x622))+new_r01);
13154 evalcond[4]=((((-1.0)*new_r02*x625))+new_r21);
13155 evalcond[5]=((((-1.0)*cj5*x624))+new_r00);
13156 evalcond[6]=(((cj5*new_r01))+(((-1.0)*new_r21*x623))+x622);
13157 evalcond[7]=(((cj5*new_r00))+(((-1.0)*x624))+(((-1.0)*new_r20*x623)));
13165 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13166 vinfos[0].jointtype = 17;
13167 vinfos[0].foffset = j0;
13168 vinfos[0].indices[0] = _ij0[0];
13169 vinfos[0].indices[1] = _ij0[1];
13170 vinfos[0].maxsolutions = _nj0;
13171 vinfos[1].jointtype = 1;
13172 vinfos[1].foffset = j1;
13173 vinfos[1].indices[0] = _ij1[0];
13174 vinfos[1].indices[1] = _ij1[1];
13175 vinfos[1].maxsolutions = _nj1;
13176 vinfos[2].jointtype = 1;
13177 vinfos[2].foffset = j2;
13178 vinfos[2].indices[0] = _ij2[0];
13179 vinfos[2].indices[1] = _ij2[1];
13180 vinfos[2].maxsolutions = _nj2;
13181 vinfos[3].jointtype = 1;
13182 vinfos[3].foffset = j3;
13183 vinfos[3].indices[0] = _ij3[0];
13184 vinfos[3].indices[1] = _ij3[1];
13185 vinfos[3].maxsolutions = _nj3;
13186 vinfos[4].jointtype = 1;
13187 vinfos[4].foffset = j4;
13188 vinfos[4].indices[0] = _ij4[0];
13189 vinfos[4].indices[1] = _ij4[1];
13190 vinfos[4].maxsolutions = _nj4;
13191 vinfos[5].jointtype = 1;
13192 vinfos[5].foffset = j5;
13193 vinfos[5].indices[0] = _ij5[0];
13194 vinfos[5].indices[1] = _ij5[1];
13195 vinfos[5].maxsolutions = _nj5;
13196 vinfos[6].jointtype = 1;
13197 vinfos[6].foffset = j6;
13198 vinfos[6].indices[0] = _ij6[0];
13199 vinfos[6].indices[1] = _ij6[1];
13200 vinfos[6].maxsolutions = _nj6;
13201 std::vector<int> vfree(0);
13209 if( bgotonextstatement )
13211 bool bgotonextstatement =
true;
13214 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
13215 evalcond[1]=new_r12;
13216 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
13218 bgotonextstatement=
false;
13223 j4=3.14159265358979;
13225 j6eval[1]=
IKsign(new_r02);
13226 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
13227 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
13233 j4=3.14159265358979;
13235 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
13241 j4=3.14159265358979;
13244 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
13247 IkReal evalcond[4];
13248 bool bgotonextstatement =
true;
13251 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
13252 evalcond[1]=new_r22;
13253 evalcond[2]=new_r01;
13254 evalcond[3]=new_r00;
13255 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
13257 bgotonextstatement=
false;
13259 IkReal j6array[1], cj6array[1], sj6array[1];
13260 bool j6valid[1]={
false};
13264 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
13265 sj6array[0]=
IKsin(j6array[0]);
13266 cj6array[0]=
IKcos(j6array[0]);
13267 if( j6array[0] >
IKPI )
13271 else if( j6array[0] < -
IKPI )
13272 { j6array[0]+=
IK2PI;
13275 for(
int ij6 = 0; ij6 < 1; ++ij6)
13277 if( !j6valid[ij6] )
13281 _ij6[0] = ij6; _ij6[1] = -1;
13282 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13286 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13289 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13291 IkReal evalcond[4];
13292 IkReal x626=
IKcos(j6);
13293 IkReal x627=((1.0)*(
IKsin(j6)));
13294 evalcond[0]=(x626+new_r20);
13295 evalcond[1]=((((-1.0)*x627))+new_r21);
13296 evalcond[2]=((((-1.0)*x627))+(((-1.0)*new_r10)));
13297 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x626)));
13305 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13306 vinfos[0].jointtype = 17;
13307 vinfos[0].foffset = j0;
13308 vinfos[0].indices[0] = _ij0[0];
13309 vinfos[0].indices[1] = _ij0[1];
13310 vinfos[0].maxsolutions = _nj0;
13311 vinfos[1].jointtype = 1;
13312 vinfos[1].foffset = j1;
13313 vinfos[1].indices[0] = _ij1[0];
13314 vinfos[1].indices[1] = _ij1[1];
13315 vinfos[1].maxsolutions = _nj1;
13316 vinfos[2].jointtype = 1;
13317 vinfos[2].foffset = j2;
13318 vinfos[2].indices[0] = _ij2[0];
13319 vinfos[2].indices[1] = _ij2[1];
13320 vinfos[2].maxsolutions = _nj2;
13321 vinfos[3].jointtype = 1;
13322 vinfos[3].foffset = j3;
13323 vinfos[3].indices[0] = _ij3[0];
13324 vinfos[3].indices[1] = _ij3[1];
13325 vinfos[3].maxsolutions = _nj3;
13326 vinfos[4].jointtype = 1;
13327 vinfos[4].foffset = j4;
13328 vinfos[4].indices[0] = _ij4[0];
13329 vinfos[4].indices[1] = _ij4[1];
13330 vinfos[4].maxsolutions = _nj4;
13331 vinfos[5].jointtype = 1;
13332 vinfos[5].foffset = j5;
13333 vinfos[5].indices[0] = _ij5[0];
13334 vinfos[5].indices[1] = _ij5[1];
13335 vinfos[5].maxsolutions = _nj5;
13336 vinfos[6].jointtype = 1;
13337 vinfos[6].foffset = j6;
13338 vinfos[6].indices[0] = _ij6[0];
13339 vinfos[6].indices[1] = _ij6[1];
13340 vinfos[6].maxsolutions = _nj6;
13341 std::vector<int> vfree(0);
13349 if( bgotonextstatement )
13351 bool bgotonextstatement =
true;
13354 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
13355 evalcond[1]=new_r22;
13356 evalcond[2]=new_r01;
13357 evalcond[3]=new_r00;
13358 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
13360 bgotonextstatement=
false;
13362 IkReal j6array[1], cj6array[1], sj6array[1];
13363 bool j6valid[1]={
false};
13367 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
13368 sj6array[0]=
IKsin(j6array[0]);
13369 cj6array[0]=
IKcos(j6array[0]);
13370 if( j6array[0] >
IKPI )
13374 else if( j6array[0] < -
IKPI )
13375 { j6array[0]+=
IK2PI;
13378 for(
int ij6 = 0; ij6 < 1; ++ij6)
13380 if( !j6valid[ij6] )
13384 _ij6[0] = ij6; _ij6[1] = -1;
13385 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13389 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13392 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13394 IkReal evalcond[4];
13395 IkReal x628=
IKsin(j6);
13396 IkReal x629=((1.0)*(
IKcos(j6)));
13397 evalcond[0]=(x628+new_r21);
13398 evalcond[1]=((((-1.0)*x629))+new_r20);
13399 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x628)));
13400 evalcond[3]=((((-1.0)*x629))+(((-1.0)*new_r11)));
13408 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13409 vinfos[0].jointtype = 17;
13410 vinfos[0].foffset = j0;
13411 vinfos[0].indices[0] = _ij0[0];
13412 vinfos[0].indices[1] = _ij0[1];
13413 vinfos[0].maxsolutions = _nj0;
13414 vinfos[1].jointtype = 1;
13415 vinfos[1].foffset = j1;
13416 vinfos[1].indices[0] = _ij1[0];
13417 vinfos[1].indices[1] = _ij1[1];
13418 vinfos[1].maxsolutions = _nj1;
13419 vinfos[2].jointtype = 1;
13420 vinfos[2].foffset = j2;
13421 vinfos[2].indices[0] = _ij2[0];
13422 vinfos[2].indices[1] = _ij2[1];
13423 vinfos[2].maxsolutions = _nj2;
13424 vinfos[3].jointtype = 1;
13425 vinfos[3].foffset = j3;
13426 vinfos[3].indices[0] = _ij3[0];
13427 vinfos[3].indices[1] = _ij3[1];
13428 vinfos[3].maxsolutions = _nj3;
13429 vinfos[4].jointtype = 1;
13430 vinfos[4].foffset = j4;
13431 vinfos[4].indices[0] = _ij4[0];
13432 vinfos[4].indices[1] = _ij4[1];
13433 vinfos[4].maxsolutions = _nj4;
13434 vinfos[5].jointtype = 1;
13435 vinfos[5].foffset = j5;
13436 vinfos[5].indices[0] = _ij5[0];
13437 vinfos[5].indices[1] = _ij5[1];
13438 vinfos[5].maxsolutions = _nj5;
13439 vinfos[6].jointtype = 1;
13440 vinfos[6].foffset = j6;
13441 vinfos[6].indices[0] = _ij6[0];
13442 vinfos[6].indices[1] = _ij6[1];
13443 vinfos[6].maxsolutions = _nj6;
13444 std::vector<int> vfree(0);
13452 if( bgotonextstatement )
13454 bool bgotonextstatement =
true;
13457 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
13458 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
13460 bgotonextstatement=
false;
13462 IkReal j6array[1], cj6array[1], sj6array[1];
13463 bool j6valid[1]={
false};
13467 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
13468 sj6array[0]=
IKsin(j6array[0]);
13469 cj6array[0]=
IKcos(j6array[0]);
13470 if( j6array[0] >
IKPI )
13474 else if( j6array[0] < -
IKPI )
13475 { j6array[0]+=
IK2PI;
13478 for(
int ij6 = 0; ij6 < 1; ++ij6)
13480 if( !j6valid[ij6] )
13484 _ij6[0] = ij6; _ij6[1] = -1;
13485 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13489 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13492 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13494 IkReal evalcond[6];
13495 IkReal x630=
IKsin(j6);
13496 IkReal x631=
IKcos(j6);
13497 IkReal x632=((-1.0)*x631);
13499 evalcond[1]=(new_r22*x630);
13501 evalcond[3]=(new_r22*x632);
13502 evalcond[4]=((((-1.0)*new_r10))+(((-1.0)*x630)));
13503 evalcond[5]=((((-1.0)*new_r11))+(((-1.0)*x631)));
13511 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13512 vinfos[0].jointtype = 17;
13513 vinfos[0].foffset = j0;
13514 vinfos[0].indices[0] = _ij0[0];
13515 vinfos[0].indices[1] = _ij0[1];
13516 vinfos[0].maxsolutions = _nj0;
13517 vinfos[1].jointtype = 1;
13518 vinfos[1].foffset = j1;
13519 vinfos[1].indices[0] = _ij1[0];
13520 vinfos[1].indices[1] = _ij1[1];
13521 vinfos[1].maxsolutions = _nj1;
13522 vinfos[2].jointtype = 1;
13523 vinfos[2].foffset = j2;
13524 vinfos[2].indices[0] = _ij2[0];
13525 vinfos[2].indices[1] = _ij2[1];
13526 vinfos[2].maxsolutions = _nj2;
13527 vinfos[3].jointtype = 1;
13528 vinfos[3].foffset = j3;
13529 vinfos[3].indices[0] = _ij3[0];
13530 vinfos[3].indices[1] = _ij3[1];
13531 vinfos[3].maxsolutions = _nj3;
13532 vinfos[4].jointtype = 1;
13533 vinfos[4].foffset = j4;
13534 vinfos[4].indices[0] = _ij4[0];
13535 vinfos[4].indices[1] = _ij4[1];
13536 vinfos[4].maxsolutions = _nj4;
13537 vinfos[5].jointtype = 1;
13538 vinfos[5].foffset = j5;
13539 vinfos[5].indices[0] = _ij5[0];
13540 vinfos[5].indices[1] = _ij5[1];
13541 vinfos[5].maxsolutions = _nj5;
13542 vinfos[6].jointtype = 1;
13543 vinfos[6].foffset = j6;
13544 vinfos[6].indices[0] = _ij6[0];
13545 vinfos[6].indices[1] = _ij6[1];
13546 vinfos[6].maxsolutions = _nj6;
13547 std::vector<int> vfree(0);
13555 if( bgotonextstatement )
13557 bool bgotonextstatement =
true;
13562 bgotonextstatement=
false;
13567 if( bgotonextstatement )
13578 IkReal j6array[1], cj6array[1], sj6array[1];
13579 bool j6valid[1]={
false};
13591 j6array[0]=
IKatan2(((-1.0)*new_r21*(x633.
value)), ((-1.0)*new_r00*(x634.
value)));
13592 sj6array[0]=
IKsin(j6array[0]);
13593 cj6array[0]=
IKcos(j6array[0]);
13594 if( j6array[0] >
IKPI )
13598 else if( j6array[0] < -
IKPI )
13599 { j6array[0]+=
IK2PI;
13602 for(
int ij6 = 0; ij6 < 1; ++ij6)
13604 if( !j6valid[ij6] )
13608 _ij6[0] = ij6; _ij6[1] = -1;
13609 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13613 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13616 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13618 IkReal evalcond[8];
13619 IkReal x635=
IKsin(j6);
13620 IkReal x636=
IKcos(j6);
13625 IkReal x637=x641.
value;
13626 IkReal x638=new_r22*new_r22;
13627 IkReal x639=((1.0)*x636);
13628 IkReal x640=(x637*x638);
13629 evalcond[0]=(((new_r02*x635))+new_r21);
13630 evalcond[1]=((((-1.0)*new_r02*x639))+new_r20);
13631 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x635)));
13632 evalcond[3]=((((-1.0)*x639))+(((-1.0)*new_r11)));
13633 evalcond[4]=(((cj5*x635))+(((-1.0)*new_r01)));
13634 evalcond[5]=((((-1.0)*cj5*x639))+(((-1.0)*new_r00)));
13635 evalcond[6]=(x635+((new_r02*new_r21))+((new_r21*x640)));
13636 evalcond[7]=((((-1.0)*x639))+((new_r20*x640))+((new_r02*new_r20)));
13644 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13645 vinfos[0].jointtype = 17;
13646 vinfos[0].foffset = j0;
13647 vinfos[0].indices[0] = _ij0[0];
13648 vinfos[0].indices[1] = _ij0[1];
13649 vinfos[0].maxsolutions = _nj0;
13650 vinfos[1].jointtype = 1;
13651 vinfos[1].foffset = j1;
13652 vinfos[1].indices[0] = _ij1[0];
13653 vinfos[1].indices[1] = _ij1[1];
13654 vinfos[1].maxsolutions = _nj1;
13655 vinfos[2].jointtype = 1;
13656 vinfos[2].foffset = j2;
13657 vinfos[2].indices[0] = _ij2[0];
13658 vinfos[2].indices[1] = _ij2[1];
13659 vinfos[2].maxsolutions = _nj2;
13660 vinfos[3].jointtype = 1;
13661 vinfos[3].foffset = j3;
13662 vinfos[3].indices[0] = _ij3[0];
13663 vinfos[3].indices[1] = _ij3[1];
13664 vinfos[3].maxsolutions = _nj3;
13665 vinfos[4].jointtype = 1;
13666 vinfos[4].foffset = j4;
13667 vinfos[4].indices[0] = _ij4[0];
13668 vinfos[4].indices[1] = _ij4[1];
13669 vinfos[4].maxsolutions = _nj4;
13670 vinfos[5].jointtype = 1;
13671 vinfos[5].foffset = j5;
13672 vinfos[5].indices[0] = _ij5[0];
13673 vinfos[5].indices[1] = _ij5[1];
13674 vinfos[5].maxsolutions = _nj5;
13675 vinfos[6].jointtype = 1;
13676 vinfos[6].foffset = j6;
13677 vinfos[6].indices[0] = _ij6[0];
13678 vinfos[6].indices[1] = _ij6[1];
13679 vinfos[6].maxsolutions = _nj6;
13680 std::vector<int> vfree(0);
13693 IkReal j6array[1], cj6array[1], sj6array[1];
13694 bool j6valid[1]={
false};
13702 j6array[0]=
IKatan2(((-1.0)*new_r10), (new_r20*(x642.
value)));
13703 sj6array[0]=
IKsin(j6array[0]);
13704 cj6array[0]=
IKcos(j6array[0]);
13705 if( j6array[0] >
IKPI )
13709 else if( j6array[0] < -
IKPI )
13710 { j6array[0]+=
IK2PI;
13713 for(
int ij6 = 0; ij6 < 1; ++ij6)
13715 if( !j6valid[ij6] )
13719 _ij6[0] = ij6; _ij6[1] = -1;
13720 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13724 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13727 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13729 IkReal evalcond[8];
13730 IkReal x643=
IKsin(j6);
13731 IkReal x644=
IKcos(j6);
13736 IkReal x645=x649.
value;
13737 IkReal x646=new_r22*new_r22;
13738 IkReal x647=((1.0)*x644);
13739 IkReal x648=(x645*x646);
13740 evalcond[0]=(new_r21+((new_r02*x643)));
13741 evalcond[1]=((((-1.0)*new_r02*x647))+new_r20);
13742 evalcond[2]=((((-1.0)*x643))+(((-1.0)*new_r10)));
13743 evalcond[3]=((((-1.0)*x647))+(((-1.0)*new_r11)));
13744 evalcond[4]=(((cj5*x643))+(((-1.0)*new_r01)));
13745 evalcond[5]=((((-1.0)*cj5*x647))+(((-1.0)*new_r00)));
13746 evalcond[6]=(x643+((new_r02*new_r21))+((new_r21*x648)));
13747 evalcond[7]=(((new_r20*x648))+(((-1.0)*x647))+((new_r02*new_r20)));
13755 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13756 vinfos[0].jointtype = 17;
13757 vinfos[0].foffset = j0;
13758 vinfos[0].indices[0] = _ij0[0];
13759 vinfos[0].indices[1] = _ij0[1];
13760 vinfos[0].maxsolutions = _nj0;
13761 vinfos[1].jointtype = 1;
13762 vinfos[1].foffset = j1;
13763 vinfos[1].indices[0] = _ij1[0];
13764 vinfos[1].indices[1] = _ij1[1];
13765 vinfos[1].maxsolutions = _nj1;
13766 vinfos[2].jointtype = 1;
13767 vinfos[2].foffset = j2;
13768 vinfos[2].indices[0] = _ij2[0];
13769 vinfos[2].indices[1] = _ij2[1];
13770 vinfos[2].maxsolutions = _nj2;
13771 vinfos[3].jointtype = 1;
13772 vinfos[3].foffset = j3;
13773 vinfos[3].indices[0] = _ij3[0];
13774 vinfos[3].indices[1] = _ij3[1];
13775 vinfos[3].maxsolutions = _nj3;
13776 vinfos[4].jointtype = 1;
13777 vinfos[4].foffset = j4;
13778 vinfos[4].indices[0] = _ij4[0];
13779 vinfos[4].indices[1] = _ij4[1];
13780 vinfos[4].maxsolutions = _nj4;
13781 vinfos[5].jointtype = 1;
13782 vinfos[5].foffset = j5;
13783 vinfos[5].indices[0] = _ij5[0];
13784 vinfos[5].indices[1] = _ij5[1];
13785 vinfos[5].maxsolutions = _nj5;
13786 vinfos[6].jointtype = 1;
13787 vinfos[6].foffset = j6;
13788 vinfos[6].indices[0] = _ij6[0];
13789 vinfos[6].indices[1] = _ij6[1];
13790 vinfos[6].maxsolutions = _nj6;
13791 std::vector<int> vfree(0);
13804 IkReal j6array[1], cj6array[1], sj6array[1];
13805 bool j6valid[1]={
false};
13815 j6array[0]=((-1.5707963267949)+(x650.
value)+(((1.5707963267949)*(x651.
value))));
13816 sj6array[0]=
IKsin(j6array[0]);
13817 cj6array[0]=
IKcos(j6array[0]);
13818 if( j6array[0] >
IKPI )
13822 else if( j6array[0] < -
IKPI )
13823 { j6array[0]+=
IK2PI;
13826 for(
int ij6 = 0; ij6 < 1; ++ij6)
13828 if( !j6valid[ij6] )
13832 _ij6[0] = ij6; _ij6[1] = -1;
13833 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13837 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13840 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13842 IkReal evalcond[8];
13843 IkReal x652=
IKsin(j6);
13844 IkReal x653=
IKcos(j6);
13849 IkReal x654=x658.
value;
13850 IkReal x655=new_r22*new_r22;
13851 IkReal x656=((1.0)*x653);
13852 IkReal x657=(x654*x655);
13853 evalcond[0]=(((new_r02*x652))+new_r21);
13854 evalcond[1]=((((-1.0)*new_r02*x656))+new_r20);
13855 evalcond[2]=((((-1.0)*x652))+(((-1.0)*new_r10)));
13856 evalcond[3]=((((-1.0)*x656))+(((-1.0)*new_r11)));
13857 evalcond[4]=(((cj5*x652))+(((-1.0)*new_r01)));
13858 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj5*x656)));
13859 evalcond[6]=(x652+((new_r21*x657))+((new_r02*new_r21)));
13860 evalcond[7]=((((-1.0)*x656))+((new_r20*x657))+((new_r02*new_r20)));
13868 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13869 vinfos[0].jointtype = 17;
13870 vinfos[0].foffset = j0;
13871 vinfos[0].indices[0] = _ij0[0];
13872 vinfos[0].indices[1] = _ij0[1];
13873 vinfos[0].maxsolutions = _nj0;
13874 vinfos[1].jointtype = 1;
13875 vinfos[1].foffset = j1;
13876 vinfos[1].indices[0] = _ij1[0];
13877 vinfos[1].indices[1] = _ij1[1];
13878 vinfos[1].maxsolutions = _nj1;
13879 vinfos[2].jointtype = 1;
13880 vinfos[2].foffset = j2;
13881 vinfos[2].indices[0] = _ij2[0];
13882 vinfos[2].indices[1] = _ij2[1];
13883 vinfos[2].maxsolutions = _nj2;
13884 vinfos[3].jointtype = 1;
13885 vinfos[3].foffset = j3;
13886 vinfos[3].indices[0] = _ij3[0];
13887 vinfos[3].indices[1] = _ij3[1];
13888 vinfos[3].maxsolutions = _nj3;
13889 vinfos[4].jointtype = 1;
13890 vinfos[4].foffset = j4;
13891 vinfos[4].indices[0] = _ij4[0];
13892 vinfos[4].indices[1] = _ij4[1];
13893 vinfos[4].maxsolutions = _nj4;
13894 vinfos[5].jointtype = 1;
13895 vinfos[5].foffset = j5;
13896 vinfos[5].indices[0] = _ij5[0];
13897 vinfos[5].indices[1] = _ij5[1];
13898 vinfos[5].maxsolutions = _nj5;
13899 vinfos[6].jointtype = 1;
13900 vinfos[6].foffset = j6;
13901 vinfos[6].indices[0] = _ij6[0];
13902 vinfos[6].indices[1] = _ij6[1];
13903 vinfos[6].maxsolutions = _nj6;
13904 std::vector<int> vfree(0);
13916 if( bgotonextstatement )
13918 bool bgotonextstatement =
true;
13921 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
13922 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
13924 bgotonextstatement=
false;
13932 if(
IKabs(j6eval[0]) < 0.0000000100000000 )
13938 IkReal op[2+1], zeror[2];
13943 polyroots2(op,zeror,numroots);
13944 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
13945 int numsolutions = 0;
13946 for(
int ij6 = 0; ij6 < numroots; ++ij6)
13948 IkReal htj6 = zeror[ij6];
13949 tempj6array[0]=((2.0)*(
atan(htj6)));
13950 for(
int kj6 = 0; kj6 < 1; ++kj6)
13952 j6array[numsolutions] = tempj6array[kj6];
13953 if( j6array[numsolutions] >
IKPI )
13955 j6array[numsolutions]-=
IK2PI;
13957 else if( j6array[numsolutions] < -
IKPI )
13959 j6array[numsolutions]+=
IK2PI;
13961 sj6array[numsolutions] =
IKsin(j6array[numsolutions]);
13962 cj6array[numsolutions] =
IKcos(j6array[numsolutions]);
13966 bool j6valid[2]={
true,
true};
13968 for(
int ij6 = 0; ij6 < numsolutions; ++ij6)
13970 if( !j6valid[ij6] )
13974 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13975 htj6 =
IKtan(j6/2);
13977 _ij6[0] = ij6; _ij6[1] = -1;
13978 for(
int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
13982 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13986 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13987 vinfos[0].jointtype = 17;
13988 vinfos[0].foffset = j0;
13989 vinfos[0].indices[0] = _ij0[0];
13990 vinfos[0].indices[1] = _ij0[1];
13991 vinfos[0].maxsolutions = _nj0;
13992 vinfos[1].jointtype = 1;
13993 vinfos[1].foffset = j1;
13994 vinfos[1].indices[0] = _ij1[0];
13995 vinfos[1].indices[1] = _ij1[1];
13996 vinfos[1].maxsolutions = _nj1;
13997 vinfos[2].jointtype = 1;
13998 vinfos[2].foffset = j2;
13999 vinfos[2].indices[0] = _ij2[0];
14000 vinfos[2].indices[1] = _ij2[1];
14001 vinfos[2].maxsolutions = _nj2;
14002 vinfos[3].jointtype = 1;
14003 vinfos[3].foffset = j3;
14004 vinfos[3].indices[0] = _ij3[0];
14005 vinfos[3].indices[1] = _ij3[1];
14006 vinfos[3].maxsolutions = _nj3;
14007 vinfos[4].jointtype = 1;
14008 vinfos[4].foffset = j4;
14009 vinfos[4].indices[0] = _ij4[0];
14010 vinfos[4].indices[1] = _ij4[1];
14011 vinfos[4].maxsolutions = _nj4;
14012 vinfos[5].jointtype = 1;
14013 vinfos[5].foffset = j5;
14014 vinfos[5].indices[0] = _ij5[0];
14015 vinfos[5].indices[1] = _ij5[1];
14016 vinfos[5].maxsolutions = _nj5;
14017 vinfos[6].jointtype = 1;
14018 vinfos[6].foffset = j6;
14019 vinfos[6].indices[0] = _ij6[0];
14020 vinfos[6].indices[1] = _ij6[1];
14021 vinfos[6].maxsolutions = _nj6;
14022 std::vector<int> vfree(0);
14033 if( bgotonextstatement )
14035 bool bgotonextstatement =
true;
14040 bgotonextstatement=
false;
14045 if( bgotonextstatement )
14062 IkReal j6array[1], cj6array[1], sj6array[1];
14063 bool j6valid[1]={
false};
14069 IkReal x659=x660.
value;
14080 j6array[0]=
IKatan2((x659*(x661.
value)*(x662.
value)*((((new_r20*sj4))+(((-1.0)*new_r01*sj5))))), ((-1.0)*new_r20*x659));
14081 sj6array[0]=
IKsin(j6array[0]);
14082 cj6array[0]=
IKcos(j6array[0]);
14083 if( j6array[0] >
IKPI )
14087 else if( j6array[0] < -
IKPI )
14088 { j6array[0]+=
IK2PI;
14091 for(
int ij6 = 0; ij6 < 1; ++ij6)
14093 if( !j6valid[ij6] )
14097 _ij6[0] = ij6; _ij6[1] = -1;
14098 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
14102 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
14105 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
14107 IkReal evalcond[12];
14108 IkReal x663=
IKsin(j6);
14109 IkReal x664=
IKcos(j6);
14110 IkReal x665=((1.0)*sj5);
14111 IkReal x666=((1.0)*sj4);
14112 IkReal x667=(cj5*sj4);
14113 IkReal x668=(cj4*new_r01);
14114 IkReal x669=(cj4*new_r00);
14115 IkReal x670=((1.0)*x664);
14116 IkReal x671=(cj5*x663);
14117 IkReal x672=((1.0)*x663);
14118 evalcond[0]=(new_r20+((sj5*x664)));
14119 evalcond[1]=((((-1.0)*x663*x665))+new_r21);
14120 evalcond[2]=(((new_r11*sj4))+x668+x671);
14121 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x666))+(((-1.0)*x672)));
14122 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x666))+(((-1.0)*x670)));
14123 evalcond[5]=(((sj4*x664))+((cj4*x671))+new_r01);
14124 evalcond[6]=(((new_r10*sj4))+(((-1.0)*cj5*x670))+x669);
14125 evalcond[7]=((((-1.0)*cj4*cj5*x670))+((sj4*x663))+new_r00);
14126 evalcond[8]=(((x663*x667))+(((-1.0)*cj4*x670))+new_r11);
14127 evalcond[9]=((((-1.0)*cj4*x672))+(((-1.0)*cj5*x664*x666))+new_r10);
14128 evalcond[10]=((((-1.0)*new_r21*x665))+((new_r11*x667))+x663+((cj5*x668)));
14129 evalcond[11]=(((new_r10*x667))+((cj5*x669))+(((-1.0)*x670))+(((-1.0)*new_r20*x665)));
14137 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14138 vinfos[0].jointtype = 17;
14139 vinfos[0].foffset = j0;
14140 vinfos[0].indices[0] = _ij0[0];
14141 vinfos[0].indices[1] = _ij0[1];
14142 vinfos[0].maxsolutions = _nj0;
14143 vinfos[1].jointtype = 1;
14144 vinfos[1].foffset = j1;
14145 vinfos[1].indices[0] = _ij1[0];
14146 vinfos[1].indices[1] = _ij1[1];
14147 vinfos[1].maxsolutions = _nj1;
14148 vinfos[2].jointtype = 1;
14149 vinfos[2].foffset = j2;
14150 vinfos[2].indices[0] = _ij2[0];
14151 vinfos[2].indices[1] = _ij2[1];
14152 vinfos[2].maxsolutions = _nj2;
14153 vinfos[3].jointtype = 1;
14154 vinfos[3].foffset = j3;
14155 vinfos[3].indices[0] = _ij3[0];
14156 vinfos[3].indices[1] = _ij3[1];
14157 vinfos[3].maxsolutions = _nj3;
14158 vinfos[4].jointtype = 1;
14159 vinfos[4].foffset = j4;
14160 vinfos[4].indices[0] = _ij4[0];
14161 vinfos[4].indices[1] = _ij4[1];
14162 vinfos[4].maxsolutions = _nj4;
14163 vinfos[5].jointtype = 1;
14164 vinfos[5].foffset = j5;
14165 vinfos[5].indices[0] = _ij5[0];
14166 vinfos[5].indices[1] = _ij5[1];
14167 vinfos[5].maxsolutions = _nj5;
14168 vinfos[6].jointtype = 1;
14169 vinfos[6].foffset = j6;
14170 vinfos[6].indices[0] = _ij6[0];
14171 vinfos[6].indices[1] = _ij6[1];
14172 vinfos[6].maxsolutions = _nj6;
14173 std::vector<int> vfree(0);
14186 IkReal j6array[1], cj6array[1], sj6array[1];
14187 bool j6valid[1]={
false};
14193 IkReal x673=x674.
value;
14200 j6array[0]=
IKatan2((x673*(x675.
value)*(((((-1.0)*new_r00*sj5))+(((-1.0)*cj4*cj5*new_r20))))), ((-1.0)*new_r20*x673));
14201 sj6array[0]=
IKsin(j6array[0]);
14202 cj6array[0]=
IKcos(j6array[0]);
14203 if( j6array[0] >
IKPI )
14207 else if( j6array[0] < -
IKPI )
14208 { j6array[0]+=
IK2PI;
14211 for(
int ij6 = 0; ij6 < 1; ++ij6)
14213 if( !j6valid[ij6] )
14217 _ij6[0] = ij6; _ij6[1] = -1;
14218 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
14222 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
14225 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
14227 IkReal evalcond[12];
14228 IkReal x676=
IKsin(j6);
14229 IkReal x677=
IKcos(j6);
14230 IkReal x678=((1.0)*sj5);
14231 IkReal x679=((1.0)*sj4);
14232 IkReal x680=(cj5*sj4);
14233 IkReal x681=(cj4*new_r01);
14234 IkReal x682=(cj4*new_r00);
14235 IkReal x683=((1.0)*x677);
14236 IkReal x684=(cj5*x676);
14237 IkReal x685=((1.0)*x676);
14238 evalcond[0]=(((sj5*x677))+new_r20);
14239 evalcond[1]=((((-1.0)*x676*x678))+new_r21);
14240 evalcond[2]=(((new_r11*sj4))+x681+x684);
14241 evalcond[3]=((((-1.0)*x685))+(((-1.0)*new_r00*x679))+((cj4*new_r10)));
14242 evalcond[4]=((((-1.0)*new_r01*x679))+(((-1.0)*x683))+((cj4*new_r11)));
14243 evalcond[5]=(((cj4*x684))+((sj4*x677))+new_r01);
14244 evalcond[6]=(((new_r10*sj4))+(((-1.0)*cj5*x683))+x682);
14245 evalcond[7]=((((-1.0)*cj4*cj5*x683))+((sj4*x676))+new_r00);
14246 evalcond[8]=((((-1.0)*cj4*x683))+((x676*x680))+new_r11);
14247 evalcond[9]=((((-1.0)*cj4*x685))+(((-1.0)*cj5*x677*x679))+new_r10);
14248 evalcond[10]=((((-1.0)*new_r21*x678))+((cj5*x681))+x676+((new_r11*x680)));
14249 evalcond[11]=((((-1.0)*x683))+((cj5*x682))+(((-1.0)*new_r20*x678))+((new_r10*x680)));
14257 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14258 vinfos[0].jointtype = 17;
14259 vinfos[0].foffset = j0;
14260 vinfos[0].indices[0] = _ij0[0];
14261 vinfos[0].indices[1] = _ij0[1];
14262 vinfos[0].maxsolutions = _nj0;
14263 vinfos[1].jointtype = 1;
14264 vinfos[1].foffset = j1;
14265 vinfos[1].indices[0] = _ij1[0];
14266 vinfos[1].indices[1] = _ij1[1];
14267 vinfos[1].maxsolutions = _nj1;
14268 vinfos[2].jointtype = 1;
14269 vinfos[2].foffset = j2;
14270 vinfos[2].indices[0] = _ij2[0];
14271 vinfos[2].indices[1] = _ij2[1];
14272 vinfos[2].maxsolutions = _nj2;
14273 vinfos[3].jointtype = 1;
14274 vinfos[3].foffset = j3;
14275 vinfos[3].indices[0] = _ij3[0];
14276 vinfos[3].indices[1] = _ij3[1];
14277 vinfos[3].maxsolutions = _nj3;
14278 vinfos[4].jointtype = 1;
14279 vinfos[4].foffset = j4;
14280 vinfos[4].indices[0] = _ij4[0];
14281 vinfos[4].indices[1] = _ij4[1];
14282 vinfos[4].maxsolutions = _nj4;
14283 vinfos[5].jointtype = 1;
14284 vinfos[5].foffset = j5;
14285 vinfos[5].indices[0] = _ij5[0];
14286 vinfos[5].indices[1] = _ij5[1];
14287 vinfos[5].maxsolutions = _nj5;
14288 vinfos[6].jointtype = 1;
14289 vinfos[6].foffset = j6;
14290 vinfos[6].indices[0] = _ij6[0];
14291 vinfos[6].indices[1] = _ij6[1];
14292 vinfos[6].maxsolutions = _nj6;
14293 std::vector<int> vfree(0);
14306 IkReal j6array[1], cj6array[1], sj6array[1];
14307 bool j6valid[1]={
false};
14317 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x686.
value)))+(x687.
value));
14318 sj6array[0]=
IKsin(j6array[0]);
14319 cj6array[0]=
IKcos(j6array[0]);
14320 if( j6array[0] >
IKPI )
14324 else if( j6array[0] < -
IKPI )
14325 { j6array[0]+=
IK2PI;
14328 for(
int ij6 = 0; ij6 < 1; ++ij6)
14330 if( !j6valid[ij6] )
14334 _ij6[0] = ij6; _ij6[1] = -1;
14335 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
14339 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
14342 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
14344 IkReal evalcond[12];
14345 IkReal x688=
IKsin(j6);
14346 IkReal x689=
IKcos(j6);
14347 IkReal x690=((1.0)*sj5);
14348 IkReal x691=((1.0)*sj4);
14349 IkReal x692=(cj5*sj4);
14350 IkReal x693=(cj4*new_r01);
14351 IkReal x694=(cj4*new_r00);
14352 IkReal x695=((1.0)*x689);
14353 IkReal x696=(cj5*x688);
14354 IkReal x697=((1.0)*x688);
14355 evalcond[0]=(((sj5*x689))+new_r20);
14356 evalcond[1]=((((-1.0)*x688*x690))+new_r21);
14357 evalcond[2]=(((new_r11*sj4))+x696+x693);
14358 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x691))+(((-1.0)*x697)));
14359 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x691))+(((-1.0)*x695)));
14360 evalcond[5]=(((cj4*x696))+new_r01+((sj4*x689)));
14361 evalcond[6]=(((new_r10*sj4))+(((-1.0)*cj5*x695))+x694);
14362 evalcond[7]=(new_r00+((sj4*x688))+(((-1.0)*cj4*cj5*x695)));
14363 evalcond[8]=(((x688*x692))+(((-1.0)*cj4*x695))+new_r11);
14364 evalcond[9]=((((-1.0)*cj4*x697))+(((-1.0)*cj5*x689*x691))+new_r10);
14365 evalcond[10]=(((new_r11*x692))+x688+(((-1.0)*new_r21*x690))+((cj5*x693)));
14366 evalcond[11]=(((new_r10*x692))+(((-1.0)*x695))+(((-1.0)*new_r20*x690))+((cj5*x694)));
14374 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14375 vinfos[0].jointtype = 17;
14376 vinfos[0].foffset = j0;
14377 vinfos[0].indices[0] = _ij0[0];
14378 vinfos[0].indices[1] = _ij0[1];
14379 vinfos[0].maxsolutions = _nj0;
14380 vinfos[1].jointtype = 1;
14381 vinfos[1].foffset = j1;
14382 vinfos[1].indices[0] = _ij1[0];
14383 vinfos[1].indices[1] = _ij1[1];
14384 vinfos[1].maxsolutions = _nj1;
14385 vinfos[2].jointtype = 1;
14386 vinfos[2].foffset = j2;
14387 vinfos[2].indices[0] = _ij2[0];
14388 vinfos[2].indices[1] = _ij2[1];
14389 vinfos[2].maxsolutions = _nj2;
14390 vinfos[3].jointtype = 1;
14391 vinfos[3].foffset = j3;
14392 vinfos[3].indices[0] = _ij3[0];
14393 vinfos[3].indices[1] = _ij3[1];
14394 vinfos[3].maxsolutions = _nj3;
14395 vinfos[4].jointtype = 1;
14396 vinfos[4].foffset = j4;
14397 vinfos[4].indices[0] = _ij4[0];
14398 vinfos[4].indices[1] = _ij4[1];
14399 vinfos[4].maxsolutions = _nj4;
14400 vinfos[5].jointtype = 1;
14401 vinfos[5].foffset = j5;
14402 vinfos[5].indices[0] = _ij5[0];
14403 vinfos[5].indices[1] = _ij5[1];
14404 vinfos[5].maxsolutions = _nj5;
14405 vinfos[6].jointtype = 1;
14406 vinfos[6].foffset = j6;
14407 vinfos[6].indices[0] = _ij6[0];
14408 vinfos[6].indices[1] = _ij6[1];
14409 vinfos[6].maxsolutions = _nj6;
14410 std::vector<int> vfree(0);
14428 }
static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3],
int& numroots)
14430 using std::complex;
14431 if( rawcoeffs[0] == 0 ) {
14433 polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
14437 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
14438 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
14439 complex<IkReal> coeffs[3];
14440 const int maxsteps = 110;
14441 for(
int i = 0; i < 3; ++i) {
14442 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
14444 complex<IkReal> roots[3];
14446 roots[0] = complex<IkReal>(1,0);
14447 roots[1] = complex<IkReal>(0.4,0.9);
14450 for(
int i = 2; i < 3; ++i) {
14451 roots[i] = roots[i-1]*roots[1];
14455 bool changed =
false;
14456 for(
int i = 0; i < 3; ++i) {
14457 if ( err[i] >= tol ) {
14460 complex<IkReal>
x = roots[i] + coeffs[0];
14461 for(
int j = 1; j < 3; ++j) {
14462 x = roots[i] * x + coeffs[j];
14464 for(
int j = 0; j < 3; ++j) {
14466 if( roots[i] != roots[j] ) {
14467 x /= (roots[i] - roots[j]);
14481 bool visited[3] = {
false};
14482 for(
int i = 0; i < 3; ++i) {
14483 if( !visited[i] ) {
14486 complex<IkReal> newroot=roots[i];
14488 for(
int j = i+1; j < 3; ++j) {
14490 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
14491 newroot += roots[j];
14500 if(
IKabs(imag(newroot)) < tolsqrt ) {
14501 rawroots[numroots++] = real(newroot);
14506 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2],
int& numroots) {
14507 IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
14511 else if( det == 0 ) {
14512 rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
14517 rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
14518 rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);
14522 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4],
int& numroots)
14524 using std::complex;
14525 if( rawcoeffs[0] == 0 ) {
14527 polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
14531 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
14532 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
14533 complex<IkReal> coeffs[4];
14534 const int maxsteps = 110;
14535 for(
int i = 0; i < 4; ++i) {
14536 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
14538 complex<IkReal> roots[4];
14540 roots[0] = complex<IkReal>(1,0);
14541 roots[1] = complex<IkReal>(0.4,0.9);
14544 for(
int i = 2; i < 4; ++i) {
14545 roots[i] = roots[i-1]*roots[1];
14549 bool changed =
false;
14550 for(
int i = 0; i < 4; ++i) {
14551 if ( err[i] >= tol ) {
14554 complex<IkReal>
x = roots[i] + coeffs[0];
14555 for(
int j = 1; j < 4; ++j) {
14556 x = roots[i] * x + coeffs[j];
14558 for(
int j = 0; j < 4; ++j) {
14560 if( roots[i] != roots[j] ) {
14561 x /= (roots[i] - roots[j]);
14575 bool visited[4] = {
false};
14576 for(
int i = 0; i < 4; ++i) {
14577 if( !visited[i] ) {
14580 complex<IkReal> newroot=roots[i];
14582 for(
int j = i+1; j < 4; ++j) {
14584 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
14585 newroot += roots[j];
14594 if(
IKabs(imag(newroot)) < tolsqrt ) {
14595 rawroots[numroots++] = real(newroot);
14607 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
14612 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
14619 #ifdef IKFAST_NAMESPACE 14623 #ifndef IKFAST_NO_MAIN 14625 #include <stdlib.h> 14626 #ifdef IKFAST_NAMESPACE 14627 using namespace IKFAST_NAMESPACE;
14632 printf(
"\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" 14633 "Returns the ik solutions given the transformation of the end effector specified by\n" 14634 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" 14641 IkReal eerot[9],eetrans[3];
14642 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
14643 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
14644 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
14645 for(std::size_t i = 0; i < vfree.size(); ++i)
14646 vfree[i] = atof(argv[13+i]);
14647 bool bSuccess =
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
14650 fprintf(stderr,
"Failed to get ik solution\n");
14654 printf(
"Found %d ik solutions:\n", (
int)solutions.
GetNumSolutions());
14658 printf(
"sol%d (free=%d): ", (
int)i, (
int)sol.
GetFree().size());
14659 std::vector<IkReal> vsolfree(sol.
GetFree().size());
14660 sol.
GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
14661 for( std::size_t j = 0; j < solvalues.size(); ++j)
14662 printf(
"%.15f, ", solvalues[j]);
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
#define IKFAST_SINCOS_THRESH
float IKatan2(float fy, float fx)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
#define IKFAST_EVALCOND_THRESH
#define IKFAST_SOLUTION_THRESH
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
virtual size_t GetNumSolutions() const
returns the number of solutions stored
float IKatan2Simple(float fy, float fx)
IKFAST_API int GetNumJoints()
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)
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)
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
float IKfmod(float x, float y)
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
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)
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 > asin(const Rall1d< T, V, S > &x)
IKFAST_API const char * GetIkFastVersion()
#define IKFAST_ATAN2_MAGTHRESH
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)
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
int main(int argc, char **argv)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKFAST_API int * GetFreeParameters()
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKFAST_API int GetIkRealSize()
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
IKFAST_API int GetNumFreeParameters()
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)
Default implementation of IkSolutionListBase.
IKFAST_API int GetIkType()
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
manages all the solutions
IKFAST_API const char * GetKinematicsHash()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
#define IKFAST_COMPILE_ASSERT(x)