1 #define IKFAST_HAS_LIBRARY 25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] 34 #define IKFAST_STRINGIZE2(s) #s 35 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s) 43 #ifndef __PRETTY_FUNCTION__ 44 #define __PRETTY_FUNCTION__ __FUNCDNAME__ 48 #ifndef __PRETTY_FUNCTION__ 49 #define __PRETTY_FUNCTION__ __func__ 52 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } } 57 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x 59 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) 62 #define IK2PI ((IkReal)6.28318530717959) 63 #define IKPI ((IkReal)3.14159265358979) 64 #define IKPI_2 ((IkReal)1.57079632679490) 74 void dgetrf_ (
const int* m,
const int* n,
double* a,
const int* lda,
int* ipiv,
int* info);
75 void zgetrf_ (
const int* m,
const int* n, std::complex<double>* a,
const int* lda,
int* ipiv,
int* info);
76 void dgetri_(
const int* n,
const double* a,
const int* lda,
int* ipiv,
double* work,
const int* lwork,
int* info);
77 void dgesv_ (
const int* n,
const int* nrhs,
double* a,
const int* lda,
int* ipiv,
double* b,
const int* ldb,
int* info);
78 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);
79 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);
84 #ifdef IKFAST_NAMESPACE 85 namespace IKFAST_NAMESPACE {
88 inline float IKabs(
float f) {
return fabsf(f); }
89 inline double IKabs(
double f) {
return fabs(f); }
91 inline float IKsqr(
float f) {
return f*f; }
92 inline double IKsqr(
double f) {
return f*f; }
94 inline float IKlog(
float f) {
return logf(f); }
95 inline double IKlog(
double f) {
return log(f); }
98 #ifndef IKFAST_SINCOS_THRESH 99 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001) 103 #ifndef IKFAST_ATAN2_MAGTHRESH 104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6) 108 #ifndef IKFAST_SOLUTION_THRESH 109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) 115 if( f <= -1 )
return float(-
IKPI_2);
116 else if( f >= 1 )
return float(
IKPI_2);
122 if( f <= -1 )
return -
IKPI_2;
123 else if( f >= 1 )
return IKPI_2;
148 if( f <= -1 )
return float(
IKPI);
149 else if( f >= 1 )
return float(0);
155 if( f <= -1 )
return IKPI;
156 else if( f >= 1 )
return 0;
159 inline float IKsin(
float f) {
return sinf(f); }
161 inline float IKcos(
float f) {
return cosf(f); }
163 inline float IKtan(
float f) {
return tanf(f); }
165 inline float IKsqrt(
float f) {
if( f <= 0.0f )
return 0.0f;
return sqrtf(f); }
166 inline double IKsqrt(
double f) {
if( f <= 0.0 )
return 0.0;
return sqrt(f); }
172 else if( isnan(fx) ) {
175 return atan2f(fy,fx);
182 else if( isnan(fx) ) {
210 IKFAST_API
void ComputeFk(
const IkReal* j, IkReal* eetrans, IkReal* eerot) {
211 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,x48,x49,x50,x51,x52,x53,x54,x55;
224 x12=((IkReal(0.0650000000000000))*(x2));
225 x13=((IkReal(0.0950000000000000))*(x6));
226 x14=((IkReal(1.00000000000000))*(x10));
227 x15=((IkReal(1.00000000000000))*(x11));
228 x16=((IkReal(1.00000000000000))*(x7));
229 x17=((IkReal(0.0950000000000000))*(x4));
230 x18=((IkReal(1.00000000000000))*(x2));
231 x19=((IkReal(0.350000000000000))*(x1));
232 x20=((IkReal(0.0650000000000000))*(x0));
233 x21=((IkReal(0.350000000000000))*(x2));
234 x22=((IkReal(0.0650000000000000))*(x7));
235 x23=((IkReal(0.0650000000000000))*(x6));
236 x24=((IkReal(0.0950000000000000))*(x2));
237 x25=((IkReal(1.00000000000000))*(x4));
245 x33=((x0)*(x1)*(x2));
248 x36=((x28)+(((IkReal(-1.00000000000000))*(x32))));
249 x37=((((x1)*(x25)))+(((x18)*(x3))));
250 x38=((IkReal(-1.00000000000000))*(x37));
251 x39=((x33)+(((IkReal(-1.00000000000000))*(x34))));
252 x40=((((x2)*(x31)))+(((IkReal(-1.00000000000000))*(x35))));
255 x43=((((IkReal(-1.00000000000000))*(x0)*(x1)*(x25)))+(((IkReal(-1.00000000000000))*(x18)*(x27))));
256 x44=((((x18)*(x27)))+(((x0)*(x1)*(x25))));
257 IkReal x56=((IkReal(1.00000000000000))*(x16));
258 x45=((((IkReal(-1.00000000000000))*(x2)*(x3)*(x56)))+(((IkReal(-1.00000000000000))*(x30)*(x56))));
259 x46=((((x16)*(x30)))+(((x16)*(x2)*(x3))));
264 x51=((x47)+(((IkReal(-1.00000000000000))*(x16)*(x5))));
265 x52=((x48)+(((x0)*(x5))));
266 x53=((((IkReal(-1.00000000000000))*(x16)*(x6)))+(((x5)*(((x34)+(((IkReal(-1.00000000000000))*(x0)*(x32))))))));
267 x54=((((x5)*(((((IkReal(-1.00000000000000))*(x1)*(x16)*(x2)))+(x35)))))+(((x0)*(x6))));
269 eerot[0]=((((x8)*(((x55)+(x49)))))+(((x53)*(x9))));
270 eerot[1]=((((x9)*(((((IkReal(-1.00000000000000))*(x15)*(x43)))+(((IkReal(-1.00000000000000))*(x14)*(x51)))))))+(((x53)*(x8))));
271 eerot[2]=((((x10)*(x44)))+(((x11)*(x51))));
272 eetrans[0]=((((x10)*(((((x12)*(x27)))+(((x20)*(x30)))))))+(((x13)*(x7)))+(((IkReal(0.350000000000000))*(x27)))+(((x0)*(x19)*(x4)))+(((x5)*(((((x0)*(x1)*(x24)))+(((IkReal(-1.00000000000000))*(x17)*(x27)))))))+(((x11)*(((((x23)*(x39)))+(((IkReal(-1.00000000000000))*(x22)*(x5)))))))+(((x21)*(x27))));
273 eerot[3]=((((x54)*(x9)))+(((x8)*(((x50)+(((x10)*(x52))))))));
274 eerot[4]=((((x54)*(x8)))+(((x9)*(((((IkReal(-1.00000000000000))*(x15)*(x45)))+(((IkReal(-1.00000000000000))*(x14)*(x52))))))));
275 eerot[5]=((((x11)*(x52)))+(((x10)*(x46))));
276 eetrans[1]=((((x21)*(x26)))+(((IkReal(-1.00000000000000))*(x0)*(x13)))+(((x19)*(x4)*(x7)))+(((IkReal(0.350000000000000))*(x26)))+(((x10)*(((((x22)*(x30)))+(((x12)*(x26)))))))+(((x11)*(((((x23)*(x40)))+(((x20)*(x5)))))))+(((x5)*(((((x24)*(x31)))+(((IkReal(-1.00000000000000))*(x17)*(x26))))))));
277 eerot[6]=((((x41)*(x9)))+(((x8)*(((((x11)*(x36)))+(((x10)*(x42))))))));
278 eerot[7]=((((x41)*(x8)))+(((x9)*(((((IkReal(-1.00000000000000))*(x14)*(x42)))+(((IkReal(-1.00000000000000))*(x15)*(x36))))))));
279 eerot[8]=((((x10)*(((x32)+(((IkReal(-1.00000000000000))*(x25)*(x3)))))))+(((x29)*(x38))));
280 IkReal x57=((IkReal(1.00000000000000))*(x3));
281 eetrans[2]=((IkReal(0.410000000000000))+(((x5)*(((((IkReal(-1.00000000000000))*(x1)*(x17)))+(((IkReal(-1.00000000000000))*(x24)*(x57)))))))+(((x10)*(((((x1)*(x12)))+(((IkReal(-0.0650000000000000))*(x28)))))))+(x19)+(((IkReal(-0.350000000000000))*(x28)))+(((x19)*(x2)))+(((x29)*(((((IkReal(-0.0650000000000000))*(x30)))+(((IkReal(-1.00000000000000))*(x12)*(x57))))))));
294 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,
sj5,htj5,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;
295 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2],
_nj5;
298 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
299 for(
int dummyiter = 0; dummyiter < 1; ++dummyiter) {
310 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
315 new_px=((px)+(((IkReal(-0.0650000000000000))*(r02))));
319 new_py=((((IkReal(-0.0650000000000000))*(r12)))+(py));
323 new_pz=((IkReal(-0.410000000000000))+(((IkReal(-0.0650000000000000))*(r22)))+(pz));
324 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;
325 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
326 npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00))));
327 npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11))));
328 npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02))));
329 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
330 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
331 rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10))));
332 rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21))));
333 rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21))));
334 rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11))));
335 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
336 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
337 rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12))));
338 IkReal op[8+1], zeror[8];
341 gconst0=((((IkReal(-1.00000000000000))*(rxp2_2)))+(((IkReal(0.0950000000000000))*(r20))));
343 gconst1=((IkReal(-0.190000000000000))*(r21));
345 gconst2=((((IkReal(-1.00000000000000))*(rxp2_2)))+(((IkReal(-0.0950000000000000))*(r20))));
347 gconst3=((IkReal(-0.00902500000000000))+(((IkReal(-0.190000000000000))*(npy)))+(((IkReal(0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp))));
349 gconst4=((IkReal(-0.380000000000000))*(npx));
351 gconst5=((IkReal(-0.00902500000000000))+(((IkReal(0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.190000000000000))*(npy))));
353 gconst6=((((IkReal(-1.00000000000000))*(rxp2_2)))+(((IkReal(0.0950000000000000))*(r20))));
355 gconst7=((IkReal(-0.190000000000000))*(r21));
357 gconst8=((((IkReal(-1.00000000000000))*(rxp2_2)))+(((IkReal(-0.0950000000000000))*(r20))));
359 gconst9=((IkReal(-0.00902500000000000))+(((IkReal(-0.190000000000000))*(npy)))+(((IkReal(0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp))));
361 gconst10=((IkReal(-0.380000000000000))*(npx));
363 gconst11=((IkReal(-0.00902500000000000))+(((IkReal(0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.190000000000000))*(npy))));
365 gconst12=((((IkReal(0.190000000000000))*(r22)))+(((IkReal(2.00000000000000))*(rxp0_2))));
367 gconst13=((IkReal(-4.00000000000000))*(rxp1_2));
369 gconst14=((((IkReal(-2.00000000000000))*(rxp0_2)))+(((IkReal(0.190000000000000))*(r22))));
371 gconst15=((IkReal(-1.40000000000000))*(npx));
373 gconst16=((IkReal(2.80000000000000))*(npy));
375 gconst17=((IkReal(1.40000000000000))*(npx));
377 gconst18=((((IkReal(0.190000000000000))*(r22)))+(((IkReal(2.00000000000000))*(rxp0_2))));
379 gconst19=((IkReal(-4.00000000000000))*(rxp1_2));
381 gconst20=((((IkReal(-2.00000000000000))*(rxp0_2)))+(((IkReal(0.190000000000000))*(r22))));
383 gconst21=((IkReal(-1.40000000000000))*(npx));
385 gconst22=((IkReal(2.80000000000000))*(npy));
387 gconst23=((IkReal(1.40000000000000))*(npx));
389 gconst24=((rxp2_2)+(((IkReal(-0.0950000000000000))*(r20))));
391 gconst25=((IkReal(0.190000000000000))*(r21));
393 gconst26=((rxp2_2)+(((IkReal(0.0950000000000000))*(r20))));
395 gconst27=((IkReal(-0.00902500000000000))+(((IkReal(-0.700000000000000))*(npz)))+(((IkReal(-0.190000000000000))*(npy)))+(((IkReal(-1.00000000000000))*(pp))));
397 gconst28=((IkReal(-0.380000000000000))*(npx));
399 gconst29=((IkReal(-0.00902500000000000))+(((IkReal(-0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.190000000000000))*(npy))));
401 gconst30=((rxp2_2)+(((IkReal(-0.0950000000000000))*(r20))));
403 gconst31=((IkReal(0.190000000000000))*(r21));
405 gconst32=((rxp2_2)+(((IkReal(0.0950000000000000))*(r20))));
407 gconst33=((IkReal(-0.00902500000000000))+(((IkReal(-0.700000000000000))*(npz)))+(((IkReal(-0.190000000000000))*(npy)))+(((IkReal(-1.00000000000000))*(pp))));
409 gconst34=((IkReal(-0.380000000000000))*(npx));
411 gconst35=((IkReal(-0.00902500000000000))+(((IkReal(-0.700000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.190000000000000))*(npy))));
412 IkReal x58=((IkReal(1.00000000000000))*(gconst26));
413 IkReal x59=((gconst30)*(gconst34));
414 IkReal x60=((gconst31)*(gconst33));
415 IkReal x61=((IkReal(1.00000000000000))*(gconst17));
416 IkReal x62=((gconst2)*(gconst6));
417 IkReal x63=((gconst11)*(gconst3));
418 IkReal x64=((gconst25)*(gconst29));
419 IkReal x65=((gconst14)*(gconst28));
420 IkReal x66=((gconst0)*(gconst5));
421 IkReal x67=((IkReal(1.00000000000000))*(gconst6));
422 IkReal x68=((IkReal(1.00000000000000))*(gconst14));
423 IkReal x69=((gconst21)*(gconst32));
424 IkReal x70=((gconst10)*(gconst6));
425 IkReal x71=((gconst21)*(gconst8));
426 IkReal x72=((gconst13)*(gconst5));
427 IkReal x73=((gconst7)*(gconst9));
428 IkReal x74=((gconst30)*(gconst35));
429 IkReal x75=((gconst26)*(gconst27));
430 IkReal x76=((gconst22)*(gconst30));
431 IkReal x77=((IkReal(1.00000000000000))*(gconst29));
432 IkReal x78=((gconst19)*(gconst33));
433 IkReal x79=((gconst26)*(gconst28));
434 IkReal x80=((gconst2)*(gconst4));
435 IkReal x81=((gconst18)*(gconst35));
436 IkReal x82=((gconst1)*(gconst5));
437 IkReal x83=((gconst21)*(gconst7));
438 IkReal x84=((gconst12)*(gconst5));
439 IkReal x85=((gconst18)*(gconst34));
440 IkReal x86=((gconst23)*(gconst30));
441 IkReal x87=((gconst3)*(gconst8));
442 IkReal x88=((gconst29)*(gconst33));
443 IkReal x89=((gconst32)*(gconst33));
444 IkReal x90=((gconst8)*(gconst9));
445 IkReal x91=((gconst21)*(gconst31));
446 IkReal x92=((gconst18)*(gconst2));
447 IkReal x93=((gconst14)*(gconst6));
448 IkReal x94=((gconst19)*(gconst9));
449 IkReal x95=((IkReal(1.00000000000000))*(gconst10)*(gconst4));
450 IkReal x96=((gconst27)*(x89));
451 IkReal x97=((IkReal(1.00000000000000))*(gconst2)*(gconst9));
452 op[0]=((((IkReal(-1.00000000000000))*(x58)*(x96)))+(((x60)*(x79)))+(((IkReal(-1.00000000000000))*(x60)*(x64)))+(((x59)*(x64)))+(((IkReal(-1.00000000000000))*(gconst28)*(x58)*(x59)))+(((x74)*(x75)))+(((IkReal(-1.00000000000000))*(gconst24)*(x74)*(x77)))+(((gconst24)*(gconst32)*(x88))));
453 op[1]=((((IkReal(-1.00000000000000))*(gconst24)*(x77)*(x86)))+(((gconst17)*(gconst25)*(x59)))+(((gconst16)*(gconst26)*(x60)))+(((x75)*(x81)))+(((IkReal(-1.00000000000000))*(gconst15)*(x58)*(x89)))+(((IkReal(-1.00000000000000))*(gconst16)*(x58)*(x59)))+(((IkReal(-1.00000000000000))*(gconst24)*(x61)*(x74)))+(((IkReal(-1.00000000000000))*(gconst28)*(x58)*(x85)))+(((x60)*(x65)))+(((gconst12)*(gconst32)*(x88)))+(((IkReal(-1.00000000000000))*(gconst28)*(x58)*(x76)))+(((IkReal(-1.00000000000000))*(gconst24)*(x77)*(x81)))+(((x78)*(x79)))+(((IkReal(-1.00000000000000))*(gconst27)*(x58)*(x69)))+(((IkReal(-1.00000000000000))*(x64)*(x91)))+(((IkReal(-1.00000000000000))*(gconst25)*(x60)*(x61)))+(((x64)*(x85)))+(((x75)*(x86)))+(((x64)*(x76)))+(((x79)*(x91)))+(((IkReal(-1.00000000000000))*(x64)*(x78)))+(((gconst14)*(gconst27)*(x74)))+(((gconst15)*(gconst26)*(x74)))+(((gconst13)*(gconst29)*(x59)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst27)*(gconst33)*(x58)))+(((gconst24)*(gconst29)*(x69)))+(((gconst17)*(gconst24)*(x89)))+(((IkReal(-1.00000000000000))*(gconst13)*(x60)*(x77)))+(((IkReal(-1.00000000000000))*(x68)*(x96)))+(((IkReal(-1.00000000000000))*(gconst12)*(x74)*(x77)))+(((IkReal(-1.00000000000000))*(x59)*(x65)))+(((gconst20)*(gconst24)*(x88))));
454 op[2]=((((gconst17)*(gconst21)*(gconst24)*(gconst32)))+(((gconst15)*(gconst23)*(gconst26)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst24)*(gconst29)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst3)*(gconst32)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst21)*(gconst29)*(gconst31)))+(((gconst13)*(gconst22)*(gconst29)*(gconst30)))+(((gconst24)*(gconst32)*(gconst33)*(gconst5)))+(((gconst14)*(gconst23)*(gconst27)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst18)*(gconst29)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst22)*(gconst26)*(gconst30)))+(((gconst16)*(gconst19)*(gconst26)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst29)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst30)*(gconst34)))+(((gconst14)*(gconst16)*(gconst31)*(gconst33)))+(((gconst0)*(gconst29)*(gconst32)*(gconst33)))+(((gconst2)*(gconst28)*(gconst31)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst29)*(gconst30)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst21)*(gconst26)*(gconst27)))+(((gconst13)*(gconst18)*(gconst29)*(gconst34)))+(((gconst10)*(gconst25)*(gconst29)*(gconst30)))+(((gconst14)*(gconst21)*(gconst28)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst27)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst28)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst23)*(gconst24)*(gconst29)))+(((gconst25)*(gconst30)*(gconst34)*(gconst5)))+(((gconst14)*(gconst19)*(gconst28)*(gconst33)))+(((gconst26)*(gconst3)*(gconst30)*(gconst35)))+(((gconst20)*(gconst21)*(gconst24)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst23)*(gconst24)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst22)*(gconst26)*(gconst28)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst18)*(gconst26)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst19)*(gconst21)*(gconst25)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst21)*(gconst27)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst18)*(gconst28)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst23)*(gconst29)*(gconst30)))+(((gconst11)*(gconst26)*(gconst27)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst18)*(gconst24)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst32)*(gconst33)))+(((gconst13)*(gconst17)*(gconst30)*(gconst34)))+(((gconst17)*(gconst18)*(gconst25)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst27)*(gconst32)*(gconst33)))+(((gconst25)*(gconst29)*(gconst34)*(gconst6)))+(((gconst24)*(gconst29)*(gconst33)*(gconst8)))+(((gconst16)*(gconst21)*(gconst26)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst19)*(gconst25)*(gconst33)))+(((gconst18)*(gconst22)*(gconst25)*(gconst29)))+(((gconst1)*(gconst29)*(gconst30)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst30)*(gconst34)*(gconst4)))+(((gconst15)*(gconst18)*(gconst26)*(gconst35)))+(((gconst12)*(gconst17)*(gconst32)*(gconst33)))+(((gconst26)*(gconst28)*(gconst33)*(gconst7)))+(((gconst26)*(gconst28)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst24)*(gconst30)*(gconst35)*(gconst5)))+(((gconst19)*(gconst21)*(gconst26)*(gconst28)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst26)*(gconst28)*(gconst30)))+(((gconst2)*(gconst27)*(gconst30)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst30)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst22)*(gconst28)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst27)*(gconst32)*(gconst9)))+(((gconst14)*(gconst15)*(gconst30)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst24)*(gconst29)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst31)*(gconst33)*(gconst5)))+(((gconst26)*(gconst31)*(gconst33)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst31)*(gconst33)))+(((gconst12)*(gconst20)*(gconst29)*(gconst33)))+(((gconst18)*(gconst23)*(gconst26)*(gconst27)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst21)*(gconst26)*(gconst32)))+(((gconst24)*(gconst29)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst28)*(gconst30)*(gconst34)))+(((gconst17)*(gconst22)*(gconst25)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst29)*(gconst31)*(gconst33)))+(((gconst26)*(gconst27)*(gconst35)*(gconst6)))+(((gconst12)*(gconst21)*(gconst29)*(gconst32)))+(((gconst14)*(gconst18)*(gconst27)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst21)*(gconst25)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst27)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst29)*(gconst33)))+(((gconst17)*(gconst20)*(gconst24)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst29)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst20)*(gconst26)*(gconst33))));
455 op[3]=((((IkReal(-1.00000000000000))*(gconst14)*(gconst27)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst26)*(gconst32)*(gconst9)))+(((gconst14)*(gconst16)*(gconst19)*(gconst33)))+(((gconst12)*(gconst29)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst25)*(gconst31)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst20)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst21)*(gconst27)))+(((gconst10)*(gconst18)*(gconst25)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst27)*(gconst32)*(gconst9)))+(((gconst13)*(gconst30)*(gconst34)*(gconst5)))+(((gconst21)*(gconst26)*(gconst28)*(gconst7)))+(((gconst17)*(gconst24)*(gconst33)*(gconst8)))+(((gconst15)*(gconst18)*(gconst23)*(gconst26)))+(((gconst13)*(gconst17)*(gconst22)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst24)*(gconst35)*(gconst5)))+(((gconst17)*(gconst24)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst28)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst21)*(gconst27)*(gconst32)))+(((gconst14)*(gconst27)*(gconst35)*(gconst6)))+(((gconst20)*(gconst24)*(gconst29)*(gconst9)))+(((gconst18)*(gconst26)*(gconst3)*(gconst35)))+(((gconst2)*(gconst23)*(gconst27)*(gconst30)))+(((gconst14)*(gconst16)*(gconst21)*(gconst31)))+(((gconst22)*(gconst25)*(gconst29)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst19)*(gconst25)*(gconst29)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst22)*(gconst28)*(gconst30)))+(((gconst0)*(gconst21)*(gconst29)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst25)*(gconst33)*(gconst7)))+(((gconst15)*(gconst2)*(gconst30)*(gconst35)))+(((gconst1)*(gconst18)*(gconst29)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst25)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst29)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst26)*(gconst34)*(gconst6)))+(((gconst11)*(gconst18)*(gconst26)*(gconst27)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst18)*(gconst23)*(gconst24)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst18)*(gconst35)))+(((gconst14)*(gconst19)*(gconst21)*(gconst28)))+(((gconst12)*(gconst17)*(gconst21)*(gconst32)))+(((gconst23)*(gconst26)*(gconst3)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst22)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(gconst26)*(gconst30)))+(((gconst12)*(gconst32)*(gconst33)*(gconst5)))+(((gconst14)*(gconst28)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst21)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst26)*(gconst27)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst24)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst19)*(gconst21)*(gconst25)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst19)*(gconst29)*(gconst33)))+(((gconst2)*(gconst21)*(gconst28)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst18)*(gconst23)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst26)*(gconst27)*(gconst8)))+(((gconst10)*(gconst13)*(gconst29)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst17)*(gconst24)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst31)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst30)*(gconst34)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst26)*(gconst34)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst23)*(gconst24)*(gconst30)*(gconst5)))+(((gconst16)*(gconst19)*(gconst21)*(gconst26)))+(((gconst14)*(gconst18)*(gconst23)*(gconst27)))+(((IkReal(-1.00000000000000))*(gconst19)*(gconst25)*(gconst33)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst18)*(gconst34)))+(((gconst17)*(gconst25)*(gconst34)*(gconst6)))+(((gconst14)*(gconst31)*(gconst33)*(gconst4)))+(((gconst17)*(gconst20)*(gconst21)*(gconst24)))+(((gconst21)*(gconst26)*(gconst31)*(gconst4)))+(((gconst13)*(gconst17)*(gconst18)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst18)*(gconst22)*(gconst28)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst26)*(gconst3)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst30)*(gconst35)))+(((gconst1)*(gconst22)*(gconst29)*(gconst30)))+(((gconst21)*(gconst24)*(gconst29)*(gconst8)))+(((gconst14)*(gconst28)*(gconst33)*(gconst7)))+(((gconst0)*(gconst17)*(gconst32)*(gconst33)))+(((gconst11)*(gconst15)*(gconst26)*(gconst30)))+(((gconst14)*(gconst15)*(gconst18)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst22)*(gconst26)*(gconst28)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst29)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst23)*(gconst29)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst22)*(gconst26)*(gconst30)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst18)*(gconst26)*(gconst28)))+(((gconst14)*(gconst15)*(gconst23)*(gconst30)))+(((gconst12)*(gconst17)*(gconst20)*(gconst33)))+(((gconst12)*(gconst29)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst21)*(gconst29)))+(((gconst19)*(gconst26)*(gconst28)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst32)*(gconst33)))+(((gconst20)*(gconst24)*(gconst33)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst2)*(gconst30)*(gconst34)))+(((gconst18)*(gconst25)*(gconst34)*(gconst5)))+(((gconst19)*(gconst26)*(gconst33)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst23)*(gconst24)*(gconst29)*(gconst6)))+(((gconst13)*(gconst18)*(gconst22)*(gconst29)))+(((gconst16)*(gconst26)*(gconst31)*(gconst9)))+(((gconst16)*(gconst2)*(gconst31)*(gconst33)))+(((gconst11)*(gconst14)*(gconst27)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst29)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst31)*(gconst33)*(gconst5)))+(((gconst23)*(gconst26)*(gconst27)*(gconst6)))+(((gconst16)*(gconst26)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst23)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst26)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst25)*(gconst29)*(gconst7)))+(((gconst22)*(gconst25)*(gconst30)*(gconst5)))+(((gconst18)*(gconst2)*(gconst27)*(gconst35)))+(((gconst21)*(gconst24)*(gconst32)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst19)*(gconst33)))+(((gconst19)*(gconst2)*(gconst28)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst26)*(gconst3)*(gconst33)))+(((gconst12)*(gconst20)*(gconst21)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst20)*(gconst21)*(gconst26)))+(((gconst15)*(gconst26)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst29)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst21)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst28)*(gconst34)*(gconst6)))+(((gconst14)*(gconst3)*(gconst30)*(gconst35)))+(((gconst1)*(gconst17)*(gconst30)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst30)*(gconst35)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst18)*(gconst24)*(gconst29)))+(((gconst0)*(gconst20)*(gconst29)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst21)*(gconst29)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst2)*(gconst28)*(gconst34)))+(((gconst17)*(gconst18)*(gconst22)*(gconst25)))+(((gconst10)*(gconst17)*(gconst25)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst18)*(gconst22)*(gconst26)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst20)*(gconst27)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst18)*(gconst29)*(gconst35)))+(((gconst13)*(gconst29)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst3)*(gconst32)*(gconst33))));
456 op[4]=((((gconst0)*(gconst32)*(gconst33)*(gconst5)))+(((gconst12)*(gconst20)*(gconst33)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst3)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst18)*(gconst34)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst29)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(gconst18)*(gconst26)))+(((gconst24)*(gconst33)*(gconst5)*(gconst8)))+(((gconst0)*(gconst20)*(gconst21)*(gconst29)))+(((gconst11)*(gconst26)*(gconst27)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst20)*(gconst21)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst2)*(gconst28)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst34)*(gconst4)*(gconst6)))+(((gconst26)*(gconst28)*(gconst7)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst18)*(gconst23)))+(((gconst2)*(gconst3)*(gconst30)*(gconst35)))+(((gconst12)*(gconst20)*(gconst29)*(gconst9)))+(((gconst12)*(gconst17)*(gconst32)*(gconst9)))+(((gconst13)*(gconst17)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst28)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst2)*(gconst22)*(gconst28)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst27)*(gconst33)*(gconst8)))+(((gconst14)*(gconst16)*(gconst19)*(gconst21)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst21)*(gconst27)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst31)*(gconst5)*(gconst9)))+(((gconst14)*(gconst21)*(gconst31)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst31)*(gconst33)*(gconst5)))+(((gconst24)*(gconst32)*(gconst5)*(gconst9)))+(((gconst12)*(gconst21)*(gconst32)*(gconst5)))+(((gconst12)*(gconst17)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst18)*(gconst35)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst22)*(gconst28)*(gconst6)))+(((gconst10)*(gconst25)*(gconst30)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst30)*(gconst34)*(gconst4)))+(((gconst19)*(gconst2)*(gconst21)*(gconst28)))+(((gconst14)*(gconst19)*(gconst28)*(gconst9)))+(((gconst1)*(gconst17)*(gconst22)*(gconst30)))+(((gconst14)*(gconst15)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst27)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst26)*(gconst28)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst27)*(gconst9)))+(((gconst2)*(gconst28)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst23)*(gconst24)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst3)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst29)*(gconst7)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst21)*(gconst3)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst21)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst21)*(gconst25)*(gconst7)))+(((gconst0)*(gconst17)*(gconst21)*(gconst32)))+(((gconst20)*(gconst21)*(gconst24)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst19)*(gconst25)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst2)*(gconst22)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst31)*(gconst9)))+(((gconst1)*(gconst30)*(gconst34)*(gconst5)))+(((gconst18)*(gconst22)*(gconst25)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst19)*(gconst21)*(gconst25)*(gconst5)))+(((gconst14)*(gconst21)*(gconst28)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst24)*(gconst29)*(gconst6)))+(((gconst12)*(gconst17)*(gconst20)*(gconst21)))+(((gconst14)*(gconst23)*(gconst27)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst23)*(gconst29)*(gconst6)))+(((gconst17)*(gconst22)*(gconst25)*(gconst6)))+(((gconst16)*(gconst21)*(gconst26)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst20)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst23)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst3)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst18)*(gconst2)*(gconst34)))+(((gconst13)*(gconst17)*(gconst18)*(gconst22)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst21)*(gconst26)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst18)*(gconst22)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst29)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst18)*(gconst28)))+(((gconst14)*(gconst23)*(gconst3)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst17)*(gconst18)*(gconst24)))+(((gconst14)*(gconst19)*(gconst33)*(gconst4)))+(((gconst13)*(gconst22)*(gconst30)*(gconst5)))+(((gconst10)*(gconst13)*(gconst17)*(gconst30)))+(((gconst1)*(gconst29)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst18)*(gconst23)*(gconst29)))+(((gconst17)*(gconst20)*(gconst24)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst18)*(gconst35)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst20)*(gconst21)*(gconst27)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst22)*(gconst26)*(gconst6)))+(((gconst15)*(gconst2)*(gconst23)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst18)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst33)*(gconst8)))+(((gconst25)*(gconst34)*(gconst5)*(gconst6)))+(((gconst15)*(gconst18)*(gconst2)*(gconst35)))+(((gconst26)*(gconst3)*(gconst35)*(gconst6)))+(((gconst14)*(gconst16)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst23)*(gconst24)*(gconst5)))+(((gconst10)*(gconst13)*(gconst18)*(gconst29)))+(((gconst10)*(gconst17)*(gconst18)*(gconst25)))+(((gconst0)*(gconst17)*(gconst20)*(gconst33)))+(((gconst16)*(gconst19)*(gconst2)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst24)*(gconst30)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst26)*(gconst30)*(gconst4)))+(((gconst14)*(gconst16)*(gconst31)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst19)*(gconst33)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst20)*(gconst26)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst22)*(gconst26)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst16)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst33)*(gconst5)*(gconst7)))+(((gconst2)*(gconst31)*(gconst33)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst19)*(gconst21)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst27)*(gconst32)*(gconst9)))+(((gconst1)*(gconst10)*(gconst29)*(gconst30)))+(((gconst14)*(gconst15)*(gconst18)*(gconst23)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst19)*(gconst21)))+(((gconst0)*(gconst29)*(gconst32)*(gconst9)))+(((gconst14)*(gconst18)*(gconst3)*(gconst35)))+(((gconst12)*(gconst21)*(gconst29)*(gconst8)))+(((gconst13)*(gconst22)*(gconst29)*(gconst6)))+(((gconst2)*(gconst27)*(gconst35)*(gconst6)))+(((gconst1)*(gconst17)*(gconst18)*(gconst34)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst21)*(gconst31)*(gconst5)))+(((gconst11)*(gconst2)*(gconst27)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst29)*(gconst33)*(gconst7)))+(((gconst11)*(gconst14)*(gconst15)*(gconst30)))+(((gconst11)*(gconst26)*(gconst3)*(gconst30)))+(((gconst18)*(gconst2)*(gconst23)*(gconst27)))+(((gconst1)*(gconst18)*(gconst22)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst21)*(gconst29)*(gconst7)))+(((gconst24)*(gconst29)*(gconst8)*(gconst9)))+(((gconst2)*(gconst28)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst30)*(gconst35)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst33)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst21)*(gconst32)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst29)*(gconst35)*(gconst6)))+(((gconst13)*(gconst18)*(gconst34)*(gconst5)))+(((gconst16)*(gconst19)*(gconst26)*(gconst9)))+(((gconst10)*(gconst25)*(gconst29)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst17)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst3)*(gconst32)*(gconst33)))+(((gconst26)*(gconst31)*(gconst4)*(gconst9)))+(((gconst11)*(gconst14)*(gconst18)*(gconst27)))+(((gconst19)*(gconst21)*(gconst26)*(gconst4)))+(((gconst17)*(gconst21)*(gconst24)*(gconst8)))+(((gconst15)*(gconst23)*(gconst26)*(gconst6)))+(((gconst26)*(gconst33)*(gconst4)*(gconst7)))+(((gconst18)*(gconst23)*(gconst26)*(gconst3)))+(((gconst11)*(gconst15)*(gconst18)*(gconst26)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst23)*(gconst30)*(gconst5)))+(((gconst16)*(gconst2)*(gconst21)*(gconst31)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst29)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst24)*(gconst35)*(gconst5)*(gconst6)))+(((gconst0)*(gconst29)*(gconst33)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst22)*(gconst30)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst21)*(gconst26)*(gconst3))));
457 op[5]=((((IkReal(-1.00000000000000))*(gconst1)*(gconst19)*(gconst29)*(gconst9)))+(((gconst2)*(gconst23)*(gconst27)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst18)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst29)*(gconst6)))+(((gconst23)*(gconst26)*(gconst3)*(gconst6)))+(((gconst0)*(gconst17)*(gconst33)*(gconst8)))+(((gconst1)*(gconst17)*(gconst34)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst33)*(gconst8)))+(((gconst14)*(gconst31)*(gconst4)*(gconst9)))+(((gconst16)*(gconst2)*(gconst31)*(gconst9)))+(((gconst11)*(gconst18)*(gconst2)*(gconst27)))+(((gconst1)*(gconst18)*(gconst34)*(gconst5)))+(((gconst14)*(gconst28)*(gconst7)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst20)*(gconst9)))+(((gconst12)*(gconst17)*(gconst20)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst16)*(gconst22)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(gconst26)*(gconst6)))+(((gconst13)*(gconst18)*(gconst22)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst21)*(gconst3)*(gconst32)))+(((gconst15)*(gconst18)*(gconst2)*(gconst23)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst35)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst26)*(gconst8)*(gconst9)))+(((gconst12)*(gconst32)*(gconst5)*(gconst9)))+(((gconst11)*(gconst18)*(gconst26)*(gconst3)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst21)*(gconst5)))+(((gconst11)*(gconst14)*(gconst27)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst31)*(gconst9)))+(((gconst2)*(gconst23)*(gconst3)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst18)*(gconst24)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst2)*(gconst34)*(gconst4)))+(((gconst0)*(gconst17)*(gconst20)*(gconst21)))+(((gconst22)*(gconst25)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst33)*(gconst5)*(gconst7)))+(((gconst0)*(gconst17)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst25)*(gconst5)*(gconst7)))+(((gconst0)*(gconst20)*(gconst29)*(gconst9)))+(((gconst0)*(gconst20)*(gconst33)*(gconst5)))+(((gconst0)*(gconst21)*(gconst32)*(gconst5)))+(((gconst18)*(gconst2)*(gconst3)*(gconst35)))+(((gconst12)*(gconst17)*(gconst21)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst3)*(gconst33)*(gconst8)))+(((gconst10)*(gconst17)*(gconst25)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst3)*(gconst32)*(gconst9)))+(((gconst14)*(gconst33)*(gconst4)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst19)*(gconst25)*(gconst5)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst20)*(gconst3)*(gconst33)))+(((gconst15)*(gconst2)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst22)*(gconst28)*(gconst6)))+(((gconst14)*(gconst15)*(gconst23)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst21)*(gconst31)*(gconst5)))+(((gconst2)*(gconst21)*(gconst28)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst30)*(gconst5)))+(((gconst19)*(gconst26)*(gconst4)*(gconst9)))+(((gconst11)*(gconst14)*(gconst3)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(gconst2)*(gconst30)))+(((gconst19)*(gconst2)*(gconst33)*(gconst4)))+(((gconst1)*(gconst10)*(gconst18)*(gconst29)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst18)*(gconst2)*(gconst28)))+(((gconst11)*(gconst15)*(gconst26)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst27)*(gconst8)*(gconst9)))+(((gconst11)*(gconst14)*(gconst15)*(gconst18)))+(((IkReal(-1.00000000000000))*(gconst17)*(gconst25)*(gconst7)*(gconst9)))+(((gconst13)*(gconst34)*(gconst5)*(gconst6)))+(((gconst16)*(gconst2)*(gconst33)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst18)*(gconst23)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst28)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst19)*(gconst33)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst18)*(gconst2)*(gconst22)))+(((gconst10)*(gconst13)*(gconst29)*(gconst6)))+(((gconst19)*(gconst2)*(gconst28)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst21)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst26)*(gconst3)*(gconst9)))+(((gconst1)*(gconst10)*(gconst17)*(gconst30)))+(((gconst14)*(gconst18)*(gconst23)*(gconst3)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst22)*(gconst30)*(gconst4)))+(((gconst16)*(gconst26)*(gconst7)*(gconst9)))+(((gconst21)*(gconst24)*(gconst5)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst18)*(gconst35)*(gconst5)))+(((gconst14)*(gconst16)*(gconst19)*(gconst9)))+(((gconst16)*(gconst19)*(gconst2)*(gconst21)))+(((gconst1)*(gconst17)*(gconst18)*(gconst22)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst16)*(gconst18)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst23)*(gconst29)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst21)*(gconst3)))+(((gconst13)*(gconst17)*(gconst22)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst21)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst21)*(gconst26)*(gconst3)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst32)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst23)*(gconst24)*(gconst5)*(gconst6)))+(((gconst1)*(gconst22)*(gconst29)*(gconst6)))+(((gconst2)*(gconst21)*(gconst31)*(gconst4)))+(((gconst10)*(gconst13)*(gconst30)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst18)*(gconst26)*(gconst4)))+(((gconst14)*(gconst3)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst23)*(gconst30)*(gconst5)))+(((gconst12)*(gconst29)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst19)*(gconst21)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst19)*(gconst9)))+(((gconst10)*(gconst18)*(gconst25)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst29)*(gconst7)*(gconst9)))+(((gconst0)*(gconst21)*(gconst29)*(gconst8)))+(((gconst20)*(gconst24)*(gconst5)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst18)*(gconst22)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst21)*(gconst29)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst31)*(gconst5)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst17)*(gconst24)*(gconst6)))+(((gconst14)*(gconst16)*(gconst21)*(gconst7)))+(((gconst21)*(gconst26)*(gconst4)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst20)*(gconst21)))+(((gconst12)*(gconst33)*(gconst5)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst17)*(gconst23)*(gconst6)))+(((gconst11)*(gconst15)*(gconst2)*(gconst30)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst20)*(gconst27)*(gconst9)))+(((gconst1)*(gconst22)*(gconst30)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst18)*(gconst23)))+(((gconst10)*(gconst13)*(gconst17)*(gconst18)))+(((gconst12)*(gconst20)*(gconst21)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst34)*(gconst4)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst17)*(gconst30)))+(((gconst17)*(gconst24)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst21)*(gconst27)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst30)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst17)*(gconst18)))+(((IkReal(-1.00000000000000))*(gconst22)*(gconst26)*(gconst4)*(gconst6)))+(((gconst14)*(gconst19)*(gconst21)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst2)*(gconst34)*(gconst6))));
458 op[6]=((((gconst12)*(gconst20)*(gconst5)*(gconst9)))+(((gconst11)*(gconst2)*(gconst3)*(gconst30)))+(((gconst14)*(gconst21)*(gconst4)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst21)*(gconst5)*(gconst7)))+(((gconst0)*(gconst20)*(gconst21)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst20)*(gconst21)*(gconst3)))+(((gconst1)*(gconst18)*(gconst22)*(gconst5)))+(((gconst11)*(gconst15)*(gconst18)*(gconst2)))+(((gconst1)*(gconst10)*(gconst30)*(gconst5)))+(((gconst14)*(gconst23)*(gconst3)*(gconst6)))+(((gconst0)*(gconst17)*(gconst21)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst19)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst19)*(gconst5)*(gconst9)))+(((gconst15)*(gconst2)*(gconst23)*(gconst6)))+(((gconst11)*(gconst26)*(gconst3)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst35)*(gconst5)*(gconst6)))+(((gconst14)*(gconst16)*(gconst7)*(gconst9)))+(((gconst12)*(gconst21)*(gconst5)*(gconst8)))+(((gconst0)*(gconst17)*(gconst20)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst21)*(gconst8)))+(((gconst2)*(gconst31)*(gconst4)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst26)*(gconst3)*(gconst8)*(gconst9)))+(((gconst24)*(gconst5)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(gconst18)*(gconst2)))+(((gconst2)*(gconst3)*(gconst35)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst29)*(gconst7)*(gconst9)))+(((gconst1)*(gconst10)*(gconst17)*(gconst18)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst22)*(gconst4)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst16)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst16)*(gconst2)*(gconst22)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst20)*(gconst3)*(gconst9)))+(((gconst18)*(gconst2)*(gconst23)*(gconst3)))+(((gconst2)*(gconst28)*(gconst7)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst17)*(gconst6)))+(((gconst13)*(gconst22)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst2)*(gconst28)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst2)*(gconst30)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst17)*(gconst21)*(gconst7)))+(((gconst10)*(gconst13)*(gconst17)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst26)*(gconst4)*(gconst6)))+(((gconst14)*(gconst19)*(gconst4)*(gconst9)))+(((gconst1)*(gconst34)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst12)*(gconst18)*(gconst5)))+(((gconst19)*(gconst2)*(gconst21)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst17)*(gconst23)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst30)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst18)*(gconst4)))+(((IkReal(-1.00000000000000))*(gconst13)*(gconst17)*(gconst7)*(gconst9)))+(((gconst26)*(gconst4)*(gconst7)*(gconst9)))+(((gconst0)*(gconst29)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst17)*(gconst18)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst27)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst3)*(gconst33)*(gconst8)))+(((gconst16)*(gconst19)*(gconst2)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst34)*(gconst4)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst12)*(gconst23)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst31)*(gconst5)*(gconst9)))+(((gconst11)*(gconst14)*(gconst18)*(gconst3)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst33)*(gconst5)*(gconst7)))+(((gconst2)*(gconst33)*(gconst4)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst21)*(gconst3)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(gconst20)*(gconst9)))+(((gconst16)*(gconst2)*(gconst21)*(gconst7)))+(((IkReal(-1.00000000000000))*(gconst25)*(gconst5)*(gconst7)*(gconst9)))+(((gconst10)*(gconst25)*(gconst5)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst18)*(gconst23)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst1)*(gconst19)*(gconst21)*(gconst5)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst3)*(gconst32)*(gconst9)))+(((gconst0)*(gconst33)*(gconst5)*(gconst8)))+(((IkReal(-1.00000000000000))*(gconst14)*(gconst15)*(gconst8)*(gconst9)))+(((gconst11)*(gconst2)*(gconst27)*(gconst6)))+(((gconst11)*(gconst14)*(gconst15)*(gconst6)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst24)*(gconst5)*(gconst6)))+(((gconst10)*(gconst13)*(gconst18)*(gconst5)))+(((gconst0)*(gconst32)*(gconst5)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst18)*(gconst2)*(gconst22)*(gconst4)))+(((gconst1)*(gconst17)*(gconst22)*(gconst6)))+(((gconst12)*(gconst17)*(gconst8)*(gconst9)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst29)*(gconst6)))+(((gconst1)*(gconst10)*(gconst29)*(gconst6))));
459 op[7]=((((x70)*(x72)))+(((gconst11)*(gconst15)*(x62)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst14)*(gconst4)*(x67)))+(((IkReal(-1.00000000000000))*(gconst11)*(gconst18)*(x66)))+(((x63)*(x93)))+(((IkReal(-1.00000000000000))*(gconst15)*(gconst2)*(x90)))+(((gconst23)*(gconst3)*(x62)))+(((IkReal(-1.00000000000000))*(x82)*(x94)))+(((IkReal(-1.00000000000000))*(gconst22)*(gconst4)*(x62)))+(((x84)*(x90)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst16)*(x62)))+(((gconst22)*(gconst6)*(x82)))+(((gconst20)*(gconst9)*(x66)))+(((x80)*(x83)))+(((gconst0)*(gconst17)*(x90)))+(((IkReal(-1.00000000000000))*(x72)*(x73)))+(((IkReal(-1.00000000000000))*(gconst10)*(gconst18)*(x80)))+(((IkReal(-1.00000000000000))*(gconst2)*(gconst3)*(x71)))+(((IkReal(-1.00000000000000))*(gconst11)*(x67)*(x84)))+(((IkReal(-1.00000000000000))*(gconst1)*(x61)*(x73)))+(((gconst10)*(gconst18)*(x82)))+(((x63)*(x92)))+(((IkReal(-1.00000000000000))*(x82)*(x83)))+(((x66)*(x71)))+(((gconst16)*(gconst2)*(x73)))+(((IkReal(-1.00000000000000))*(gconst0)*(gconst11)*(gconst6)*(x61)))+(((IkReal(-1.00000000000000))*(gconst23)*(x66)*(x67)))+(((gconst14)*(gconst4)*(x73)))+(((x80)*(x94)))+(((gconst1)*(gconst17)*(x70)))+(((IkReal(-1.00000000000000))*(gconst9)*(x68)*(x87)))+(((IkReal(-1.00000000000000))*(gconst20)*(gconst3)*(x97))));
460 op[8]=((((IkReal(-1.00000000000000))*(gconst11)*(x66)*(x67)))+(((x66)*(x90)))+(((IkReal(-1.00000000000000))*(x73)*(x82)))+(((x62)*(x63)))+(((x73)*(x80)))+(((x70)*(x82)))+(((IkReal(-1.00000000000000))*(x87)*(x97)))+(((IkReal(-1.00000000000000))*(x62)*(x95))));
461 polyroots8(op,zeror,numroots);
462 IkReal j4array[8], cj4array[8], sj4array[8], tempj4array[1];
463 int numsolutions = 0;
464 for(
int ij4 = 0; ij4 < numroots; ++ij4)
466 IkReal htj4 = zeror[ij4];
467 tempj4array[0]=((IkReal(2.00000000000000))*(
atan(htj4)));
468 for(
int kj4 = 0; kj4 < 1; ++kj4)
470 j4array[numsolutions] = tempj4array[kj4];
471 if( j4array[numsolutions] >
IKPI )
473 j4array[numsolutions]-=
IK2PI;
475 else if( j4array[numsolutions] < -
IKPI )
477 j4array[numsolutions]+=
IK2PI;
479 sj4array[numsolutions] =
IKsin(j4array[numsolutions]);
480 cj4array[numsolutions] =
IKcos(j4array[numsolutions]);
484 bool j4valid[8]={
true,
true,
true,
true,
true,
true,
true,
true};
486 for(
int ij4 = 0; ij4 < numsolutions; ++ij4)
492 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
495 _ij4[0] = ij4; _ij4[1] = -1;
496 for(
int iij4 = ij4+1; iij4 < numsolutions; ++iij4)
500 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
506 IkReal x98=(sj4)*(sj4);
507 IkReal x99=((IkReal(0.0180500000000000))*(cj4));
508 IkReal x100=((IkReal(0.190000000000000))*(sj4));
509 IkReal x101=((IkReal(0.0665000000000000))*(cj4)*(sj4));
510 IkReal x102=((IkReal(0.700000000000000))*(x98));
511 gconst52=
IKsign(((((IkReal(-1.00000000000000))*(npy)*(r21)*(x99)))+(((npy)*(r20)*(x101)))+(((IkReal(-1.00000000000000))*(npx)*(rxp0_2)*(x100)))+(((npy)*(rxp0_2)*(x102)))+(((IkReal(-1.00000000000000))*(npx)*(rxp1_2)*(x102)))+(((IkReal(-1.00000000000000))*(npx)*(r21)*(x101)))+(((IkReal(-1.00000000000000))*(npy)*(rxp1_2)*(x100)))+(((IkReal(-1.00000000000000))*(npx)*(r20)*(x99)))));
512 IkReal x103=(sj4)*(sj4);
513 IkReal x104=((cj4)*(r20));
514 IkReal x105=((npy)*(sj4));
515 IkReal x106=((cj4)*(r21));
516 IkReal x107=((npx)*(sj4));
517 IkReal x108=((IkReal(38.7811634349030))*(x103));
518 dummyeval[0]=((((IkReal(-1.00000000000000))*(npx)*(rxp1_2)*(x108)))+(((IkReal(-10.5263157894737))*(rxp0_2)*(x107)))+(((npy)*(rxp0_2)*(x108)))+(((IkReal(-1.00000000000000))*(npy)*(x106)))+(((IkReal(3.68421052631579))*(x104)*(x105)))+(((IkReal(-3.68421052631579))*(x106)*(x107)))+(((IkReal(-1.00000000000000))*(npx)*(x104)))+(((IkReal(-10.5263157894737))*(rxp1_2)*(x105))));
519 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
526 IkReal j5array[1], cj5array[1], sj5array[1];
527 bool j5valid[1]={
false};
529 IkReal x109=(cj4)*(cj4);
530 IkReal x110=(sj4)*(sj4);
531 IkReal x111=((rxp0_2)*(sj4));
532 IkReal x112=((IkReal(0.700000000000000))*(cj4));
533 IkReal x113=((npx)*(rxp2_2));
534 IkReal x114=((IkReal(0.190000000000000))*(cj4));
535 IkReal x115=((rxp1_2)*(sj4));
536 IkReal x116=((IkReal(0.000857375000000000))*(cj4));
537 IkReal x117=((npy)*(rxp2_2));
538 IkReal x118=((IkReal(0.0950000000000000))*(cj4)*(pp));
539 IkReal x119=((IkReal(0.0180500000000000))*(r22)*(sj4));
540 IkReal x120=((IkReal(0.0665000000000000))*(npz)*(x109));
541 IkReal x121=((IkReal(0.0665000000000000))*(r22)*(x110));
542 if(
IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(npy)*(x119)))+(((r20)*(x118)))+(((pp)*(x111)))+(((IkReal(-1.00000000000000))*(r20)*(x120)))+(((IkReal(-1.00000000000000))*(npx)*(x121)))+(((sj4)*(x112)*(x113)))+(((r20)*(x116)))+(((IkReal(-1.00000000000000))*(npz)*(x111)*(x112)))+(((IkReal(0.00902500000000000))*(x111)))+(((x114)*(x117))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst52)*(((((IkReal(0.00902500000000000))*(x115)))+(((IkReal(-1.00000000000000))*(npy)*(x121)))+(((IkReal(-1.00000000000000))*(x113)*(x114)))+(((IkReal(-1.00000000000000))*(r21)*(x120)))+(((r21)*(x118)))+(((sj4)*(x112)*(x117)))+(((IkReal(-1.00000000000000))*(npz)*(x112)*(x115)))+(((npx)*(x119)))+(((r21)*(x116)))+(((pp)*(x115))))))) <
IKFAST_ATAN2_MAGTHRESH )
544 j5array[0]=
IKatan2(((gconst52)*(((((IkReal(-1.00000000000000))*(npy)*(x119)))+(((r20)*(x118)))+(((pp)*(x111)))+(((IkReal(-1.00000000000000))*(r20)*(x120)))+(((IkReal(-1.00000000000000))*(npx)*(x121)))+(((sj4)*(x112)*(x113)))+(((r20)*(x116)))+(((IkReal(-1.00000000000000))*(npz)*(x111)*(x112)))+(((IkReal(0.00902500000000000))*(x111)))+(((x114)*(x117)))))), ((gconst52)*(((((IkReal(0.00902500000000000))*(x115)))+(((IkReal(-1.00000000000000))*(npy)*(x121)))+(((IkReal(-1.00000000000000))*(x113)*(x114)))+(((IkReal(-1.00000000000000))*(r21)*(x120)))+(((r21)*(x118)))+(((sj4)*(x112)*(x117)))+(((IkReal(-1.00000000000000))*(npz)*(x112)*(x115)))+(((npx)*(x119)))+(((r21)*(x116)))+(((pp)*(x115)))))));
545 sj5array[0]=
IKsin(j5array[0]);
546 cj5array[0]=
IKcos(j5array[0]);
547 if( j5array[0] >
IKPI )
551 else if( j5array[0] < -
IKPI )
555 for(
int ij5 = 0; ij5 < 1; ++ij5)
561 _ij5[0] = ij5; _ij5[1] = -1;
562 for(
int iij5 = ij5+1; iij5 < 1; ++iij5)
566 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
569 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
572 IkReal x122=
IKcos(j5);
573 IkReal x123=
IKsin(j5);
574 IkReal x124=((IkReal(0.0950000000000000))*(cj4));
575 IkReal x125=((sj4)*(x122));
576 IkReal x126=((sj4)*(x123));
577 evalcond[0]=((((IkReal(-1.00000000000000))*(rxp1_2)*(x126)))+(((rxp0_2)*(x125)))+(((IkReal(-1.00000000000000))*(r21)*(x123)*(x124)))+(((IkReal(-1.00000000000000))*(cj4)*(rxp2_2)))+(((r20)*(x122)*(x124)))+(((IkReal(0.0950000000000000))*(r22)*(sj4))));
578 evalcond[1]=((IkReal(-0.00902500000000000))+(((IkReal(-0.190000000000000))*(npx)*(x123)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.700000000000000))*(npy)*(x126)))+(((IkReal(0.700000000000000))*(cj4)*(npz)))+(((IkReal(-0.190000000000000))*(npy)*(x122)))+(((IkReal(-0.700000000000000))*(npx)*(x125))));
579 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
586 IkReal j2array[2], cj2array[2], sj2array[2];
587 bool j2valid[2]={
false};
589 IkReal x127=((IkReal(2.85714285714286))*(sj4));
590 cj2array[0]=((IkReal(-1.00000000000000))+(((IkReal(2.85714285714286))*(cj4)*(npz)))+(((npy)*(sj5)*(x127)))+(((IkReal(-1.00000000000000))*(cj5)*(npx)*(x127))));
593 j2valid[0] = j2valid[1] =
true;
594 j2array[0] =
IKacos(cj2array[0]);
595 sj2array[0] =
IKsin(j2array[0]);
596 cj2array[1] = cj2array[0];
597 j2array[1] = -j2array[0];
598 sj2array[1] = -sj2array[0];
600 else if( isnan(cj2array[0]) )
604 cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
606 for(
int ij2 = 0; ij2 < 2; ++ij2)
612 _ij2[0] = ij2; _ij2[1] = -1;
613 for(
int iij2 = ij2+1; iij2 < 2; ++iij2)
617 j2valid[iij2]=
false; _ij2[1] = iij2;
break;
620 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
627 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
630 IkReal j0array[2], cj0array[2], sj0array[2];
631 bool j0valid[2]={
false};
633 IkReal x128=((cj5)*(sj4));
634 IkReal x129=((sj4)*(sj5));
637 IkReal x130=
IKatan2(((((IkReal(-1.00000000000000))*(cj4)*(r12)))+(((IkReal(-1.00000000000000))*(r11)*(x129)))+(((r10)*(x128)))), ((((IkReal(-1.00000000000000))*(r00)*(x128)))+(((cj4)*(r02)))+(((r01)*(x129)))));
638 j0array[0]=((IkReal(-1.00000000000000))*(x130));
639 sj0array[0]=
IKsin(j0array[0]);
640 cj0array[0]=
IKcos(j0array[0]);
641 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x130))));
642 sj0array[1]=
IKsin(j0array[1]);
643 cj0array[1]=
IKcos(j0array[1]);
644 if( j0array[0] >
IKPI )
648 else if( j0array[0] < -
IKPI )
652 if( j0array[1] >
IKPI )
656 else if( j0array[1] < -
IKPI )
660 for(
int ij0 = 0; ij0 < 2; ++ij0)
666 _ij0[0] = ij0; _ij0[1] = -1;
667 for(
int iij0 = ij0+1; iij0 < 2; ++iij0)
671 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
674 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
677 IkReal j3array[1], cj3array[1], sj3array[1];
678 bool j3valid[1]={
false};
680 IkReal x131=((cj4)*(cj5));
681 IkReal x132=((IkReal(1.00000000000000))*(sj0));
682 IkReal x133=((cj0)*(r10));
683 IkReal x134=((cj0)*(r11));
684 IkReal x135=((cj4)*(sj5));
685 if(
IKabs(((((x131)*(x133)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(x131)*(x132)))+(((IkReal(-1.00000000000000))*(x134)*(x135)))+(((cj0)*(r12)*(sj4)))+(((r01)*(sj0)*(x135))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((((cj5)*(x134)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x132)))+(((sj5)*(x133))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(
IKsqr(((((x131)*(x133)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(x131)*(x132)))+(((IkReal(-1.00000000000000))*(x134)*(x135)))+(((cj0)*(r12)*(sj4)))+(((r01)*(sj0)*(x135)))))+
IKsqr(((((cj5)*(x134)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x132)))+(((sj5)*(x133)))))-1) <=
IKFAST_SINCOS_THRESH )
687 j3array[0]=
IKatan2(((((x131)*(x133)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(x131)*(x132)))+(((IkReal(-1.00000000000000))*(x134)*(x135)))+(((cj0)*(r12)*(sj4)))+(((r01)*(sj0)*(x135)))), ((((cj5)*(x134)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x132)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x132)))+(((sj5)*(x133)))));
688 sj3array[0]=
IKsin(j3array[0]);
689 cj3array[0]=
IKcos(j3array[0]);
690 if( j3array[0] >
IKPI )
694 else if( j3array[0] < -
IKPI )
698 for(
int ij3 = 0; ij3 < 1; ++ij3)
704 _ij3[0] = ij3; _ij3[1] = -1;
705 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
709 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
712 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
715 IkReal x136=
IKcos(j3);
716 IkReal x137=
IKsin(j3);
717 IkReal x138=((r00)*(sj0));
718 IkReal x139=((cj4)*(cj5));
719 IkReal x140=((IkReal(1.00000000000000))*(cj0));
720 IkReal x141=((IkReal(0.350000000000000))*(sj2));
721 IkReal x142=((cj4)*(sj5));
722 IkReal x143=((r01)*(sj0));
723 evalcond[0]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(-1.00000000000000))*(x137)*(x141)))+(((npx)*(sj5))));
724 evalcond[1]=((((x136)*(x141)))+(((npx)*(x139)))+(((IkReal(-1.00000000000000))*(npy)*(x142)))+(((npz)*(sj4))));
725 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj5)*(x140)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x140)))+(((sj5)*(x138)))+(((cj5)*(x143)))+(x136));
726 evalcond[3]=((((IkReal(-1.00000000000000))*(r10)*(x139)*(x140)))+(((cj0)*(r11)*(x142)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x140)))+(((r02)*(sj0)*(sj4)))+(((x138)*(x139)))+(x137)+(((IkReal(-1.00000000000000))*(x142)*(x143))));
727 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
736 gconst74=
IKsign(((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2))))));
737 dummyeval[0]=((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2)))));
738 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
743 gconst75=
IKsign(((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2))))));
744 dummyeval[0]=((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2)))));
745 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
749 IkReal x144=((sj0)*(sj4));
750 IkReal x145=((IkReal(1.00000000000000))*(r12));
751 IkReal x146=((cj4)*(cj5));
752 IkReal x147=((IkReal(1.00000000000000))*(cj0));
753 IkReal x148=((cj4)*(sj0));
754 IkReal x149=((IkReal(1.00000000000000))*(sj5));
755 IkReal x150=((cj0)*(cj4));
756 IkReal x151=((r00)*(sj0));
757 IkReal x152=((sj4)*(sj5));
758 IkReal x153=((IkReal(1.00000000000000))*(cj5));
759 IkReal x154=((r01)*(sj5));
760 IkReal x155=((cj0)*(sj4));
761 IkReal x156=((r11)*(sj5));
762 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(1.57079632679490))+(j3)), IkReal(6.28318530717959))));
763 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(-0.350000000000000))*(sj2)))+(((npx)*(sj5))));
764 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((npy)*(x152)))+(((cj4)*(npz)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x153))));
765 evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(npy)*(x149)))+(((npz)*(sj4)))+(((npx)*(x146))));
766 evalcond[4]=((((sj5)*(x151)))+(((cj5)*(r01)*(sj0)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x147)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x147))));
767 evalcond[5]=((((r20)*(x146)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x149))));
768 evalcond[6]=((((IkReal(-1.00000000000000))*(r00)*(x144)*(x153)))+(((IkReal(-1.00000000000000))*(r11)*(x147)*(x152)))+(((IkReal(-1.00000000000000))*(x145)*(x150)))+(((r02)*(x148)))+(((cj5)*(r10)*(x155)))+(((x144)*(x154))));
769 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r01)*(x148)*(x149)))+(((r02)*(x144)))+(((x146)*(x151)))+(((x150)*(x156)))+(((IkReal(-1.00000000000000))*(x145)*(x155)))+(((IkReal(-1.00000000000000))*(r10)*(x146)*(x147))));
770 evalcond[8]=((((IkReal(-1.00000000000000))*(x144)*(x145)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x147)))+(((x148)*(x156)))+(((x150)*(x154)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x146)))+(((IkReal(-1.00000000000000))*(r00)*(x146)*(x147))));
771 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
776 gconst76=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
777 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
778 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
783 gconst77=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
784 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
785 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
792 IkReal j1array[1], cj1array[1], sj1array[1];
793 bool j1valid[1]={
false};
795 IkReal x157=((cj2)*(sj5));
796 IkReal x158=((r10)*(sj0));
797 IkReal x159=((cj2)*(cj5));
798 IkReal x160=((cj0)*(r00));
799 IkReal x161=((sj2)*(sj5));
800 IkReal x162=((r11)*(sj0));
801 IkReal x163=((cj5)*(sj2));
802 IkReal x164=((cj0)*(r01));
803 if(
IKabs(((gconst77)*(((((x160)*(x161)))+(((r21)*(x159)))+(((x162)*(x163)))+(((x163)*(x164)))+(((x158)*(x161)))+(((r20)*(x157))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(x159)*(x162)))+(((IkReal(-1.00000000000000))*(x159)*(x164)))+(((IkReal(-1.00000000000000))*(x157)*(x158)))+(((IkReal(-1.00000000000000))*(x157)*(x160)))+(((r20)*(x161)))+(((r21)*(x163))))))) <
IKFAST_ATAN2_MAGTHRESH )
805 j1array[0]=
IKatan2(((gconst77)*(((((x160)*(x161)))+(((r21)*(x159)))+(((x162)*(x163)))+(((x163)*(x164)))+(((x158)*(x161)))+(((r20)*(x157)))))), ((gconst77)*(((((IkReal(-1.00000000000000))*(x159)*(x162)))+(((IkReal(-1.00000000000000))*(x159)*(x164)))+(((IkReal(-1.00000000000000))*(x157)*(x158)))+(((IkReal(-1.00000000000000))*(x157)*(x160)))+(((r20)*(x161)))+(((r21)*(x163)))))));
806 sj1array[0]=
IKsin(j1array[0]);
807 cj1array[0]=
IKcos(j1array[0]);
808 if( j1array[0] >
IKPI )
812 else if( j1array[0] < -
IKPI )
816 for(
int ij1 = 0; ij1 < 1; ++ij1)
822 _ij1[0] = ij1; _ij1[1] = -1;
823 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
827 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
830 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
833 IkReal x165=
IKsin(j1);
834 IkReal x166=
IKcos(j1);
835 IkReal x167=((cj0)*(sj4));
836 IkReal x168=((IkReal(1.00000000000000))*(sj5));
837 IkReal x169=((cj5)*(sj0));
838 IkReal x170=((IkReal(1.00000000000000))*(cj4));
839 IkReal x171=((IkReal(1.00000000000000))*(cj5));
840 IkReal x172=((sj2)*(x165));
841 IkReal x173=((IkReal(1.00000000000000))*(x166));
842 IkReal x174=((cj2)*(x165));
843 IkReal x175=((cj2)*(x173));
844 evalcond[0]=((((IkReal(-1.00000000000000))*(x174)))+(((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(sj2)*(x173)))+(((cj5)*(r21))));
845 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(sj4)*(x171)))+(((IkReal(-1.00000000000000))*(x175)))+(x172)+(((cj4)*(r22)))+(((r21)*(sj4)*(sj5))));
846 evalcond[2]=((((IkReal(-1.00000000000000))*(x175)))+(x172)+(((IkReal(-1.00000000000000))*(r11)*(x169)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x168)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x168)))+(((IkReal(-1.00000000000000))*(cj0)*(r01)*(x171))));
847 evalcond[3]=((((r10)*(sj4)*(x169)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x170)))+(x174)+(((cj5)*(r00)*(x167)))+(((sj2)*(x166)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x170)))+(((IkReal(-1.00000000000000))*(r01)*(x167)*(x168)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x168))));
848 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
855 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
856 vinfos[0].jointtype = 1;
857 vinfos[0].foffset = j0;
858 vinfos[0].indices[0] = _ij0[0];
859 vinfos[0].indices[1] = _ij0[1];
860 vinfos[0].maxsolutions = _nj0;
861 vinfos[1].jointtype = 1;
862 vinfos[1].foffset = j1;
863 vinfos[1].indices[0] = _ij1[0];
864 vinfos[1].indices[1] = _ij1[1];
865 vinfos[1].maxsolutions = _nj1;
866 vinfos[2].jointtype = 1;
867 vinfos[2].foffset = j2;
868 vinfos[2].indices[0] = _ij2[0];
869 vinfos[2].indices[1] = _ij2[1];
870 vinfos[2].maxsolutions = _nj2;
871 vinfos[3].jointtype = 1;
872 vinfos[3].foffset = j3;
873 vinfos[3].indices[0] = _ij3[0];
874 vinfos[3].indices[1] = _ij3[1];
875 vinfos[3].maxsolutions = _nj3;
876 vinfos[4].jointtype = 1;
877 vinfos[4].foffset = j4;
878 vinfos[4].indices[0] = _ij4[0];
879 vinfos[4].indices[1] = _ij4[1];
880 vinfos[4].maxsolutions = _nj4;
881 vinfos[5].jointtype = 1;
882 vinfos[5].foffset = j5;
883 vinfos[5].indices[0] = _ij5[0];
884 vinfos[5].indices[1] = _ij5[1];
885 vinfos[5].maxsolutions = _nj5;
886 std::vector<int> vfree(0);
899 IkReal j1array[1], cj1array[1], sj1array[1];
900 bool j1valid[1]={
false};
902 IkReal x176=((r20)*(sj5));
903 IkReal x177=((sj2)*(sj4));
904 IkReal x178=((cj5)*(r20));
905 IkReal x179=((cj5)*(r21));
906 IkReal x180=((cj4)*(r22));
907 IkReal x181=((r21)*(sj5));
908 IkReal x182=((cj2)*(sj4));
909 if(
IKabs(((gconst76)*(((((IkReal(-1.00000000000000))*(x177)*(x181)))+(((x177)*(x178)))+(((IkReal(-1.00000000000000))*(sj2)*(x180)))+(((cj2)*(x179)))+(((cj2)*(x176))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst76)*(((((IkReal(-1.00000000000000))*(x178)*(x182)))+(((sj2)*(x179)))+(((cj2)*(x180)))+(((x181)*(x182)))+(((sj2)*(x176))))))) <
IKFAST_ATAN2_MAGTHRESH )
911 j1array[0]=
IKatan2(((gconst76)*(((((IkReal(-1.00000000000000))*(x177)*(x181)))+(((x177)*(x178)))+(((IkReal(-1.00000000000000))*(sj2)*(x180)))+(((cj2)*(x179)))+(((cj2)*(x176)))))), ((gconst76)*(((((IkReal(-1.00000000000000))*(x178)*(x182)))+(((sj2)*(x179)))+(((cj2)*(x180)))+(((x181)*(x182)))+(((sj2)*(x176)))))));
912 sj1array[0]=
IKsin(j1array[0]);
913 cj1array[0]=
IKcos(j1array[0]);
914 if( j1array[0] >
IKPI )
918 else if( j1array[0] < -
IKPI )
922 for(
int ij1 = 0; ij1 < 1; ++ij1)
928 _ij1[0] = ij1; _ij1[1] = -1;
929 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
933 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
936 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
939 IkReal x183=
IKsin(j1);
940 IkReal x184=
IKcos(j1);
941 IkReal x185=((cj0)*(sj4));
942 IkReal x186=((IkReal(1.00000000000000))*(sj5));
943 IkReal x187=((cj5)*(sj0));
944 IkReal x188=((IkReal(1.00000000000000))*(cj4));
945 IkReal x189=((IkReal(1.00000000000000))*(cj5));
946 IkReal x190=((sj2)*(x183));
947 IkReal x191=((IkReal(1.00000000000000))*(x184));
948 IkReal x192=((cj2)*(x183));
949 IkReal x193=((cj2)*(x191));
950 evalcond[0]=((((IkReal(-1.00000000000000))*(x192)))+(((IkReal(-1.00000000000000))*(sj2)*(x191)))+(((r20)*(sj5)))+(((cj5)*(r21))));
951 evalcond[1]=((((IkReal(-1.00000000000000))*(x193)))+(x190)+(((cj4)*(r22)))+(((r21)*(sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x189))));
952 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj0)*(x186)))+(((IkReal(-1.00000000000000))*(x193)))+(((IkReal(-1.00000000000000))*(cj0)*(r01)*(x189)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x186)))+(x190)+(((IkReal(-1.00000000000000))*(r11)*(x187))));
953 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj0)*(x188)))+(((sj2)*(x184)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x188)))+(x192)+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x186)))+(((cj5)*(r00)*(x185)))+(((IkReal(-1.00000000000000))*(r01)*(x185)*(x186)))+(((r10)*(sj4)*(x187))));
954 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
961 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
962 vinfos[0].jointtype = 1;
963 vinfos[0].foffset = j0;
964 vinfos[0].indices[0] = _ij0[0];
965 vinfos[0].indices[1] = _ij0[1];
966 vinfos[0].maxsolutions = _nj0;
967 vinfos[1].jointtype = 1;
968 vinfos[1].foffset = j1;
969 vinfos[1].indices[0] = _ij1[0];
970 vinfos[1].indices[1] = _ij1[1];
971 vinfos[1].maxsolutions = _nj1;
972 vinfos[2].jointtype = 1;
973 vinfos[2].foffset = j2;
974 vinfos[2].indices[0] = _ij2[0];
975 vinfos[2].indices[1] = _ij2[1];
976 vinfos[2].maxsolutions = _nj2;
977 vinfos[3].jointtype = 1;
978 vinfos[3].foffset = j3;
979 vinfos[3].indices[0] = _ij3[0];
980 vinfos[3].indices[1] = _ij3[1];
981 vinfos[3].maxsolutions = _nj3;
982 vinfos[4].jointtype = 1;
983 vinfos[4].foffset = j4;
984 vinfos[4].indices[0] = _ij4[0];
985 vinfos[4].indices[1] = _ij4[1];
986 vinfos[4].maxsolutions = _nj4;
987 vinfos[5].jointtype = 1;
988 vinfos[5].foffset = j5;
989 vinfos[5].indices[0] = _ij5[0];
990 vinfos[5].indices[1] = _ij5[1];
991 vinfos[5].maxsolutions = _nj5;
992 std::vector<int> vfree(0);
1004 IkReal x194=((sj0)*(sj4));
1005 IkReal x195=((IkReal(1.00000000000000))*(r12));
1006 IkReal x196=((cj4)*(cj5));
1007 IkReal x197=((IkReal(1.00000000000000))*(cj0));
1008 IkReal x198=((cj4)*(sj0));
1009 IkReal x199=((IkReal(1.00000000000000))*(sj5));
1010 IkReal x200=((cj0)*(cj4));
1011 IkReal x201=((r00)*(sj0));
1012 IkReal x202=((sj4)*(sj5));
1013 IkReal x203=((IkReal(1.00000000000000))*(cj5));
1014 IkReal x204=((r01)*(sj5));
1015 IkReal x205=((cj0)*(sj4));
1016 IkReal x206=((r11)*(sj5));
1017 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(4.71238898038469))+(j3)), IkReal(6.28318530717959))));
1018 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(0.350000000000000))*(sj2)))+(((npx)*(sj5))));
1019 evalcond[2]=((IkReal(-0.350000000000000))+(((npy)*(x202)))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x203))));
1020 evalcond[3]=((((npx)*(x196)))+(((IkReal(-1.00000000000000))*(cj4)*(npy)*(x199)))+(((npz)*(sj4))));
1021 evalcond[4]=((((sj5)*(x201)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x197)))+(((cj5)*(r01)*(sj0)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x197))));
1022 evalcond[5]=((((r22)*(sj4)))+(((r20)*(x196)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x199))));
1023 evalcond[6]=((((x194)*(x204)))+(((IkReal(-1.00000000000000))*(x195)*(x200)))+(((cj5)*(r10)*(x205)))+(((IkReal(-1.00000000000000))*(r11)*(x197)*(x202)))+(((IkReal(-1.00000000000000))*(r00)*(x194)*(x203)))+(((r02)*(x198))));
1024 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r01)*(x198)*(x199)))+(((IkReal(-1.00000000000000))*(x195)*(x205)))+(((r02)*(x194)))+(((x196)*(x201)))+(((x200)*(x206)))+(((IkReal(-1.00000000000000))*(r10)*(x196)*(x197))));
1025 evalcond[8]=((((IkReal(-1.00000000000000))*(r00)*(x196)*(x197)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x197)))+(((IkReal(-1.00000000000000))*(x194)*(x195)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x196)))+(((x198)*(x206)))+(((x200)*(x204))));
1026 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
1029 IkReal dummyeval[1];
1031 gconst78=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
1032 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
1033 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1036 IkReal dummyeval[1];
1038 gconst79=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
1039 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
1040 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1047 IkReal j1array[1], cj1array[1], sj1array[1];
1048 bool j1valid[1]={
false};
1050 IkReal x207=((IkReal(1.00000000000000))*(sj2));
1051 IkReal x208=((cj2)*(sj5));
1052 IkReal x209=((cj0)*(r00));
1053 IkReal x210=((cj5)*(r21));
1054 IkReal x211=((cj5)*(r11)*(sj0));
1055 IkReal x212=((r10)*(sj0)*(sj5));
1056 IkReal x213=((cj0)*(cj5)*(r01));
1057 if(
IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(sj5)*(x207)*(x209)))+(((IkReal(-1.00000000000000))*(x207)*(x212)))+(((IkReal(-1.00000000000000))*(x207)*(x211)))+(((IkReal(-1.00000000000000))*(cj2)*(x210)))+(((IkReal(-1.00000000000000))*(x207)*(x213)))+(((IkReal(-1.00000000000000))*(r20)*(x208))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst79)*(((((r10)*(sj0)*(x208)))+(((x208)*(x209)))+(((IkReal(-1.00000000000000))*(x207)*(x210)))+(((cj2)*(x213)))+(((cj2)*(x211)))+(((IkReal(-1.00000000000000))*(r20)*(sj5)*(x207))))))) <
IKFAST_ATAN2_MAGTHRESH )
1059 j1array[0]=
IKatan2(((gconst79)*(((((IkReal(-1.00000000000000))*(sj5)*(x207)*(x209)))+(((IkReal(-1.00000000000000))*(x207)*(x212)))+(((IkReal(-1.00000000000000))*(x207)*(x211)))+(((IkReal(-1.00000000000000))*(cj2)*(x210)))+(((IkReal(-1.00000000000000))*(x207)*(x213)))+(((IkReal(-1.00000000000000))*(r20)*(x208)))))), ((gconst79)*(((((r10)*(sj0)*(x208)))+(((x208)*(x209)))+(((IkReal(-1.00000000000000))*(x207)*(x210)))+(((cj2)*(x213)))+(((cj2)*(x211)))+(((IkReal(-1.00000000000000))*(r20)*(sj5)*(x207)))))));
1060 sj1array[0]=
IKsin(j1array[0]);
1061 cj1array[0]=
IKcos(j1array[0]);
1062 if( j1array[0] >
IKPI )
1066 else if( j1array[0] < -
IKPI )
1067 { j1array[0]+=
IK2PI;
1070 for(
int ij1 = 0; ij1 < 1; ++ij1)
1076 _ij1[0] = ij1; _ij1[1] = -1;
1077 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1081 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1084 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1087 IkReal x214=
IKsin(j1);
1088 IkReal x215=
IKcos(j1);
1089 IkReal x216=((cj5)*(sj0));
1090 IkReal x217=((IkReal(1.00000000000000))*(r11));
1091 IkReal x218=((cj5)*(sj4));
1092 IkReal x219=((cj0)*(r00));
1093 IkReal x220=((IkReal(1.00000000000000))*(sj0));
1094 IkReal x221=((sj4)*(sj5));
1095 IkReal x222=((IkReal(1.00000000000000))*(cj0));
1096 IkReal x223=((sj2)*(x215));
1097 IkReal x224=((cj2)*(x214));
1098 IkReal x225=((cj2)*(x215));
1099 IkReal x226=((sj2)*(x214));
1100 IkReal x227=((x223)+(x224));
1101 evalcond[0]=((((r20)*(sj5)))+(x227)+(((cj5)*(r21))));
1102 evalcond[1]=((((r21)*(x221)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x225)))+(x226)+(((IkReal(-1.00000000000000))*(r20)*(x218))));
1103 evalcond[2]=((((IkReal(-1.00000000000000))*(sj5)*(x219)))+(((IkReal(-1.00000000000000))*(x216)*(x217)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x220)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x222)))+(x225)+(((IkReal(-1.00000000000000))*(x226))));
1104 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x221)*(x222)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x222)))+(((x218)*(x219)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x220)))+(((IkReal(-1.00000000000000))*(sj0)*(x217)*(x221)))+(x227)+(((r10)*(sj4)*(x216))));
1105 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1112 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1113 vinfos[0].jointtype = 1;
1114 vinfos[0].foffset = j0;
1115 vinfos[0].indices[0] = _ij0[0];
1116 vinfos[0].indices[1] = _ij0[1];
1117 vinfos[0].maxsolutions = _nj0;
1118 vinfos[1].jointtype = 1;
1119 vinfos[1].foffset = j1;
1120 vinfos[1].indices[0] = _ij1[0];
1121 vinfos[1].indices[1] = _ij1[1];
1122 vinfos[1].maxsolutions = _nj1;
1123 vinfos[2].jointtype = 1;
1124 vinfos[2].foffset = j2;
1125 vinfos[2].indices[0] = _ij2[0];
1126 vinfos[2].indices[1] = _ij2[1];
1127 vinfos[2].maxsolutions = _nj2;
1128 vinfos[3].jointtype = 1;
1129 vinfos[3].foffset = j3;
1130 vinfos[3].indices[0] = _ij3[0];
1131 vinfos[3].indices[1] = _ij3[1];
1132 vinfos[3].maxsolutions = _nj3;
1133 vinfos[4].jointtype = 1;
1134 vinfos[4].foffset = j4;
1135 vinfos[4].indices[0] = _ij4[0];
1136 vinfos[4].indices[1] = _ij4[1];
1137 vinfos[4].maxsolutions = _nj4;
1138 vinfos[5].jointtype = 1;
1139 vinfos[5].foffset = j5;
1140 vinfos[5].indices[0] = _ij5[0];
1141 vinfos[5].indices[1] = _ij5[1];
1142 vinfos[5].maxsolutions = _nj5;
1143 std::vector<int> vfree(0);
1156 IkReal j1array[1], cj1array[1], sj1array[1];
1157 bool j1valid[1]={
false};
1159 IkReal x228=((cj4)*(r22));
1160 IkReal x229=((cj2)*(r20));
1161 IkReal x230=((cj5)*(r21));
1162 IkReal x231=((sj2)*(sj5));
1163 IkReal x232=((r21)*(sj4));
1164 IkReal x233=((IkReal(1.00000000000000))*(cj2));
1165 IkReal x234=((cj5)*(sj4));
1166 if(
IKabs(((gconst78)*(((((sj2)*(x228)))+(((x231)*(x232)))+(((cj2)*(x230)))+(((IkReal(-1.00000000000000))*(r20)*(sj2)*(x234)))+(((sj5)*(x229))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst78)*(((((r20)*(x231)))+(((IkReal(-1.00000000000000))*(x228)*(x233)))+(((sj2)*(x230)))+(((x229)*(x234)))+(((IkReal(-1.00000000000000))*(sj5)*(x232)*(x233))))))) <
IKFAST_ATAN2_MAGTHRESH )
1168 j1array[0]=
IKatan2(((gconst78)*(((((sj2)*(x228)))+(((x231)*(x232)))+(((cj2)*(x230)))+(((IkReal(-1.00000000000000))*(r20)*(sj2)*(x234)))+(((sj5)*(x229)))))), ((gconst78)*(((((r20)*(x231)))+(((IkReal(-1.00000000000000))*(x228)*(x233)))+(((sj2)*(x230)))+(((x229)*(x234)))+(((IkReal(-1.00000000000000))*(sj5)*(x232)*(x233)))))));
1169 sj1array[0]=
IKsin(j1array[0]);
1170 cj1array[0]=
IKcos(j1array[0]);
1171 if( j1array[0] >
IKPI )
1175 else if( j1array[0] < -
IKPI )
1176 { j1array[0]+=
IK2PI;
1179 for(
int ij1 = 0; ij1 < 1; ++ij1)
1185 _ij1[0] = ij1; _ij1[1] = -1;
1186 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1190 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1193 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1196 IkReal x235=
IKsin(j1);
1197 IkReal x236=
IKcos(j1);
1198 IkReal x237=((cj5)*(sj0));
1199 IkReal x238=((IkReal(1.00000000000000))*(r11));
1200 IkReal x239=((cj5)*(sj4));
1201 IkReal x240=((cj0)*(r00));
1202 IkReal x241=((IkReal(1.00000000000000))*(sj0));
1203 IkReal x242=((sj4)*(sj5));
1204 IkReal x243=((IkReal(1.00000000000000))*(cj0));
1205 IkReal x244=((sj2)*(x236));
1206 IkReal x245=((cj2)*(x235));
1207 IkReal x246=((cj2)*(x236));
1208 IkReal x247=((sj2)*(x235));
1209 IkReal x248=((x245)+(x244));
1210 evalcond[0]=((((r20)*(sj5)))+(x248)+(((cj5)*(r21))));
1211 evalcond[1]=((((IkReal(-1.00000000000000))*(x246)))+(((cj4)*(r22)))+(((r21)*(x242)))+(x247)+(((IkReal(-1.00000000000000))*(r20)*(x239))));
1212 evalcond[2]=((((IkReal(-1.00000000000000))*(x247)))+(((IkReal(-1.00000000000000))*(sj5)*(x240)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x241)))+(x246)+(((IkReal(-1.00000000000000))*(x237)*(x238)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x243))));
1213 evalcond[3]=((((x239)*(x240)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x241)))+(((IkReal(-1.00000000000000))*(r01)*(x242)*(x243)))+(((r10)*(sj4)*(x237)))+(x248)+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x243)))+(((IkReal(-1.00000000000000))*(sj0)*(x238)*(x242))));
1214 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1221 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1222 vinfos[0].jointtype = 1;
1223 vinfos[0].foffset = j0;
1224 vinfos[0].indices[0] = _ij0[0];
1225 vinfos[0].indices[1] = _ij0[1];
1226 vinfos[0].maxsolutions = _nj0;
1227 vinfos[1].jointtype = 1;
1228 vinfos[1].foffset = j1;
1229 vinfos[1].indices[0] = _ij1[0];
1230 vinfos[1].indices[1] = _ij1[1];
1231 vinfos[1].maxsolutions = _nj1;
1232 vinfos[2].jointtype = 1;
1233 vinfos[2].foffset = j2;
1234 vinfos[2].indices[0] = _ij2[0];
1235 vinfos[2].indices[1] = _ij2[1];
1236 vinfos[2].maxsolutions = _nj2;
1237 vinfos[3].jointtype = 1;
1238 vinfos[3].foffset = j3;
1239 vinfos[3].indices[0] = _ij3[0];
1240 vinfos[3].indices[1] = _ij3[1];
1241 vinfos[3].maxsolutions = _nj3;
1242 vinfos[4].jointtype = 1;
1243 vinfos[4].foffset = j4;
1244 vinfos[4].indices[0] = _ij4[0];
1245 vinfos[4].indices[1] = _ij4[1];
1246 vinfos[4].maxsolutions = _nj4;
1247 vinfos[5].jointtype = 1;
1248 vinfos[5].foffset = j5;
1249 vinfos[5].indices[0] = _ij5[0];
1250 vinfos[5].indices[1] = _ij5[1];
1251 vinfos[5].maxsolutions = _nj5;
1252 std::vector<int> vfree(0);
1264 IkReal x249=((IkReal(1.00000000000000))*(r10));
1265 IkReal x250=((sj0)*(sj5));
1266 IkReal x251=((r02)*(sj0));
1267 IkReal x252=((cj4)*(cj5));
1268 IkReal x253=((IkReal(1.00000000000000))*(cj4));
1269 IkReal x254=((cj0)*(r12));
1270 IkReal x255=((IkReal(1.00000000000000))*(cj0));
1271 IkReal x256=((cj5)*(r11));
1272 IkReal x257=((cj5)*(r01));
1273 IkReal x258=((r00)*(sj0));
1274 IkReal x259=((npy)*(sj5));
1275 IkReal x260=((IkReal(1.00000000000000))*(sj4));
1276 IkReal x261=((cj0)*(sj5));
1277 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(3.14159265358979))+(j3)), IkReal(6.28318530717959))));
1278 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
1279 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(cj5)*(npx)*(x260)))+(((cj4)*(npz)))+(((sj4)*(x259))));
1280 evalcond[3]=((((npx)*(x252)))+(((IkReal(-1.00000000000000))*(x253)*(x259)))+(((IkReal(0.350000000000000))*(sj2)))+(((npz)*(sj4))));
1281 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
1282 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x255)*(x256)))+(((sj0)*(x257)))+(((IkReal(-1.00000000000000))*(x249)*(x261)))+(((r00)*(x250))));
1283 evalcond[6]=((((IkReal(-1.00000000000000))*(x255)*(x257)))+(((IkReal(-1.00000000000000))*(sj0)*(x256)))+(((IkReal(-1.00000000000000))*(x249)*(x250)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x255))));
1284 evalcond[7]=((((IkReal(-1.00000000000000))*(r11)*(sj4)*(sj5)*(x255)))+(((cj4)*(x251)))+(((r01)*(sj4)*(x250)))+(((IkReal(-1.00000000000000))*(x253)*(x254)))+(((cj0)*(cj5)*(r10)*(sj4)))+(((IkReal(-1.00000000000000))*(cj5)*(x258)*(x260))));
1285 evalcond[8]=((((IkReal(-1.00000000000000))*(cj0)*(x249)*(x252)))+(((IkReal(-1.00000000000000))*(x254)*(x260)))+(((x252)*(x258)))+(((sj4)*(x251)))+(((cj4)*(r11)*(x261)))+(((IkReal(-1.00000000000000))*(r01)*(x250)*(x253))));
1286 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
1289 IkReal dummyeval[1];
1291 gconst80=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
1292 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
1293 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1296 IkReal dummyeval[1];
1298 gconst81=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
1299 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
1300 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1307 IkReal j1array[1], cj1array[1], sj1array[1];
1308 bool j1valid[1]={
false};
1310 IkReal x262=((cj4)*(r22));
1311 IkReal x263=((IkReal(1.00000000000000))*(sj0));
1312 IkReal x264=((IkReal(1.00000000000000))*(cj2));
1313 IkReal x265=((cj4)*(r12));
1314 IkReal x266=((cj5)*(sj4));
1315 IkReal x267=((IkReal(1.00000000000000))*(sj2));
1316 IkReal x268=((r10)*(sj0));
1317 IkReal x269=((cj0)*(r00));
1318 IkReal x270=((cj0)*(r01));
1319 IkReal x271=((sj4)*(sj5));
1320 IkReal x272=((sj2)*(x271));
1321 IkReal x273=((cj0)*(cj4)*(r02));
1322 if(
IKabs(((gconst81)*(((((IkReal(-1.00000000000000))*(cj2)*(r11)*(x263)*(x271)))+(((IkReal(-1.00000000000000))*(x264)*(x270)*(x271)))+(((cj2)*(x266)*(x269)))+(((cj2)*(x266)*(x268)))+(((IkReal(-1.00000000000000))*(cj2)*(x263)*(x265)))+(((IkReal(-1.00000000000000))*(r20)*(x266)*(x267)))+(((IkReal(-1.00000000000000))*(x264)*(x273)))+(((r21)*(x272)))+(((sj2)*(x262))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst81)*(((((IkReal(-1.00000000000000))*(r11)*(x263)*(x272)))+(((IkReal(-1.00000000000000))*(sj2)*(x263)*(x265)))+(((sj2)*(x266)*(x269)))+(((cj2)*(r20)*(x266)))+(((sj2)*(x266)*(x268)))+(((IkReal(-1.00000000000000))*(x267)*(x270)*(x271)))+(((IkReal(-1.00000000000000))*(r21)*(x264)*(x271)))+(((IkReal(-1.00000000000000))*(x262)*(x264)))+(((IkReal(-1.00000000000000))*(x267)*(x273))))))) <
IKFAST_ATAN2_MAGTHRESH )
1324 j1array[0]=
IKatan2(((gconst81)*(((((IkReal(-1.00000000000000))*(cj2)*(r11)*(x263)*(x271)))+(((IkReal(-1.00000000000000))*(x264)*(x270)*(x271)))+(((cj2)*(x266)*(x269)))+(((cj2)*(x266)*(x268)))+(((IkReal(-1.00000000000000))*(cj2)*(x263)*(x265)))+(((IkReal(-1.00000000000000))*(r20)*(x266)*(x267)))+(((IkReal(-1.00000000000000))*(x264)*(x273)))+(((r21)*(x272)))+(((sj2)*(x262)))))), ((gconst81)*(((((IkReal(-1.00000000000000))*(r11)*(x263)*(x272)))+(((IkReal(-1.00000000000000))*(sj2)*(x263)*(x265)))+(((sj2)*(x266)*(x269)))+(((cj2)*(r20)*(x266)))+(((sj2)*(x266)*(x268)))+(((IkReal(-1.00000000000000))*(x267)*(x270)*(x271)))+(((IkReal(-1.00000000000000))*(r21)*(x264)*(x271)))+(((IkReal(-1.00000000000000))*(x262)*(x264)))+(((IkReal(-1.00000000000000))*(x267)*(x273)))))));
1325 sj1array[0]=
IKsin(j1array[0]);
1326 cj1array[0]=
IKcos(j1array[0]);
1327 if( j1array[0] >
IKPI )
1331 else if( j1array[0] < -
IKPI )
1332 { j1array[0]+=
IK2PI;
1335 for(
int ij1 = 0; ij1 < 1; ++ij1)
1341 _ij1[0] = ij1; _ij1[1] = -1;
1342 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1346 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1349 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1352 IkReal x274=
IKcos(j1);
1353 IkReal x275=
IKsin(j1);
1354 IkReal x276=((IkReal(1.00000000000000))*(sj4));
1355 IkReal x277=((cj5)*(r00));
1356 IkReal x278=((cj5)*(r20));
1357 IkReal x279=((r12)*(sj0));
1358 IkReal x280=((r21)*(sj5));
1359 IkReal x281=((IkReal(1.00000000000000))*(cj4));
1360 IkReal x282=((sj2)*(x274));
1361 IkReal x283=((cj2)*(x275));
1362 IkReal x284=((cj2)*(x274));
1363 IkReal x285=((r11)*(sj0)*(sj5));
1364 IkReal x286=((cj0)*(x281));
1365 IkReal x287=((cj0)*(r01)*(sj5));
1366 IkReal x288=((cj5)*(r10)*(sj0));
1367 IkReal x289=((sj2)*(x275));
1368 IkReal x290=((x283)+(x282));
1369 evalcond[0]=((((sj4)*(x280)))+(x289)+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x284)))+(((IkReal(-1.00000000000000))*(x276)*(x278))));
1370 evalcond[1]=((((IkReal(-1.00000000000000))*(x280)*(x281)))+(((cj4)*(x278)))+(((r22)*(sj4)))+(x290));
1371 evalcond[2]=((((IkReal(-1.00000000000000))*(x279)*(x281)))+(((cj0)*(sj4)*(x277)))+(x290)+(((IkReal(-1.00000000000000))*(x276)*(x285)))+(((sj4)*(x288)))+(((IkReal(-1.00000000000000))*(x276)*(x287)))+(((IkReal(-1.00000000000000))*(r02)*(x286))));
1372 evalcond[3]=((((IkReal(-1.00000000000000))*(x281)*(x288)))+(x284)+(((IkReal(-1.00000000000000))*(x276)*(x279)))+(((cj4)*(x285)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x276)))+(((cj4)*(x287)))+(((IkReal(-1.00000000000000))*(x289)))+(((IkReal(-1.00000000000000))*(x277)*(x286))));
1373 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1380 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1381 vinfos[0].jointtype = 1;
1382 vinfos[0].foffset = j0;
1383 vinfos[0].indices[0] = _ij0[0];
1384 vinfos[0].indices[1] = _ij0[1];
1385 vinfos[0].maxsolutions = _nj0;
1386 vinfos[1].jointtype = 1;
1387 vinfos[1].foffset = j1;
1388 vinfos[1].indices[0] = _ij1[0];
1389 vinfos[1].indices[1] = _ij1[1];
1390 vinfos[1].maxsolutions = _nj1;
1391 vinfos[2].jointtype = 1;
1392 vinfos[2].foffset = j2;
1393 vinfos[2].indices[0] = _ij2[0];
1394 vinfos[2].indices[1] = _ij2[1];
1395 vinfos[2].maxsolutions = _nj2;
1396 vinfos[3].jointtype = 1;
1397 vinfos[3].foffset = j3;
1398 vinfos[3].indices[0] = _ij3[0];
1399 vinfos[3].indices[1] = _ij3[1];
1400 vinfos[3].maxsolutions = _nj3;
1401 vinfos[4].jointtype = 1;
1402 vinfos[4].foffset = j4;
1403 vinfos[4].indices[0] = _ij4[0];
1404 vinfos[4].indices[1] = _ij4[1];
1405 vinfos[4].maxsolutions = _nj4;
1406 vinfos[5].jointtype = 1;
1407 vinfos[5].foffset = j5;
1408 vinfos[5].indices[0] = _ij5[0];
1409 vinfos[5].indices[1] = _ij5[1];
1410 vinfos[5].maxsolutions = _nj5;
1411 std::vector<int> vfree(0);
1424 IkReal j1array[1], cj1array[1], sj1array[1];
1425 bool j1valid[1]={
false};
1427 IkReal x291=((cj4)*(r22));
1428 IkReal x292=((r21)*(sj5));
1429 IkReal x293=((cj2)*(cj4));
1430 IkReal x294=((cj2)*(sj4));
1431 IkReal x295=((sj2)*(sj4));
1432 IkReal x296=((cj5)*(r20));
1433 IkReal x297=((cj4)*(sj2));
1434 if(
IKabs(((gconst80)*(((((IkReal(-1.00000000000000))*(x295)*(x296)))+(((x292)*(x295)))+(((r22)*(x294)))+(((sj2)*(x291)))+(((IkReal(-1.00000000000000))*(x292)*(x293)))+(((x293)*(x296))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst80)*(((((x294)*(x296)))+(((x296)*(x297)))+(((IkReal(-1.00000000000000))*(cj2)*(x291)))+(((IkReal(-1.00000000000000))*(x292)*(x297)))+(((IkReal(-1.00000000000000))*(x292)*(x294)))+(((r22)*(x295))))))) <
IKFAST_ATAN2_MAGTHRESH )
1436 j1array[0]=
IKatan2(((gconst80)*(((((IkReal(-1.00000000000000))*(x295)*(x296)))+(((x292)*(x295)))+(((r22)*(x294)))+(((sj2)*(x291)))+(((IkReal(-1.00000000000000))*(x292)*(x293)))+(((x293)*(x296)))))), ((gconst80)*(((((x294)*(x296)))+(((x296)*(x297)))+(((IkReal(-1.00000000000000))*(cj2)*(x291)))+(((IkReal(-1.00000000000000))*(x292)*(x297)))+(((IkReal(-1.00000000000000))*(x292)*(x294)))+(((r22)*(x295)))))));
1437 sj1array[0]=
IKsin(j1array[0]);
1438 cj1array[0]=
IKcos(j1array[0]);
1439 if( j1array[0] >
IKPI )
1443 else if( j1array[0] < -
IKPI )
1444 { j1array[0]+=
IK2PI;
1447 for(
int ij1 = 0; ij1 < 1; ++ij1)
1453 _ij1[0] = ij1; _ij1[1] = -1;
1454 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1458 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1461 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1464 IkReal x298=
IKcos(j1);
1465 IkReal x299=
IKsin(j1);
1466 IkReal x300=((IkReal(1.00000000000000))*(sj4));
1467 IkReal x301=((cj5)*(r00));
1468 IkReal x302=((cj5)*(r20));
1469 IkReal x303=((r12)*(sj0));
1470 IkReal x304=((r21)*(sj5));
1471 IkReal x305=((IkReal(1.00000000000000))*(cj4));
1472 IkReal x306=((sj2)*(x298));
1473 IkReal x307=((cj2)*(x299));
1474 IkReal x308=((cj2)*(x298));
1475 IkReal x309=((r11)*(sj0)*(sj5));
1476 IkReal x310=((cj0)*(x305));
1477 IkReal x311=((cj0)*(r01)*(sj5));
1478 IkReal x312=((cj5)*(r10)*(sj0));
1479 IkReal x313=((sj2)*(x299));
1480 IkReal x314=((x306)+(x307));
1481 evalcond[0]=((((cj4)*(r22)))+(x313)+(((IkReal(-1.00000000000000))*(x308)))+(((IkReal(-1.00000000000000))*(x300)*(x302)))+(((sj4)*(x304))));
1482 evalcond[1]=((((r22)*(sj4)))+(((cj4)*(x302)))+(x314)+(((IkReal(-1.00000000000000))*(x304)*(x305))));
1483 evalcond[2]=((((sj4)*(x312)))+(((IkReal(-1.00000000000000))*(x303)*(x305)))+(((IkReal(-1.00000000000000))*(x300)*(x309)))+(((cj0)*(sj4)*(x301)))+(x314)+(((IkReal(-1.00000000000000))*(r02)*(x310)))+(((IkReal(-1.00000000000000))*(x300)*(x311))));
1484 evalcond[3]=((((cj4)*(x309)))+(((IkReal(-1.00000000000000))*(x301)*(x310)))+(((IkReal(-1.00000000000000))*(x300)*(x303)))+(x308)+(((cj4)*(x311)))+(((IkReal(-1.00000000000000))*(x305)*(x312)))+(((IkReal(-1.00000000000000))*(x313)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x300))));
1485 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1492 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1493 vinfos[0].jointtype = 1;
1494 vinfos[0].foffset = j0;
1495 vinfos[0].indices[0] = _ij0[0];
1496 vinfos[0].indices[1] = _ij0[1];
1497 vinfos[0].maxsolutions = _nj0;
1498 vinfos[1].jointtype = 1;
1499 vinfos[1].foffset = j1;
1500 vinfos[1].indices[0] = _ij1[0];
1501 vinfos[1].indices[1] = _ij1[1];
1502 vinfos[1].maxsolutions = _nj1;
1503 vinfos[2].jointtype = 1;
1504 vinfos[2].foffset = j2;
1505 vinfos[2].indices[0] = _ij2[0];
1506 vinfos[2].indices[1] = _ij2[1];
1507 vinfos[2].maxsolutions = _nj2;
1508 vinfos[3].jointtype = 1;
1509 vinfos[3].foffset = j3;
1510 vinfos[3].indices[0] = _ij3[0];
1511 vinfos[3].indices[1] = _ij3[1];
1512 vinfos[3].maxsolutions = _nj3;
1513 vinfos[4].jointtype = 1;
1514 vinfos[4].foffset = j4;
1515 vinfos[4].indices[0] = _ij4[0];
1516 vinfos[4].indices[1] = _ij4[1];
1517 vinfos[4].maxsolutions = _nj4;
1518 vinfos[5].jointtype = 1;
1519 vinfos[5].foffset = j5;
1520 vinfos[5].indices[0] = _ij5[0];
1521 vinfos[5].indices[1] = _ij5[1];
1522 vinfos[5].maxsolutions = _nj5;
1523 std::vector<int> vfree(0);
1535 IkReal x315=((IkReal(1.00000000000000))*(r10));
1536 IkReal x316=((sj0)*(sj5));
1537 IkReal x317=((r02)*(sj0));
1538 IkReal x318=((cj4)*(cj5));
1539 IkReal x319=((IkReal(1.00000000000000))*(cj4));
1540 IkReal x320=((cj0)*(r12));
1541 IkReal x321=((IkReal(1.00000000000000))*(cj0));
1542 IkReal x322=((cj5)*(r11));
1543 IkReal x323=((cj5)*(r01));
1544 IkReal x324=((r00)*(sj0));
1545 IkReal x325=((npy)*(sj5));
1546 IkReal x326=((IkReal(1.00000000000000))*(sj4));
1547 IkReal x327=((cj0)*(sj5));
1548 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(j3, IkReal(6.28318530717959))));
1549 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
1550 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((sj4)*(x325)))+(((IkReal(-1.00000000000000))*(cj5)*(npx)*(x326))));
1551 evalcond[3]=((((npx)*(x318)))+(((IkReal(-1.00000000000000))*(x319)*(x325)))+(((IkReal(-0.350000000000000))*(sj2)))+(((npz)*(sj4))));
1552 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
1553 evalcond[5]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x321)*(x322)))+(((sj0)*(x323)))+(((IkReal(-1.00000000000000))*(x315)*(x327)))+(((r00)*(x316))));
1554 evalcond[6]=((((IkReal(-1.00000000000000))*(sj0)*(x322)))+(((IkReal(-1.00000000000000))*(x321)*(x323)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x321)))+(((IkReal(-1.00000000000000))*(x315)*(x316))));
1555 evalcond[7]=((((IkReal(-1.00000000000000))*(x319)*(x320)))+(((cj4)*(x317)))+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(sj5)*(x321)))+(((IkReal(-1.00000000000000))*(cj5)*(x324)*(x326)))+(((cj0)*(cj5)*(r10)*(sj4)))+(((r01)*(sj4)*(x316))));
1556 evalcond[8]=((((IkReal(-1.00000000000000))*(x320)*(x326)))+(((x318)*(x324)))+(((IkReal(-1.00000000000000))*(r01)*(x316)*(x319)))+(((sj4)*(x317)))+(((IkReal(-1.00000000000000))*(cj0)*(x315)*(x318)))+(((cj4)*(r11)*(x327))));
1557 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
1560 IkReal dummyeval[1];
1562 gconst82=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
1563 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
1564 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1567 IkReal dummyeval[1];
1569 gconst83=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
1570 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
1571 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
1578 IkReal j1array[1], cj1array[1], sj1array[1];
1579 bool j1valid[1]={
false};
1581 IkReal x328=((cj4)*(r22));
1582 IkReal x329=((r11)*(sj0));
1583 IkReal x330=((IkReal(1.00000000000000))*(cj2));
1584 IkReal x331=((sj2)*(sj4));
1585 IkReal x332=((IkReal(1.00000000000000))*(sj2));
1586 IkReal x333=((cj2)*(sj4));
1587 IkReal x334=((cj0)*(r01));
1588 IkReal x335=((cj5)*(r20));
1589 IkReal x336=((IkReal(1.00000000000000))*(sj4)*(sj5));
1590 IkReal x337=((cj0)*(cj4)*(r02));
1591 IkReal x338=((cj4)*(r12)*(sj0));
1592 IkReal x339=((cj5)*(r10)*(sj0));
1593 IkReal x340=((cj0)*(cj5)*(r00));
1594 if(
IKabs(((gconst83)*(((((x333)*(x339)))+(((x333)*(x340)))+(((IkReal(-1.00000000000000))*(x330)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x335)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x330)*(x334)))+(((sj2)*(x328)))+(((IkReal(-1.00000000000000))*(x330)*(x338)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x329)*(x330)))+(((r21)*(sj5)*(x331))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst83)*(((((x333)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x330)))+(((IkReal(-1.00000000000000))*(x332)*(x338)))+(((x331)*(x339)))+(((x331)*(x340)))+(((IkReal(-1.00000000000000))*(x332)*(x337)))+(((IkReal(-1.00000000000000))*(x328)*(x330)))+(((IkReal(-1.00000000000000))*(sj5)*(x329)*(x331)))+(((IkReal(-1.00000000000000))*(sj5)*(x331)*(x334))))))) <
IKFAST_ATAN2_MAGTHRESH )
1596 j1array[0]=
IKatan2(((gconst83)*(((((x333)*(x339)))+(((x333)*(x340)))+(((IkReal(-1.00000000000000))*(x330)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x335)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x330)*(x334)))+(((sj2)*(x328)))+(((IkReal(-1.00000000000000))*(x330)*(x338)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x329)*(x330)))+(((r21)*(sj5)*(x331)))))), ((gconst83)*(((((x333)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x330)))+(((IkReal(-1.00000000000000))*(x332)*(x338)))+(((x331)*(x339)))+(((x331)*(x340)))+(((IkReal(-1.00000000000000))*(x332)*(x337)))+(((IkReal(-1.00000000000000))*(x328)*(x330)))+(((IkReal(-1.00000000000000))*(sj5)*(x329)*(x331)))+(((IkReal(-1.00000000000000))*(sj5)*(x331)*(x334)))))));
1597 sj1array[0]=
IKsin(j1array[0]);
1598 cj1array[0]=
IKcos(j1array[0]);
1599 if( j1array[0] >
IKPI )
1603 else if( j1array[0] < -
IKPI )
1604 { j1array[0]+=
IK2PI;
1607 for(
int ij1 = 0; ij1 < 1; ++ij1)
1613 _ij1[0] = ij1; _ij1[1] = -1;
1614 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1618 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1621 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1624 IkReal x341=
IKcos(j1);
1625 IkReal x342=
IKsin(j1);
1626 IkReal x343=((IkReal(1.00000000000000))*(sj4));
1627 IkReal x344=((cj5)*(r00));
1628 IkReal x345=((cj5)*(r20));
1629 IkReal x346=((r12)*(sj0));
1630 IkReal x347=((r21)*(sj5));
1631 IkReal x348=((IkReal(1.00000000000000))*(cj4));
1632 IkReal x349=((sj2)*(x342));
1633 IkReal x350=((IkReal(1.00000000000000))*(x341));
1634 IkReal x351=((r11)*(sj0)*(sj5));
1635 IkReal x352=((cj0)*(x348));
1636 IkReal x353=((cj0)*(r01)*(sj5));
1637 IkReal x354=((cj2)*(x342));
1638 IkReal x355=((cj5)*(r10)*(sj0));
1639 IkReal x356=((cj2)*(x350));
1640 evalcond[0]=((((sj4)*(x347)))+(((IkReal(-1.00000000000000))*(x343)*(x345)))+(((cj4)*(r22)))+(x349)+(((IkReal(-1.00000000000000))*(x356))));
1641 evalcond[1]=((((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(x347)*(x348)))+(((IkReal(-1.00000000000000))*(sj2)*(x350)))+(((IkReal(-1.00000000000000))*(x354)))+(((cj4)*(x345))));
1642 evalcond[2]=((((IkReal(-1.00000000000000))*(x343)*(x351)))+(((sj2)*(x341)))+(((IkReal(-1.00000000000000))*(x346)*(x348)))+(((IkReal(-1.00000000000000))*(x343)*(x353)))+(((IkReal(-1.00000000000000))*(r02)*(x352)))+(x354)+(((sj4)*(x355)))+(((cj0)*(sj4)*(x344))));
1643 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(r02)*(x343)))+(((IkReal(-1.00000000000000))*(x348)*(x355)))+(((IkReal(-1.00000000000000))*(x344)*(x352)))+(((IkReal(-1.00000000000000))*(x343)*(x346)))+(x349)+(((cj4)*(x353)))+(((IkReal(-1.00000000000000))*(x356)))+(((cj4)*(x351))));
1644 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1651 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1652 vinfos[0].jointtype = 1;
1653 vinfos[0].foffset = j0;
1654 vinfos[0].indices[0] = _ij0[0];
1655 vinfos[0].indices[1] = _ij0[1];
1656 vinfos[0].maxsolutions = _nj0;
1657 vinfos[1].jointtype = 1;
1658 vinfos[1].foffset = j1;
1659 vinfos[1].indices[0] = _ij1[0];
1660 vinfos[1].indices[1] = _ij1[1];
1661 vinfos[1].maxsolutions = _nj1;
1662 vinfos[2].jointtype = 1;
1663 vinfos[2].foffset = j2;
1664 vinfos[2].indices[0] = _ij2[0];
1665 vinfos[2].indices[1] = _ij2[1];
1666 vinfos[2].maxsolutions = _nj2;
1667 vinfos[3].jointtype = 1;
1668 vinfos[3].foffset = j3;
1669 vinfos[3].indices[0] = _ij3[0];
1670 vinfos[3].indices[1] = _ij3[1];
1671 vinfos[3].maxsolutions = _nj3;
1672 vinfos[4].jointtype = 1;
1673 vinfos[4].foffset = j4;
1674 vinfos[4].indices[0] = _ij4[0];
1675 vinfos[4].indices[1] = _ij4[1];
1676 vinfos[4].maxsolutions = _nj4;
1677 vinfos[5].jointtype = 1;
1678 vinfos[5].foffset = j5;
1679 vinfos[5].indices[0] = _ij5[0];
1680 vinfos[5].indices[1] = _ij5[1];
1681 vinfos[5].maxsolutions = _nj5;
1682 std::vector<int> vfree(0);
1695 IkReal j1array[1], cj1array[1], sj1array[1];
1696 bool j1valid[1]={
false};
1698 IkReal x357=((IkReal(1.00000000000000))*(cj2));
1699 IkReal x358=((cj4)*(r22));
1700 IkReal x359=((r21)*(sj5));
1701 IkReal x360=((r22)*(sj4));
1702 IkReal x361=((cj5)*(r20)*(sj4));
1703 IkReal x362=((cj4)*(cj5)*(r20));
1704 IkReal x363=((IkReal(1.00000000000000))*(sj2)*(x359));
1705 if(
IKabs(((gconst82)*(((((cj2)*(x360)))+(((IkReal(-1.00000000000000))*(sj2)*(x358)))+(((IkReal(-1.00000000000000))*(cj4)*(x357)*(x359)))+(((sj2)*(x361)))+(((cj2)*(x362)))+(((IkReal(-1.00000000000000))*(sj4)*(x363))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst82)*(((((IkReal(-1.00000000000000))*(cj4)*(x363)))+(((cj2)*(sj4)*(x359)))+(((sj2)*(x360)))+(((sj2)*(x362)))+(((IkReal(-1.00000000000000))*(x357)*(x361)))+(((cj2)*(x358))))))) <
IKFAST_ATAN2_MAGTHRESH )
1707 j1array[0]=
IKatan2(((gconst82)*(((((cj2)*(x360)))+(((IkReal(-1.00000000000000))*(sj2)*(x358)))+(((IkReal(-1.00000000000000))*(cj4)*(x357)*(x359)))+(((sj2)*(x361)))+(((cj2)*(x362)))+(((IkReal(-1.00000000000000))*(sj4)*(x363)))))), ((gconst82)*(((((IkReal(-1.00000000000000))*(cj4)*(x363)))+(((cj2)*(sj4)*(x359)))+(((sj2)*(x360)))+(((sj2)*(x362)))+(((IkReal(-1.00000000000000))*(x357)*(x361)))+(((cj2)*(x358)))))));
1708 sj1array[0]=
IKsin(j1array[0]);
1709 cj1array[0]=
IKcos(j1array[0]);
1710 if( j1array[0] >
IKPI )
1714 else if( j1array[0] < -
IKPI )
1715 { j1array[0]+=
IK2PI;
1718 for(
int ij1 = 0; ij1 < 1; ++ij1)
1724 _ij1[0] = ij1; _ij1[1] = -1;
1725 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1729 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1732 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1735 IkReal x364=
IKcos(j1);
1736 IkReal x365=
IKsin(j1);
1737 IkReal x366=((IkReal(1.00000000000000))*(sj4));
1738 IkReal x367=((cj5)*(r00));
1739 IkReal x368=((cj5)*(r20));
1740 IkReal x369=((r12)*(sj0));
1741 IkReal x370=((r21)*(sj5));
1742 IkReal x371=((IkReal(1.00000000000000))*(cj4));
1743 IkReal x372=((sj2)*(x365));
1744 IkReal x373=((IkReal(1.00000000000000))*(x364));
1745 IkReal x374=((r11)*(sj0)*(sj5));
1746 IkReal x375=((cj0)*(x371));
1747 IkReal x376=((cj0)*(r01)*(sj5));
1748 IkReal x377=((cj2)*(x365));
1749 IkReal x378=((cj5)*(r10)*(sj0));
1750 IkReal x379=((cj2)*(x373));
1751 evalcond[0]=((((sj4)*(x370)))+(x372)+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x379)))+(((IkReal(-1.00000000000000))*(x366)*(x368))));
1752 evalcond[1]=((((IkReal(-1.00000000000000))*(sj2)*(x373)))+(((cj4)*(x368)))+(((IkReal(-1.00000000000000))*(x370)*(x371)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(x377))));
1753 evalcond[2]=((((sj2)*(x364)))+(((cj0)*(sj4)*(x367)))+(x377)+(((IkReal(-1.00000000000000))*(x366)*(x374)))+(((sj4)*(x378)))+(((IkReal(-1.00000000000000))*(x366)*(x376)))+(((IkReal(-1.00000000000000))*(x369)*(x371)))+(((IkReal(-1.00000000000000))*(r02)*(x375))));
1754 evalcond[3]=((((IkReal(-1.00000000000000))*(x371)*(x378)))+(x372)+(((cj4)*(x376)))+(((cj4)*(x374)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x366)))+(((IkReal(-1.00000000000000))*(x379)))+(((IkReal(-1.00000000000000))*(x367)*(x375)))+(((IkReal(-1.00000000000000))*(x366)*(x369))));
1755 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
1762 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1763 vinfos[0].jointtype = 1;
1764 vinfos[0].foffset = j0;
1765 vinfos[0].indices[0] = _ij0[0];
1766 vinfos[0].indices[1] = _ij0[1];
1767 vinfos[0].maxsolutions = _nj0;
1768 vinfos[1].jointtype = 1;
1769 vinfos[1].foffset = j1;
1770 vinfos[1].indices[0] = _ij1[0];
1771 vinfos[1].indices[1] = _ij1[1];
1772 vinfos[1].maxsolutions = _nj1;
1773 vinfos[2].jointtype = 1;
1774 vinfos[2].foffset = j2;
1775 vinfos[2].indices[0] = _ij2[0];
1776 vinfos[2].indices[1] = _ij2[1];
1777 vinfos[2].maxsolutions = _nj2;
1778 vinfos[3].jointtype = 1;
1779 vinfos[3].foffset = j3;
1780 vinfos[3].indices[0] = _ij3[0];
1781 vinfos[3].indices[1] = _ij3[1];
1782 vinfos[3].maxsolutions = _nj3;
1783 vinfos[4].jointtype = 1;
1784 vinfos[4].foffset = j4;
1785 vinfos[4].indices[0] = _ij4[0];
1786 vinfos[4].indices[1] = _ij4[1];
1787 vinfos[4].maxsolutions = _nj4;
1788 vinfos[5].jointtype = 1;
1789 vinfos[5].foffset = j5;
1790 vinfos[5].indices[0] = _ij5[0];
1791 vinfos[5].indices[1] = _ij5[1];
1792 vinfos[5].maxsolutions = _nj5;
1793 std::vector<int> vfree(0);
1821 IkReal j1array[1], cj1array[1], sj1array[1];
1822 bool j1valid[1]={
false};
1824 IkReal x380=((IkReal(1.00000000000000))*(sj4));
1825 IkReal x381=((r22)*(sj2));
1826 IkReal x382=((cj2)*(cj4));
1827 IkReal x383=((cj3)*(cj5)*(r20));
1828 IkReal x384=((cj2)*(r21)*(sj5));
1829 IkReal x385=((r21)*(sj2)*(sj5));
1830 IkReal x386=((IkReal(1.00000000000000))*(cj5)*(r20));
1831 if(
IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(x382)*(x386)))+(((r21)*(sj5)*(x382)))+(((IkReal(-1.00000000000000))*(cj3)*(x380)*(x385)))+(((sj2)*(sj4)*(x383)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x381)))+(((IkReal(-1.00000000000000))*(cj2)*(r22)*(x380))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst75)*(((((cj3)*(r22)*(x382)))+(((cj4)*(x385)))+(((IkReal(-1.00000000000000))*(x380)*(x381)))+(((IkReal(-1.00000000000000))*(cj2)*(x380)*(x383)))+(((cj3)*(sj4)*(x384)))+(((IkReal(-1.00000000000000))*(cj4)*(sj2)*(x386))))))) <
IKFAST_ATAN2_MAGTHRESH )
1833 j1array[0]=
IKatan2(((gconst75)*(((((IkReal(-1.00000000000000))*(x382)*(x386)))+(((r21)*(sj5)*(x382)))+(((IkReal(-1.00000000000000))*(cj3)*(x380)*(x385)))+(((sj2)*(sj4)*(x383)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x381)))+(((IkReal(-1.00000000000000))*(cj2)*(r22)*(x380)))))), ((gconst75)*(((((cj3)*(r22)*(x382)))+(((cj4)*(x385)))+(((IkReal(-1.00000000000000))*(x380)*(x381)))+(((IkReal(-1.00000000000000))*(cj2)*(x380)*(x383)))+(((cj3)*(sj4)*(x384)))+(((IkReal(-1.00000000000000))*(cj4)*(sj2)*(x386)))))));
1834 sj1array[0]=
IKsin(j1array[0]);
1835 cj1array[0]=
IKcos(j1array[0]);
1836 if( j1array[0] >
IKPI )
1840 else if( j1array[0] < -
IKPI )
1841 { j1array[0]+=
IK2PI;
1844 for(
int ij1 = 0; ij1 < 1; ++ij1)
1850 _ij1[0] = ij1; _ij1[1] = -1;
1851 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1855 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1858 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1861 IkReal x387=
IKsin(j1);
1862 IkReal x388=
IKcos(j1);
1863 IkReal x389=((IkReal(1.00000000000000))*(sj2));
1864 IkReal x390=((cj5)*(r00));
1865 IkReal x391=((cj0)*(sj4));
1866 IkReal x392=((cj5)*(sj4));
1867 IkReal x393=((IkReal(1.00000000000000))*(sj5));
1868 IkReal x394=((r10)*(sj0));
1869 IkReal x395=((IkReal(1.00000000000000))*(sj0));
1870 IkReal x396=((cj0)*(r01));
1871 IkReal x397=((cj4)*(sj5));
1872 IkReal x398=((IkReal(1.00000000000000))*(cj5));
1873 IkReal x399=((cj2)*(x388));
1874 IkReal x400=((sj0)*(x393));
1875 IkReal x401=((cj3)*(x387));
1876 IkReal x402=((IkReal(1.00000000000000))*(cj0)*(cj4));
1877 IkReal x403=((sj3)*(x387));
1878 IkReal x404=((sj2)*(x388));
1879 evalcond[0]=((((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(cj2)*(x403)))+(((IkReal(-1.00000000000000))*(sj3)*(x388)*(x389)))+(((cj5)*(r21))));
1880 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x392)))+(((cj4)*(r22)))+(((sj2)*(x387)))+(((IkReal(-1.00000000000000))*(x399)))+(((r21)*(sj4)*(sj5))));
1881 evalcond[2]=((((cj2)*(x401)))+(((cj3)*(x404)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x393)))+(((cj4)*(cj5)*(r20)))+(((r22)*(sj4))));
1882 evalcond[3]=((((sj2)*(x403)))+(((IkReal(-1.00000000000000))*(x393)*(x394)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x395)))+(((IkReal(-1.00000000000000))*(sj3)*(x399)))+(((IkReal(-1.00000000000000))*(x396)*(x398)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x393))));
1883 evalcond[4]=((((IkReal(-1.00000000000000))*(cj4)*(r12)*(x395)))+(((cj2)*(x387)))+(((IkReal(-1.00000000000000))*(r02)*(x402)))+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(x400)))+(((x390)*(x391)))+(((x392)*(x394)))+(x404)+(((IkReal(-1.00000000000000))*(r01)*(x391)*(x393))));
1884 evalcond[5]=((((IkReal(-1.00000000000000))*(x390)*(x402)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x395)))+(((IkReal(-1.00000000000000))*(r02)*(x391)))+(((cj3)*(x399)))+(((IkReal(-1.00000000000000))*(x389)*(x401)))+(((r11)*(sj0)*(x397)))+(((x396)*(x397)))+(((IkReal(-1.00000000000000))*(cj4)*(x394)*(x398))));
1885 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
1892 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1893 vinfos[0].jointtype = 1;
1894 vinfos[0].foffset = j0;
1895 vinfos[0].indices[0] = _ij0[0];
1896 vinfos[0].indices[1] = _ij0[1];
1897 vinfos[0].maxsolutions = _nj0;
1898 vinfos[1].jointtype = 1;
1899 vinfos[1].foffset = j1;
1900 vinfos[1].indices[0] = _ij1[0];
1901 vinfos[1].indices[1] = _ij1[1];
1902 vinfos[1].maxsolutions = _nj1;
1903 vinfos[2].jointtype = 1;
1904 vinfos[2].foffset = j2;
1905 vinfos[2].indices[0] = _ij2[0];
1906 vinfos[2].indices[1] = _ij2[1];
1907 vinfos[2].maxsolutions = _nj2;
1908 vinfos[3].jointtype = 1;
1909 vinfos[3].foffset = j3;
1910 vinfos[3].indices[0] = _ij3[0];
1911 vinfos[3].indices[1] = _ij3[1];
1912 vinfos[3].maxsolutions = _nj3;
1913 vinfos[4].jointtype = 1;
1914 vinfos[4].foffset = j4;
1915 vinfos[4].indices[0] = _ij4[0];
1916 vinfos[4].indices[1] = _ij4[1];
1917 vinfos[4].maxsolutions = _nj4;
1918 vinfos[5].jointtype = 1;
1919 vinfos[5].foffset = j5;
1920 vinfos[5].indices[0] = _ij5[0];
1921 vinfos[5].indices[1] = _ij5[1];
1922 vinfos[5].maxsolutions = _nj5;
1923 std::vector<int> vfree(0);
1936 IkReal j1array[1], cj1array[1], sj1array[1];
1937 bool j1valid[1]={
false};
1939 IkReal x405=((cj2)*(r20));
1940 IkReal x406=((sj3)*(sj4));
1941 IkReal x407=((cj2)*(r21));
1942 IkReal x408=((sj2)*(sj5));
1943 IkReal x409=((cj5)*(sj2));
1944 IkReal x410=((cj4)*(r22)*(sj3));
1945 if(
IKabs(((gconst74)*(((((cj5)*(x407)))+(((r20)*(x406)*(x409)))+(((IkReal(-1.00000000000000))*(r21)*(x406)*(x408)))+(((IkReal(-1.00000000000000))*(sj2)*(x410)))+(((sj5)*(x405))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst74)*(((((sj5)*(x406)*(x407)))+(((r20)*(x408)))+(((r21)*(x409)))+(((cj2)*(x410)))+(((IkReal(-1.00000000000000))*(cj5)*(x405)*(x406))))))) <
IKFAST_ATAN2_MAGTHRESH )
1947 j1array[0]=
IKatan2(((gconst74)*(((((cj5)*(x407)))+(((r20)*(x406)*(x409)))+(((IkReal(-1.00000000000000))*(r21)*(x406)*(x408)))+(((IkReal(-1.00000000000000))*(sj2)*(x410)))+(((sj5)*(x405)))))), ((gconst74)*(((((sj5)*(x406)*(x407)))+(((r20)*(x408)))+(((r21)*(x409)))+(((cj2)*(x410)))+(((IkReal(-1.00000000000000))*(cj5)*(x405)*(x406)))))));
1948 sj1array[0]=
IKsin(j1array[0]);
1949 cj1array[0]=
IKcos(j1array[0]);
1950 if( j1array[0] >
IKPI )
1954 else if( j1array[0] < -
IKPI )
1955 { j1array[0]+=
IK2PI;
1958 for(
int ij1 = 0; ij1 < 1; ++ij1)
1964 _ij1[0] = ij1; _ij1[1] = -1;
1965 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1969 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1972 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1975 IkReal x411=
IKsin(j1);
1976 IkReal x412=
IKcos(j1);
1977 IkReal x413=((IkReal(1.00000000000000))*(sj2));
1978 IkReal x414=((cj5)*(r00));
1979 IkReal x415=((cj0)*(sj4));
1980 IkReal x416=((cj5)*(sj4));
1981 IkReal x417=((IkReal(1.00000000000000))*(sj5));
1982 IkReal x418=((r10)*(sj0));
1983 IkReal x419=((IkReal(1.00000000000000))*(sj0));
1984 IkReal x420=((cj0)*(r01));
1985 IkReal x421=((cj4)*(sj5));
1986 IkReal x422=((IkReal(1.00000000000000))*(cj5));
1987 IkReal x423=((cj2)*(x412));
1988 IkReal x424=((sj0)*(x417));
1989 IkReal x425=((cj3)*(x411));
1990 IkReal x426=((IkReal(1.00000000000000))*(cj0)*(cj4));
1991 IkReal x427=((sj3)*(x411));
1992 IkReal x428=((sj2)*(x412));
1993 evalcond[0]=((((IkReal(-1.00000000000000))*(sj3)*(x412)*(x413)))+(((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(cj2)*(x427)))+(((cj5)*(r21))));
1994 evalcond[1]=((((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x423)))+(((IkReal(-1.00000000000000))*(r20)*(x416)))+(((sj2)*(x411)))+(((r21)*(sj4)*(sj5))));
1995 evalcond[2]=((((cj3)*(x428)))+(((cj4)*(cj5)*(r20)))+(((r22)*(sj4)))+(((cj2)*(x425)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x417))));
1996 evalcond[3]=((((IkReal(-1.00000000000000))*(sj3)*(x423)))+(((IkReal(-1.00000000000000))*(x420)*(x422)))+(((sj2)*(x427)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x419)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x417)))+(((IkReal(-1.00000000000000))*(x417)*(x418))));
1997 evalcond[4]=((((x414)*(x415)))+(((IkReal(-1.00000000000000))*(r01)*(x415)*(x417)))+(x428)+(((x416)*(x418)))+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(x424)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x419)))+(((IkReal(-1.00000000000000))*(r02)*(x426)))+(((cj2)*(x411))));
1998 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x418)*(x422)))+(((cj3)*(x423)))+(((IkReal(-1.00000000000000))*(x414)*(x426)))+(((x420)*(x421)))+(((r11)*(sj0)*(x421)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x419)))+(((IkReal(-1.00000000000000))*(r02)*(x415)))+(((IkReal(-1.00000000000000))*(x413)*(x425))));
1999 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
2006 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2007 vinfos[0].jointtype = 1;
2008 vinfos[0].foffset = j0;
2009 vinfos[0].indices[0] = _ij0[0];
2010 vinfos[0].indices[1] = _ij0[1];
2011 vinfos[0].maxsolutions = _nj0;
2012 vinfos[1].jointtype = 1;
2013 vinfos[1].foffset = j1;
2014 vinfos[1].indices[0] = _ij1[0];
2015 vinfos[1].indices[1] = _ij1[1];
2016 vinfos[1].maxsolutions = _nj1;
2017 vinfos[2].jointtype = 1;
2018 vinfos[2].foffset = j2;
2019 vinfos[2].indices[0] = _ij2[0];
2020 vinfos[2].indices[1] = _ij2[1];
2021 vinfos[2].maxsolutions = _nj2;
2022 vinfos[3].jointtype = 1;
2023 vinfos[3].foffset = j3;
2024 vinfos[3].indices[0] = _ij3[0];
2025 vinfos[3].indices[1] = _ij3[1];
2026 vinfos[3].maxsolutions = _nj3;
2027 vinfos[4].jointtype = 1;
2028 vinfos[4].foffset = j4;
2029 vinfos[4].indices[0] = _ij4[0];
2030 vinfos[4].indices[1] = _ij4[1];
2031 vinfos[4].maxsolutions = _nj4;
2032 vinfos[5].jointtype = 1;
2033 vinfos[5].foffset = j5;
2034 vinfos[5].indices[0] = _ij5[0];
2035 vinfos[5].indices[1] = _ij5[1];
2036 vinfos[5].maxsolutions = _nj5;
2037 std::vector<int> vfree(0);
2054 IkReal j3array[1], cj3array[1], sj3array[1];
2055 bool j3valid[1]={
false};
2057 IkReal x429=((IkReal(20.0000000000000))*(sj5));
2058 IkReal x430=((IkReal(20.0000000000000))*(cj5));
2059 if(
IKabs(((gconst53)*(((IkReal(1.90000000000000))+(((npy)*(x430)))+(((npx)*(x429))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst53)*(((((cj4)*(npy)*(x429)))+(((IkReal(-1.00000000000000))*(cj4)*(npx)*(x430)))+(((IkReal(-20.0000000000000))*(npz)*(sj4))))))) <
IKFAST_ATAN2_MAGTHRESH )
2061 j3array[0]=
IKatan2(((gconst53)*(((IkReal(1.90000000000000))+(((npy)*(x430)))+(((npx)*(x429)))))), ((gconst53)*(((((cj4)*(npy)*(x429)))+(((IkReal(-1.00000000000000))*(cj4)*(npx)*(x430)))+(((IkReal(-20.0000000000000))*(npz)*(sj4)))))));
2062 sj3array[0]=
IKsin(j3array[0]);
2063 cj3array[0]=
IKcos(j3array[0]);
2064 if( j3array[0] >
IKPI )
2068 else if( j3array[0] < -
IKPI )
2069 { j3array[0]+=
IK2PI;
2072 for(
int ij3 = 0; ij3 < 1; ++ij3)
2078 _ij3[0] = ij3; _ij3[1] = -1;
2079 for(
int iij3 = ij3+1; iij3 < 1; ++iij3)
2083 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
2086 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2089 IkReal x431=((IkReal(0.350000000000000))*(sj2));
2090 evalcond[0]=((IkReal(0.0950000000000000))+(((IkReal(-1.00000000000000))*(x431)*(
IKsin(j3))))+(((cj5)*(npy)))+(((npx)*(sj5))));
2091 evalcond[1]=((((IkReal(-1.00000000000000))*(cj4)*(npy)*(sj5)))+(((cj4)*(cj5)*(npx)))+(((x431)*(
IKcos(j3))))+(((npz)*(sj4))));
2092 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
2099 IkReal dummyeval[1];
2101 gconst86=
IKsign(((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2))))));
2102 dummyeval[0]=((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2)))));
2103 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2106 IkReal dummyeval[1];
2108 gconst87=
IKsign(((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2))))));
2109 dummyeval[0]=((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2)))));
2110 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2113 IkReal dummyeval[1];
2115 IkReal x432=(cj5)*(cj5);
2116 IkReal x433=(sj5)*(sj5);
2117 IkReal x434=((IkReal(1.00000000000000))*(r10));
2118 IkReal x435=((cj4)*(sj5));
2119 IkReal x436=((r00)*(r11));
2120 IkReal x437=((cj4)*(cj5));
2121 IkReal x438=((sj4)*(x432));
2122 IkReal x439=((sj4)*(x433));
2123 gconst84=
IKsign(((((x436)*(x439)))+(((x436)*(x438)))+(((r00)*(r12)*(x435)))+(((r01)*(r12)*(x437)))+(((IkReal(-1.00000000000000))*(r02)*(x434)*(x435)))+(((IkReal(-1.00000000000000))*(r01)*(x434)*(x439)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x437)))+(((IkReal(-1.00000000000000))*(r01)*(x434)*(x438)))));
2124 IkReal x440=(cj5)*(cj5);
2125 IkReal x441=(sj5)*(sj5);
2126 IkReal x442=((IkReal(1.00000000000000))*(r10));
2127 IkReal x443=((cj4)*(sj5));
2128 IkReal x444=((r00)*(r11));
2129 IkReal x445=((cj4)*(cj5));
2130 IkReal x446=((sj4)*(x440));
2131 IkReal x447=((sj4)*(x441));
2132 dummyeval[0]=((((r00)*(r12)*(x443)))+(((IkReal(-1.00000000000000))*(r01)*(x442)*(x447)))+(((IkReal(-1.00000000000000))*(r01)*(x442)*(x446)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x445)))+(((x444)*(x447)))+(((x444)*(x446)))+(((IkReal(-1.00000000000000))*(r02)*(x442)*(x443)))+(((r01)*(r12)*(x445))));
2133 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2137 IkReal x448=((cj5)*(npx));
2138 IkReal x449=((IkReal(1.00000000000000))*(cj4)*(sj5));
2139 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(1.57079632679490))+(j3)), IkReal(6.28318530717959))));
2140 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(-0.350000000000000))*(sj2)))+(((npx)*(sj5))));
2141 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((npy)*(sj4)*(sj5)))+(((IkReal(-1.00000000000000))*(sj4)*(x448))));
2142 evalcond[3]=((((cj4)*(x448)))+(((IkReal(-1.00000000000000))*(npy)*(x449)))+(((npz)*(sj4))));
2143 evalcond[4]=((((cj4)*(cj5)*(r20)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(r21)*(x449))));
2144 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 )
2147 IkReal dummyeval[1];
2149 gconst102=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
2150 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
2151 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2154 IkReal dummyeval[1];
2156 IkReal x450=(cj5)*(cj5);
2157 IkReal x451=(sj5)*(sj5);
2158 IkReal x452=((sj4)*(sj5));
2159 IkReal x453=((IkReal(1.00000000000000))*(r12));
2160 IkReal x454=((cj5)*(sj4));
2161 IkReal x455=((cj4)*(r00)*(r11));
2162 IkReal x456=((IkReal(1.00000000000000))*(cj4)*(r01)*(r10));
2163 gconst100=
IKsign(((((IkReal(-1.00000000000000))*(r00)*(x452)*(x453)))+(((IkReal(-1.00000000000000))*(x450)*(x456)))+(((x450)*(x455)))+(((x451)*(x455)))+(((IkReal(-1.00000000000000))*(x451)*(x456)))+(((r02)*(r11)*(x454)))+(((r02)*(r10)*(x452)))+(((IkReal(-1.00000000000000))*(r01)*(x453)*(x454)))));
2164 IkReal x457=(cj5)*(cj5);
2165 IkReal x458=(sj5)*(sj5);
2166 IkReal x459=((sj4)*(sj5));
2167 IkReal x460=((IkReal(1.00000000000000))*(r12));
2168 IkReal x461=((cj5)*(sj4));
2169 IkReal x462=((cj4)*(r00)*(r11));
2171 dummyeval[0]=((((IkReal(-1.00000000000000))*(x458)*(x463)))+(((x457)*(x462)))+(((r02)*(r10)*(x459)))+(((IkReal(-1.00000000000000))*(r01)*(x460)*(x461)))+(((IkReal(-1.00000000000000))*(x457)*(x463)))+(((IkReal(-1.00000000000000))*(r00)*(x459)*(x460)))+(((x458)*(x462)))+(((r02)*(r11)*(x461))));
2172 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2175 IkReal dummyeval[1];
2177 IkReal x464=(cj4)*(cj4);
2178 IkReal x465=(sj4)*(sj4);
2179 IkReal x466=((r00)*(r12));
2180 IkReal x467=((r02)*(r11));
2181 IkReal x468=((cj5)*(x464));
2182 IkReal x469=((sj5)*(x465));
2183 IkReal x470=((IkReal(1.00000000000000))*(r02)*(r10));
2184 IkReal x471=((IkReal(1.00000000000000))*(r01)*(r12));
2185 IkReal x472=((cj5)*(x465));
2186 IkReal x473=((sj5)*(x464));
2187 gconst101=
IKsign(((((IkReal(-1.00000000000000))*(x468)*(x470)))+(((x467)*(x473)))+(((IkReal(-1.00000000000000))*(x471)*(x473)))+(((x466)*(x468)))+(((x466)*(x472)))+(((IkReal(-1.00000000000000))*(x470)*(x472)))+(((x467)*(x469)))+(((IkReal(-1.00000000000000))*(x469)*(x471)))));
2188 IkReal x474=(cj4)*(cj4);
2189 IkReal x475=(sj4)*(sj4);
2190 IkReal x476=((r00)*(r12));
2191 IkReal x477=((r02)*(r11));
2192 IkReal x478=((cj5)*(x474));
2193 IkReal x479=((sj5)*(x475));
2194 IkReal x480=((IkReal(1.00000000000000))*(r02)*(r10));
2195 IkReal x481=((IkReal(1.00000000000000))*(r01)*(r12));
2196 IkReal x482=((cj5)*(x475));
2197 IkReal x483=((sj5)*(x474));
2198 dummyeval[0]=((((x476)*(x482)))+(((IkReal(-1.00000000000000))*(x478)*(x480)))+(((x476)*(x478)))+(((IkReal(-1.00000000000000))*(x481)*(x483)))+(((x477)*(x483)))+(((x477)*(x479)))+(((IkReal(-1.00000000000000))*(x480)*(x482)))+(((IkReal(-1.00000000000000))*(x479)*(x481))));
2199 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2206 IkReal j0array[1], cj0array[1], sj0array[1];
2207 bool j0valid[1]={
false};
2209 IkReal x484=((cj5)*(sj4));
2210 IkReal x485=((IkReal(1.00000000000000))*(cj4));
2211 IkReal x486=((IkReal(1.00000000000000))*(sj4)*(sj5));
2212 if(
IKabs(((gconst101)*(((((r10)*(x484)))+(((IkReal(-1.00000000000000))*(r11)*(x486)))+(((IkReal(-1.00000000000000))*(r12)*(x485))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst101)*(((((IkReal(-1.00000000000000))*(r01)*(x486)))+(((r00)*(x484)))+(((IkReal(-1.00000000000000))*(r02)*(x485))))))) <
IKFAST_ATAN2_MAGTHRESH )
2214 j0array[0]=
IKatan2(((gconst101)*(((((r10)*(x484)))+(((IkReal(-1.00000000000000))*(r11)*(x486)))+(((IkReal(-1.00000000000000))*(r12)*(x485)))))), ((gconst101)*(((((IkReal(-1.00000000000000))*(r01)*(x486)))+(((r00)*(x484)))+(((IkReal(-1.00000000000000))*(r02)*(x485)))))));
2215 sj0array[0]=
IKsin(j0array[0]);
2216 cj0array[0]=
IKcos(j0array[0]);
2217 if( j0array[0] >
IKPI )
2221 else if( j0array[0] < -
IKPI )
2222 { j0array[0]+=
IK2PI;
2225 for(
int ij0 = 0; ij0 < 1; ++ij0)
2231 _ij0[0] = ij0; _ij0[1] = -1;
2232 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
2236 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
2239 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
2242 IkReal x487=
IKsin(j0);
2243 IkReal x488=
IKcos(j0);
2244 IkReal x489=((IkReal(1.00000000000000))*(sj5));
2245 IkReal x490=((cj5)*(r10));
2246 IkReal x491=((IkReal(1.00000000000000))*(r12));
2247 IkReal x492=((cj4)*(cj5));
2248 IkReal x493=((r11)*(sj5));
2249 IkReal x494=((IkReal(1.00000000000000))*(cj5));
2250 IkReal x495=((r00)*(x487));
2251 IkReal x496=((sj4)*(x488));
2252 IkReal x497=((r01)*(x487));
2253 IkReal x498=((sj4)*(x487));
2254 IkReal x499=((cj4)*(x487));
2255 IkReal x500=((cj4)*(x488));
2256 evalcond[0]=((((cj5)*(x497)))+(((IkReal(-1.00000000000000))*(r10)*(x488)*(x489)))+(((sj5)*(x495)))+(((IkReal(-1.00000000000000))*(r11)*(x488)*(x494))));
2257 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x489)*(x496)))+(((x490)*(x496)))+(((r02)*(x499)))+(((IkReal(-1.00000000000000))*(x491)*(x500)))+(((IkReal(-1.00000000000000))*(sj4)*(x494)*(x495)))+(((sj4)*(sj5)*(x497))));
2258 evalcond[2]=((IkReal(1.00000000000000))+(((x493)*(x500)))+(((IkReal(-1.00000000000000))*(x491)*(x496)))+(((r02)*(x498)))+(((IkReal(-1.00000000000000))*(cj4)*(x489)*(x497)))+(((IkReal(-1.00000000000000))*(x490)*(x500)))+(((x492)*(x495))));
2259 evalcond[3]=((((IkReal(-1.00000000000000))*(x491)*(x498)))+(((IkReal(-1.00000000000000))*(x490)*(x499)))+(((IkReal(-1.00000000000000))*(r02)*(x496)))+(((IkReal(-1.00000000000000))*(r00)*(x488)*(x492)))+(((r01)*(sj5)*(x500)))+(((x493)*(x499))));
2260 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2267 IkReal dummyeval[1];
2269 gconst103=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
2270 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
2271 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2274 IkReal dummyeval[1];
2276 gconst104=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
2277 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
2278 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2285 IkReal j1array[1], cj1array[1], sj1array[1];
2286 bool j1valid[1]={
false};
2288 IkReal x501=((cj2)*(sj5));
2289 IkReal x502=((r10)*(sj0));
2290 IkReal x503=((cj2)*(cj5));
2291 IkReal x504=((cj0)*(r00));
2292 IkReal x505=((sj2)*(sj5));
2293 IkReal x506=((r11)*(sj0));
2294 IkReal x507=((cj5)*(sj2));
2295 IkReal x508=((cj0)*(r01));
2296 if(
IKabs(((gconst104)*(((((r20)*(x501)))+(((x504)*(x505)))+(((x507)*(x508)))+(((x506)*(x507)))+(((x502)*(x505)))+(((r21)*(x503))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst104)*(((((r20)*(x505)))+(((IkReal(-1.00000000000000))*(x501)*(x504)))+(((IkReal(-1.00000000000000))*(x501)*(x502)))+(((r21)*(x507)))+(((IkReal(-1.00000000000000))*(x503)*(x506)))+(((IkReal(-1.00000000000000))*(x503)*(x508))))))) <
IKFAST_ATAN2_MAGTHRESH )
2298 j1array[0]=
IKatan2(((gconst104)*(((((r20)*(x501)))+(((x504)*(x505)))+(((x507)*(x508)))+(((x506)*(x507)))+(((x502)*(x505)))+(((r21)*(x503)))))), ((gconst104)*(((((r20)*(x505)))+(((IkReal(-1.00000000000000))*(x501)*(x504)))+(((IkReal(-1.00000000000000))*(x501)*(x502)))+(((r21)*(x507)))+(((IkReal(-1.00000000000000))*(x503)*(x506)))+(((IkReal(-1.00000000000000))*(x503)*(x508)))))));
2299 sj1array[0]=
IKsin(j1array[0]);
2300 cj1array[0]=
IKcos(j1array[0]);
2301 if( j1array[0] >
IKPI )
2305 else if( j1array[0] < -
IKPI )
2306 { j1array[0]+=
IK2PI;
2309 for(
int ij1 = 0; ij1 < 1; ++ij1)
2315 _ij1[0] = ij1; _ij1[1] = -1;
2316 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2320 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2323 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2326 IkReal x509=
IKsin(j1);
2327 IkReal x510=
IKcos(j1);
2328 IkReal x511=((cj0)*(sj4));
2329 IkReal x512=((IkReal(1.00000000000000))*(sj5));
2330 IkReal x513=((cj5)*(sj0));
2331 IkReal x514=((IkReal(1.00000000000000))*(cj4));
2332 IkReal x515=((IkReal(1.00000000000000))*(cj5));
2333 IkReal x516=((sj2)*(x509));
2334 IkReal x517=((IkReal(1.00000000000000))*(x510));
2335 IkReal x518=((cj2)*(x509));
2336 IkReal x519=((cj2)*(x517));
2337 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x517)))+(((IkReal(-1.00000000000000))*(x518)))+(((r20)*(sj5)))+(((cj5)*(r21))));
2338 evalcond[1]=((x516)+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x515)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x519)))+(((r21)*(sj4)*(sj5))));
2339 evalcond[2]=((x516)+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x512)))+(((IkReal(-1.00000000000000))*(cj0)*(r01)*(x515)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x512)))+(((IkReal(-1.00000000000000))*(x519)))+(((IkReal(-1.00000000000000))*(r11)*(x513))));
2340 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x512)))+(((sj2)*(x510)))+(x518)+(((r10)*(sj4)*(x513)))+(((IkReal(-1.00000000000000))*(r01)*(x511)*(x512)))+(((cj5)*(r00)*(x511)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x514)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x514))));
2341 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2348 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2349 vinfos[0].jointtype = 1;
2350 vinfos[0].foffset = j0;
2351 vinfos[0].indices[0] = _ij0[0];
2352 vinfos[0].indices[1] = _ij0[1];
2353 vinfos[0].maxsolutions = _nj0;
2354 vinfos[1].jointtype = 1;
2355 vinfos[1].foffset = j1;
2356 vinfos[1].indices[0] = _ij1[0];
2357 vinfos[1].indices[1] = _ij1[1];
2358 vinfos[1].maxsolutions = _nj1;
2359 vinfos[2].jointtype = 1;
2360 vinfos[2].foffset = j2;
2361 vinfos[2].indices[0] = _ij2[0];
2362 vinfos[2].indices[1] = _ij2[1];
2363 vinfos[2].maxsolutions = _nj2;
2364 vinfos[3].jointtype = 1;
2365 vinfos[3].foffset = j3;
2366 vinfos[3].indices[0] = _ij3[0];
2367 vinfos[3].indices[1] = _ij3[1];
2368 vinfos[3].maxsolutions = _nj3;
2369 vinfos[4].jointtype = 1;
2370 vinfos[4].foffset = j4;
2371 vinfos[4].indices[0] = _ij4[0];
2372 vinfos[4].indices[1] = _ij4[1];
2373 vinfos[4].maxsolutions = _nj4;
2374 vinfos[5].jointtype = 1;
2375 vinfos[5].foffset = j5;
2376 vinfos[5].indices[0] = _ij5[0];
2377 vinfos[5].indices[1] = _ij5[1];
2378 vinfos[5].maxsolutions = _nj5;
2379 std::vector<int> vfree(0);
2392 IkReal j1array[1], cj1array[1], sj1array[1];
2393 bool j1valid[1]={
false};
2395 IkReal x520=((r20)*(sj5));
2396 IkReal x521=((cj4)*(r22));
2397 IkReal x522=((sj2)*(sj4));
2398 IkReal x523=((cj5)*(r20));
2399 IkReal x524=((cj5)*(r21));
2400 IkReal x525=((r21)*(sj5));
2401 IkReal x526=((cj2)*(sj4));
2402 if(
IKabs(((gconst103)*(((((x522)*(x523)))+(((IkReal(-1.00000000000000))*(sj2)*(x521)))+(((cj2)*(x520)))+(((IkReal(-1.00000000000000))*(x522)*(x525)))+(((cj2)*(x524))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst103)*(((((cj2)*(x521)))+(((sj2)*(x524)))+(((sj2)*(x520)))+(((IkReal(-1.00000000000000))*(x523)*(x526)))+(((x525)*(x526))))))) <
IKFAST_ATAN2_MAGTHRESH )
2404 j1array[0]=
IKatan2(((gconst103)*(((((x522)*(x523)))+(((IkReal(-1.00000000000000))*(sj2)*(x521)))+(((cj2)*(x520)))+(((IkReal(-1.00000000000000))*(x522)*(x525)))+(((cj2)*(x524)))))), ((gconst103)*(((((cj2)*(x521)))+(((sj2)*(x524)))+(((sj2)*(x520)))+(((IkReal(-1.00000000000000))*(x523)*(x526)))+(((x525)*(x526)))))));
2405 sj1array[0]=
IKsin(j1array[0]);
2406 cj1array[0]=
IKcos(j1array[0]);
2407 if( j1array[0] >
IKPI )
2411 else if( j1array[0] < -
IKPI )
2412 { j1array[0]+=
IK2PI;
2415 for(
int ij1 = 0; ij1 < 1; ++ij1)
2421 _ij1[0] = ij1; _ij1[1] = -1;
2422 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2426 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2429 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2432 IkReal x527=
IKsin(j1);
2433 IkReal x528=
IKcos(j1);
2434 IkReal x529=((cj0)*(sj4));
2435 IkReal x530=((IkReal(1.00000000000000))*(sj5));
2436 IkReal x531=((cj5)*(sj0));
2437 IkReal x532=((IkReal(1.00000000000000))*(cj4));
2438 IkReal x533=((IkReal(1.00000000000000))*(cj5));
2439 IkReal x534=((sj2)*(x527));
2440 IkReal x535=((IkReal(1.00000000000000))*(x528));
2441 IkReal x536=((cj2)*(x527));
2442 IkReal x537=((cj2)*(x535));
2443 evalcond[0]=((((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x536)))+(((IkReal(-1.00000000000000))*(sj2)*(x535)))+(((cj5)*(r21))));
2444 evalcond[1]=((((IkReal(-1.00000000000000))*(x537)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x533)))+(((r21)*(sj4)*(sj5)))+(x534));
2445 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(r01)*(x533)))+(((IkReal(-1.00000000000000))*(x537)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x530)))+(((IkReal(-1.00000000000000))*(r11)*(x531)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x530)))+(x534));
2446 evalcond[3]=((((sj2)*(x528)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x532)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x530)))+(((IkReal(-1.00000000000000))*(r01)*(x529)*(x530)))+(((cj5)*(r00)*(x529)))+(((r10)*(sj4)*(x531)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x532)))+(x536));
2447 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2454 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2455 vinfos[0].jointtype = 1;
2456 vinfos[0].foffset = j0;
2457 vinfos[0].indices[0] = _ij0[0];
2458 vinfos[0].indices[1] = _ij0[1];
2459 vinfos[0].maxsolutions = _nj0;
2460 vinfos[1].jointtype = 1;
2461 vinfos[1].foffset = j1;
2462 vinfos[1].indices[0] = _ij1[0];
2463 vinfos[1].indices[1] = _ij1[1];
2464 vinfos[1].maxsolutions = _nj1;
2465 vinfos[2].jointtype = 1;
2466 vinfos[2].foffset = j2;
2467 vinfos[2].indices[0] = _ij2[0];
2468 vinfos[2].indices[1] = _ij2[1];
2469 vinfos[2].maxsolutions = _nj2;
2470 vinfos[3].jointtype = 1;
2471 vinfos[3].foffset = j3;
2472 vinfos[3].indices[0] = _ij3[0];
2473 vinfos[3].indices[1] = _ij3[1];
2474 vinfos[3].maxsolutions = _nj3;
2475 vinfos[4].jointtype = 1;
2476 vinfos[4].foffset = j4;
2477 vinfos[4].indices[0] = _ij4[0];
2478 vinfos[4].indices[1] = _ij4[1];
2479 vinfos[4].maxsolutions = _nj4;
2480 vinfos[5].jointtype = 1;
2481 vinfos[5].foffset = j5;
2482 vinfos[5].indices[0] = _ij5[0];
2483 vinfos[5].indices[1] = _ij5[1];
2484 vinfos[5].maxsolutions = _nj5;
2485 std::vector<int> vfree(0);
2504 IkReal j0array[1], cj0array[1], sj0array[1];
2505 bool j0valid[1]={
false};
2507 IkReal x538=((IkReal(1.00000000000000))*(sj5));
2508 IkReal x539=((IkReal(1.00000000000000))*(cj5));
2509 if(
IKabs(((gconst100)*(((((IkReal(-1.00000000000000))*(r10)*(x538)))+(((IkReal(-1.00000000000000))*(r11)*(x539))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst100)*(((((IkReal(-1.00000000000000))*(r01)*(x539)))+(((IkReal(-1.00000000000000))*(r00)*(x538))))))) <
IKFAST_ATAN2_MAGTHRESH )
2511 j0array[0]=
IKatan2(((gconst100)*(((((IkReal(-1.00000000000000))*(r10)*(x538)))+(((IkReal(-1.00000000000000))*(r11)*(x539)))))), ((gconst100)*(((((IkReal(-1.00000000000000))*(r01)*(x539)))+(((IkReal(-1.00000000000000))*(r00)*(x538)))))));
2512 sj0array[0]=
IKsin(j0array[0]);
2513 cj0array[0]=
IKcos(j0array[0]);
2514 if( j0array[0] >
IKPI )
2518 else if( j0array[0] < -
IKPI )
2519 { j0array[0]+=
IK2PI;
2522 for(
int ij0 = 0; ij0 < 1; ++ij0)
2528 _ij0[0] = ij0; _ij0[1] = -1;
2529 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
2533 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
2536 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
2539 IkReal x540=
IKsin(j0);
2540 IkReal x541=
IKcos(j0);
2541 IkReal x542=((IkReal(1.00000000000000))*(sj5));
2542 IkReal x543=((cj5)*(r10));
2543 IkReal x544=((IkReal(1.00000000000000))*(r12));
2544 IkReal x545=((cj4)*(cj5));
2545 IkReal x546=((r11)*(sj5));
2546 IkReal x547=((IkReal(1.00000000000000))*(cj5));
2547 IkReal x548=((r00)*(x540));
2548 IkReal x549=((sj4)*(x541));
2549 IkReal x550=((r01)*(x540));
2550 IkReal x551=((sj4)*(x540));
2551 IkReal x552=((cj4)*(x540));
2552 IkReal x553=((cj4)*(x541));
2553 evalcond[0]=((((cj5)*(x550)))+(((IkReal(-1.00000000000000))*(r11)*(x541)*(x547)))+(((sj5)*(x548)))+(((IkReal(-1.00000000000000))*(r10)*(x541)*(x542))));
2554 evalcond[1]=((((IkReal(-1.00000000000000))*(x544)*(x553)))+(((IkReal(-1.00000000000000))*(sj4)*(x547)*(x548)))+(((sj4)*(sj5)*(x550)))+(((r02)*(x552)))+(((IkReal(-1.00000000000000))*(r11)*(x542)*(x549)))+(((x543)*(x549))));
2555 evalcond[2]=((IkReal(1.00000000000000))+(((x545)*(x548)))+(((IkReal(-1.00000000000000))*(x543)*(x553)))+(((IkReal(-1.00000000000000))*(cj4)*(x542)*(x550)))+(((x546)*(x553)))+(((r02)*(x551)))+(((IkReal(-1.00000000000000))*(x544)*(x549))));
2556 evalcond[3]=((((x546)*(x552)))+(((r01)*(sj5)*(x553)))+(((IkReal(-1.00000000000000))*(r02)*(x549)))+(((IkReal(-1.00000000000000))*(x543)*(x552)))+(((IkReal(-1.00000000000000))*(r00)*(x541)*(x545)))+(((IkReal(-1.00000000000000))*(x544)*(x551))));
2557 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2564 IkReal dummyeval[1];
2566 gconst103=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
2567 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
2568 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2571 IkReal dummyeval[1];
2573 gconst104=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
2574 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
2575 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2582 IkReal j1array[1], cj1array[1], sj1array[1];
2583 bool j1valid[1]={
false};
2585 IkReal x554=((cj2)*(sj5));
2586 IkReal x555=((r10)*(sj0));
2587 IkReal x556=((cj2)*(cj5));
2588 IkReal x557=((cj0)*(r00));
2589 IkReal x558=((sj2)*(sj5));
2590 IkReal x559=((r11)*(sj0));
2591 IkReal x560=((cj5)*(sj2));
2592 IkReal x561=((cj0)*(r01));
2593 if(
IKabs(((gconst104)*(((((x560)*(x561)))+(((r21)*(x556)))+(((r20)*(x554)))+(((x555)*(x558)))+(((x559)*(x560)))+(((x557)*(x558))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst104)*(((((IkReal(-1.00000000000000))*(x554)*(x557)))+(((r20)*(x558)))+(((IkReal(-1.00000000000000))*(x554)*(x555)))+(((IkReal(-1.00000000000000))*(x556)*(x559)))+(((IkReal(-1.00000000000000))*(x556)*(x561)))+(((r21)*(x560))))))) <
IKFAST_ATAN2_MAGTHRESH )
2595 j1array[0]=
IKatan2(((gconst104)*(((((x560)*(x561)))+(((r21)*(x556)))+(((r20)*(x554)))+(((x555)*(x558)))+(((x559)*(x560)))+(((x557)*(x558)))))), ((gconst104)*(((((IkReal(-1.00000000000000))*(x554)*(x557)))+(((r20)*(x558)))+(((IkReal(-1.00000000000000))*(x554)*(x555)))+(((IkReal(-1.00000000000000))*(x556)*(x559)))+(((IkReal(-1.00000000000000))*(x556)*(x561)))+(((r21)*(x560)))))));
2596 sj1array[0]=
IKsin(j1array[0]);
2597 cj1array[0]=
IKcos(j1array[0]);
2598 if( j1array[0] >
IKPI )
2602 else if( j1array[0] < -
IKPI )
2603 { j1array[0]+=
IK2PI;
2606 for(
int ij1 = 0; ij1 < 1; ++ij1)
2612 _ij1[0] = ij1; _ij1[1] = -1;
2613 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2617 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2620 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2623 IkReal x562=
IKsin(j1);
2624 IkReal x563=
IKcos(j1);
2625 IkReal x564=((cj0)*(sj4));
2626 IkReal x565=((IkReal(1.00000000000000))*(sj5));
2627 IkReal x566=((cj5)*(sj0));
2628 IkReal x567=((IkReal(1.00000000000000))*(cj4));
2629 IkReal x568=((IkReal(1.00000000000000))*(cj5));
2630 IkReal x569=((sj2)*(x562));
2631 IkReal x570=((IkReal(1.00000000000000))*(x563));
2632 IkReal x571=((cj2)*(x562));
2633 IkReal x572=((cj2)*(x570));
2634 evalcond[0]=((((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(sj2)*(x570)))+(((IkReal(-1.00000000000000))*(x571)))+(((cj5)*(r21))));
2635 evalcond[1]=((x569)+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x568)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x572)))+(((r21)*(sj4)*(sj5))));
2636 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(r01)*(x568)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x565)))+(x569)+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x565)))+(((IkReal(-1.00000000000000))*(x572)))+(((IkReal(-1.00000000000000))*(r11)*(x566))));
2637 evalcond[3]=((x571)+(((r10)*(sj4)*(x566)))+(((cj5)*(r00)*(x564)))+(((IkReal(-1.00000000000000))*(r01)*(x564)*(x565)))+(((sj2)*(x563)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x565)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x567)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x567))));
2638 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2646 vinfos[0].jointtype = 1;
2647 vinfos[0].foffset = j0;
2648 vinfos[0].indices[0] = _ij0[0];
2649 vinfos[0].indices[1] = _ij0[1];
2650 vinfos[0].maxsolutions = _nj0;
2651 vinfos[1].jointtype = 1;
2652 vinfos[1].foffset = j1;
2653 vinfos[1].indices[0] = _ij1[0];
2654 vinfos[1].indices[1] = _ij1[1];
2655 vinfos[1].maxsolutions = _nj1;
2656 vinfos[2].jointtype = 1;
2657 vinfos[2].foffset = j2;
2658 vinfos[2].indices[0] = _ij2[0];
2659 vinfos[2].indices[1] = _ij2[1];
2660 vinfos[2].maxsolutions = _nj2;
2661 vinfos[3].jointtype = 1;
2662 vinfos[3].foffset = j3;
2663 vinfos[3].indices[0] = _ij3[0];
2664 vinfos[3].indices[1] = _ij3[1];
2665 vinfos[3].maxsolutions = _nj3;
2666 vinfos[4].jointtype = 1;
2667 vinfos[4].foffset = j4;
2668 vinfos[4].indices[0] = _ij4[0];
2669 vinfos[4].indices[1] = _ij4[1];
2670 vinfos[4].maxsolutions = _nj4;
2671 vinfos[5].jointtype = 1;
2672 vinfos[5].foffset = j5;
2673 vinfos[5].indices[0] = _ij5[0];
2674 vinfos[5].indices[1] = _ij5[1];
2675 vinfos[5].maxsolutions = _nj5;
2676 std::vector<int> vfree(0);
2689 IkReal j1array[1], cj1array[1], sj1array[1];
2690 bool j1valid[1]={
false};
2692 IkReal x573=((r20)*(sj5));
2693 IkReal x574=((cj4)*(r22));
2694 IkReal x575=((sj2)*(sj4));
2695 IkReal x576=((cj5)*(r20));
2696 IkReal x577=((cj5)*(r21));
2697 IkReal x578=((r21)*(sj5));
2698 IkReal x579=((cj2)*(sj4));
2699 if(
IKabs(((gconst103)*(((((cj2)*(x573)))+(((x575)*(x576)))+(((IkReal(-1.00000000000000))*(sj2)*(x574)))+(((cj2)*(x577)))+(((IkReal(-1.00000000000000))*(x575)*(x578))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst103)*(((((IkReal(-1.00000000000000))*(x576)*(x579)))+(((cj2)*(x574)))+(((sj2)*(x573)))+(((sj2)*(x577)))+(((x578)*(x579))))))) <
IKFAST_ATAN2_MAGTHRESH )
2701 j1array[0]=
IKatan2(((gconst103)*(((((cj2)*(x573)))+(((x575)*(x576)))+(((IkReal(-1.00000000000000))*(sj2)*(x574)))+(((cj2)*(x577)))+(((IkReal(-1.00000000000000))*(x575)*(x578)))))), ((gconst103)*(((((IkReal(-1.00000000000000))*(x576)*(x579)))+(((cj2)*(x574)))+(((sj2)*(x573)))+(((sj2)*(x577)))+(((x578)*(x579)))))));
2702 sj1array[0]=
IKsin(j1array[0]);
2703 cj1array[0]=
IKcos(j1array[0]);
2704 if( j1array[0] >
IKPI )
2708 else if( j1array[0] < -
IKPI )
2709 { j1array[0]+=
IK2PI;
2712 for(
int ij1 = 0; ij1 < 1; ++ij1)
2718 _ij1[0] = ij1; _ij1[1] = -1;
2719 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2723 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2726 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2729 IkReal x580=
IKsin(j1);
2730 IkReal x581=
IKcos(j1);
2731 IkReal x582=((cj0)*(sj4));
2732 IkReal x583=((IkReal(1.00000000000000))*(sj5));
2733 IkReal x584=((cj5)*(sj0));
2734 IkReal x585=((IkReal(1.00000000000000))*(cj4));
2735 IkReal x586=((IkReal(1.00000000000000))*(cj5));
2736 IkReal x587=((sj2)*(x580));
2737 IkReal x588=((IkReal(1.00000000000000))*(x581));
2738 IkReal x589=((cj2)*(x580));
2739 IkReal x590=((cj2)*(x588));
2740 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x588)))+(((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x589)))+(((cj5)*(r21))));
2741 evalcond[1]=((((IkReal(-1.00000000000000))*(x590)))+(((cj4)*(r22)))+(x587)+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x586)))+(((r21)*(sj4)*(sj5))));
2742 evalcond[2]=((((IkReal(-1.00000000000000))*(x590)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x583)))+(((IkReal(-1.00000000000000))*(r11)*(x584)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x583)))+(((IkReal(-1.00000000000000))*(cj0)*(r01)*(x586)))+(x587));
2743 evalcond[3]=((((r10)*(sj4)*(x584)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x585)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x585)))+(((IkReal(-1.00000000000000))*(r01)*(x582)*(x583)))+(((sj2)*(x581)))+(x589)+(((cj5)*(r00)*(x582)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x583))));
2744 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
2751 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2752 vinfos[0].jointtype = 1;
2753 vinfos[0].foffset = j0;
2754 vinfos[0].indices[0] = _ij0[0];
2755 vinfos[0].indices[1] = _ij0[1];
2756 vinfos[0].maxsolutions = _nj0;
2757 vinfos[1].jointtype = 1;
2758 vinfos[1].foffset = j1;
2759 vinfos[1].indices[0] = _ij1[0];
2760 vinfos[1].indices[1] = _ij1[1];
2761 vinfos[1].maxsolutions = _nj1;
2762 vinfos[2].jointtype = 1;
2763 vinfos[2].foffset = j2;
2764 vinfos[2].indices[0] = _ij2[0];
2765 vinfos[2].indices[1] = _ij2[1];
2766 vinfos[2].maxsolutions = _nj2;
2767 vinfos[3].jointtype = 1;
2768 vinfos[3].foffset = j3;
2769 vinfos[3].indices[0] = _ij3[0];
2770 vinfos[3].indices[1] = _ij3[1];
2771 vinfos[3].maxsolutions = _nj3;
2772 vinfos[4].jointtype = 1;
2773 vinfos[4].foffset = j4;
2774 vinfos[4].indices[0] = _ij4[0];
2775 vinfos[4].indices[1] = _ij4[1];
2776 vinfos[4].maxsolutions = _nj4;
2777 vinfos[5].jointtype = 1;
2778 vinfos[5].foffset = j5;
2779 vinfos[5].indices[0] = _ij5[0];
2780 vinfos[5].indices[1] = _ij5[1];
2781 vinfos[5].maxsolutions = _nj5;
2782 std::vector<int> vfree(0);
2801 IkReal j1array[1], cj1array[1], sj1array[1];
2802 bool j1valid[1]={
false};
2804 IkReal x591=((cj2)*(sj5));
2805 IkReal x592=((r21)*(sj4));
2806 IkReal x593=((r20)*(sj2));
2807 IkReal x594=((cj5)*(sj4));
2808 IkReal x595=((cj5)*(r21));
2809 IkReal x596=((IkReal(1.00000000000000))*(sj2));
2810 IkReal x597=((cj4)*(r22));
2811 if(
IKabs(((gconst102)*(((((x593)*(x594)))+(((cj2)*(x595)))+(((IkReal(-1.00000000000000))*(x596)*(x597)))+(((r20)*(x591)))+(((IkReal(-1.00000000000000))*(sj5)*(x592)*(x596))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst102)*(((((cj2)*(x597)))+(((sj2)*(x595)))+(((IkReal(-1.00000000000000))*(cj2)*(r20)*(x594)))+(((x591)*(x592)))+(((sj5)*(x593))))))) <
IKFAST_ATAN2_MAGTHRESH )
2813 j1array[0]=
IKatan2(((gconst102)*(((((x593)*(x594)))+(((cj2)*(x595)))+(((IkReal(-1.00000000000000))*(x596)*(x597)))+(((r20)*(x591)))+(((IkReal(-1.00000000000000))*(sj5)*(x592)*(x596)))))), ((gconst102)*(((((cj2)*(x597)))+(((sj2)*(x595)))+(((IkReal(-1.00000000000000))*(cj2)*(r20)*(x594)))+(((x591)*(x592)))+(((sj5)*(x593)))))));
2814 sj1array[0]=
IKsin(j1array[0]);
2815 cj1array[0]=
IKcos(j1array[0]);
2816 if( j1array[0] >
IKPI )
2820 else if( j1array[0] < -
IKPI )
2821 { j1array[0]+=
IK2PI;
2824 for(
int ij1 = 0; ij1 < 1; ++ij1)
2830 _ij1[0] = ij1; _ij1[1] = -1;
2831 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2835 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2838 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2841 IkReal x598=
IKsin(j1);
2842 IkReal x599=
IKcos(j1);
2843 IkReal x600=((IkReal(1.00000000000000))*(x599));
2844 evalcond[0]=((((IkReal(-1.00000000000000))*(sj2)*(x600)))+(((IkReal(-1.00000000000000))*(cj2)*(x598)))+(((r20)*(sj5)))+(((cj5)*(r21))));
2845 evalcond[1]=((((IkReal(-1.00000000000000))*(cj5)*(r20)*(sj4)))+(((IkReal(-1.00000000000000))*(cj2)*(x600)))+(((cj4)*(r22)))+(((r21)*(sj4)*(sj5)))+(((sj2)*(x598))));
2846 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
2853 IkReal dummyeval[1];
2855 IkReal x601=(cj5)*(cj5);
2856 IkReal x602=(sj5)*(sj5);
2857 IkReal x603=((sj4)*(sj5));
2858 IkReal x604=((IkReal(1.00000000000000))*(r12));
2859 IkReal x605=((cj5)*(sj4));
2860 IkReal x606=((cj4)*(r00)*(r11));
2861 IkReal x607=((IkReal(1.00000000000000))*(cj4)*(r01)*(r10));
2862 gconst106=
IKsign(((((x602)*(x606)))+(((r02)*(r10)*(x603)))+(((r02)*(r11)*(x605)))+(((IkReal(-1.00000000000000))*(x601)*(x607)))+(((x601)*(x606)))+(((IkReal(-1.00000000000000))*(x602)*(x607)))+(((IkReal(-1.00000000000000))*(r00)*(x603)*(x604)))+(((IkReal(-1.00000000000000))*(r01)*(x604)*(x605)))));
2863 IkReal x608=(cj5)*(cj5);
2864 IkReal x609=(sj5)*(sj5);
2865 IkReal x610=((sj4)*(sj5));
2866 IkReal x611=((IkReal(1.00000000000000))*(r12));
2867 IkReal x612=((cj5)*(sj4));
2868 IkReal x613=((cj4)*(r00)*(r11));
2870 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(x610)*(x611)))+(((x609)*(x613)))+(((r02)*(r10)*(x610)))+(((x608)*(x613)))+(((IkReal(-1.00000000000000))*(r01)*(x611)*(x612)))+(((r02)*(r11)*(x612)))+(((IkReal(-1.00000000000000))*(x608)*(x614)))+(((IkReal(-1.00000000000000))*(x609)*(x614))));
2871 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2874 IkReal dummyeval[1];
2876 IkReal x615=(cj5)*(cj5);
2877 IkReal x616=(sj5)*(sj5);
2878 IkReal x617=((IkReal(1.00000000000000))*(x616));
2879 IkReal x618=((IkReal(2.00000000000000))*(cj5)*(sj5));
2880 IkReal x619=((IkReal(1.00000000000000))*(x615));
2881 gconst105=
IKsign(((((IkReal(-1.00000000000000))*(x617)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x618)))+(((IkReal(-1.00000000000000))*(x617)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x618)))+(((IkReal(-1.00000000000000))*(x619)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x619)*((r01)*(r01))))));
2882 IkReal x620=(cj5)*(cj5);
2883 IkReal x621=(sj5)*(sj5);
2884 IkReal x622=((IkReal(1.00000000000000))*(x621));
2885 IkReal x623=((IkReal(2.00000000000000))*(cj5)*(sj5));
2886 IkReal x624=((IkReal(1.00000000000000))*(x620));
2887 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r01)*(x623)))+(((IkReal(-1.00000000000000))*(x624)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x622)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x622)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x623)))+(((IkReal(-1.00000000000000))*(x624)*((r01)*(r01)))));
2888 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
2895 IkReal j0array[1], cj0array[1], sj0array[1];
2896 bool j0valid[1]={
false};
2898 IkReal x625=((cj1)*(cj2));
2899 IkReal x626=((r00)*(sj5));
2900 IkReal x627=((cj5)*(r01));
2901 IkReal x628=((r10)*(sj5));
2902 IkReal x629=((cj5)*(r11));
2903 IkReal x630=((IkReal(1.00000000000000))*(sj1)*(sj2));
2904 if(
IKabs(((gconst105)*(((((IkReal(-1.00000000000000))*(x629)*(x630)))+(((x625)*(x628)))+(((x625)*(x629)))+(((IkReal(-1.00000000000000))*(x628)*(x630))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst105)*(((((IkReal(-1.00000000000000))*(x626)*(x630)))+(((IkReal(-1.00000000000000))*(x627)*(x630)))+(((x625)*(x626)))+(((x625)*(x627))))))) <
IKFAST_ATAN2_MAGTHRESH )
2906 j0array[0]=
IKatan2(((gconst105)*(((((IkReal(-1.00000000000000))*(x629)*(x630)))+(((x625)*(x628)))+(((x625)*(x629)))+(((IkReal(-1.00000000000000))*(x628)*(x630)))))), ((gconst105)*(((((IkReal(-1.00000000000000))*(x626)*(x630)))+(((IkReal(-1.00000000000000))*(x627)*(x630)))+(((x625)*(x626)))+(((x625)*(x627)))))));
2907 sj0array[0]=
IKsin(j0array[0]);
2908 cj0array[0]=
IKcos(j0array[0]);
2909 if( j0array[0] >
IKPI )
2913 else if( j0array[0] < -
IKPI )
2914 { j0array[0]+=
IK2PI;
2917 for(
int ij0 = 0; ij0 < 1; ++ij0)
2923 _ij0[0] = ij0; _ij0[1] = -1;
2924 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
2928 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
2931 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
2934 IkReal x631=
IKsin(j0);
2935 IkReal x632=
IKcos(j0);
2936 IkReal x633=((IkReal(1.00000000000000))*(r10));
2937 IkReal x634=((IkReal(1.00000000000000))*(sj4));
2938 IkReal x635=((cj5)*(r01));
2939 IkReal x636=((cj5)*(r00));
2940 IkReal x637=((cj4)*(r01));
2941 IkReal x638=((IkReal(1.00000000000000))*(cj4));
2942 IkReal x639=((cj4)*(r11));
2943 IkReal x640=((sj5)*(x631));
2944 IkReal x641=((r11)*(x634));
2945 IkReal x642=((sj5)*(x632));
2946 IkReal x643=((r02)*(x632));
2947 IkReal x644=((r12)*(x632));
2948 IkReal x645=((IkReal(1.00000000000000))*(x632));
2949 IkReal x646=((cj4)*(x631));
2950 IkReal x647=((cj5)*(x631));
2951 IkReal x648=((cj5)*(sj4)*(x632));
2952 evalcond[0]=((((IkReal(-1.00000000000000))*(cj5)*(r11)*(x645)))+(((r00)*(x640)))+(((x631)*(x635)))+(((IkReal(-1.00000000000000))*(x633)*(x642))));
2953 evalcond[1]=((((IkReal(-1.00000000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x633)*(x640)))+(((sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(r00)*(x642)))+(((IkReal(-1.00000000000000))*(x635)*(x645)))+(((IkReal(-1.00000000000000))*(r11)*(x647))));
2954 evalcond[2]=((((IkReal(-1.00000000000000))*(x638)*(x644)))+(((IkReal(-1.00000000000000))*(x631)*(x634)*(x636)))+(((r01)*(sj4)*(x640)))+(((r10)*(x648)))+(((r02)*(x646)))+(((IkReal(-1.00000000000000))*(x641)*(x642))));
2955 evalcond[3]=((IkReal(1.00000000000000))+(((x639)*(x642)))+(((r02)*(sj4)*(x631)))+(((IkReal(-1.00000000000000))*(x637)*(x640)))+(((IkReal(-1.00000000000000))*(x634)*(x644)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x632)*(x633)))+(((x636)*(x646))));
2956 evalcond[4]=((((cj1)*(sj2)))+(((r10)*(sj4)*(x647)))+(((IkReal(-1.00000000000000))*(x640)*(x641)))+(((IkReal(-1.00000000000000))*(r12)*(x631)*(x638)))+(((IkReal(-1.00000000000000))*(r01)*(x634)*(x642)))+(((sj4)*(x632)*(x636)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x638)*(x643))));
2957 evalcond[5]=((((x639)*(x640)))+(((IkReal(-1.00000000000000))*(x632)*(x636)*(x638)))+(((IkReal(-1.00000000000000))*(cj5)*(x633)*(x646)))+(((IkReal(-1.00000000000000))*(r12)*(x631)*(x634)))+(((x637)*(x642)))+(((IkReal(-1.00000000000000))*(x634)*(x643))));
2958 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
2965 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2966 vinfos[0].jointtype = 1;
2967 vinfos[0].foffset = j0;
2968 vinfos[0].indices[0] = _ij0[0];
2969 vinfos[0].indices[1] = _ij0[1];
2970 vinfos[0].maxsolutions = _nj0;
2971 vinfos[1].jointtype = 1;
2972 vinfos[1].foffset = j1;
2973 vinfos[1].indices[0] = _ij1[0];
2974 vinfos[1].indices[1] = _ij1[1];
2975 vinfos[1].maxsolutions = _nj1;
2976 vinfos[2].jointtype = 1;
2977 vinfos[2].foffset = j2;
2978 vinfos[2].indices[0] = _ij2[0];
2979 vinfos[2].indices[1] = _ij2[1];
2980 vinfos[2].maxsolutions = _nj2;
2981 vinfos[3].jointtype = 1;
2982 vinfos[3].foffset = j3;
2983 vinfos[3].indices[0] = _ij3[0];
2984 vinfos[3].indices[1] = _ij3[1];
2985 vinfos[3].maxsolutions = _nj3;
2986 vinfos[4].jointtype = 1;
2987 vinfos[4].foffset = j4;
2988 vinfos[4].indices[0] = _ij4[0];
2989 vinfos[4].indices[1] = _ij4[1];
2990 vinfos[4].maxsolutions = _nj4;
2991 vinfos[5].jointtype = 1;
2992 vinfos[5].foffset = j5;
2993 vinfos[5].indices[0] = _ij5[0];
2994 vinfos[5].indices[1] = _ij5[1];
2995 vinfos[5].maxsolutions = _nj5;
2996 std::vector<int> vfree(0);
3009 IkReal j0array[1], cj0array[1], sj0array[1];
3010 bool j0valid[1]={
false};
3012 IkReal x649=((IkReal(1.00000000000000))*(cj5));
3013 IkReal x650=((IkReal(1.00000000000000))*(sj5));
3014 if(
IKabs(((gconst106)*(((((IkReal(-1.00000000000000))*(r11)*(x649)))+(((IkReal(-1.00000000000000))*(r10)*(x650))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst106)*(((((IkReal(-1.00000000000000))*(r00)*(x650)))+(((IkReal(-1.00000000000000))*(r01)*(x649))))))) <
IKFAST_ATAN2_MAGTHRESH )
3016 j0array[0]=
IKatan2(((gconst106)*(((((IkReal(-1.00000000000000))*(r11)*(x649)))+(((IkReal(-1.00000000000000))*(r10)*(x650)))))), ((gconst106)*(((((IkReal(-1.00000000000000))*(r00)*(x650)))+(((IkReal(-1.00000000000000))*(r01)*(x649)))))));
3017 sj0array[0]=
IKsin(j0array[0]);
3018 cj0array[0]=
IKcos(j0array[0]);
3019 if( j0array[0] >
IKPI )
3023 else if( j0array[0] < -
IKPI )
3024 { j0array[0]+=
IK2PI;
3027 for(
int ij0 = 0; ij0 < 1; ++ij0)
3033 _ij0[0] = ij0; _ij0[1] = -1;
3034 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
3038 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
3041 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
3044 IkReal x651=
IKsin(j0);
3045 IkReal x652=
IKcos(j0);
3046 IkReal x653=((IkReal(1.00000000000000))*(r10));
3047 IkReal x654=((IkReal(1.00000000000000))*(sj4));
3048 IkReal x655=((cj5)*(r01));
3049 IkReal x656=((cj5)*(r00));
3050 IkReal x657=((cj4)*(r01));
3051 IkReal x658=((IkReal(1.00000000000000))*(cj4));
3052 IkReal x659=((cj4)*(r11));
3053 IkReal x660=((sj5)*(x651));
3054 IkReal x661=((r11)*(x654));
3055 IkReal x662=((sj5)*(x652));
3056 IkReal x663=((r02)*(x652));
3057 IkReal x664=((r12)*(x652));
3058 IkReal x665=((IkReal(1.00000000000000))*(x652));
3059 IkReal x666=((cj4)*(x651));
3060 IkReal x667=((cj5)*(x651));
3061 IkReal x668=((cj5)*(sj4)*(x652));
3062 evalcond[0]=((((x651)*(x655)))+(((r00)*(x660)))+(((IkReal(-1.00000000000000))*(x653)*(x662)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x665))));
3063 evalcond[1]=((((IkReal(-1.00000000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(r00)*(x662)))+(((IkReal(-1.00000000000000))*(x655)*(x665)))+(((IkReal(-1.00000000000000))*(x653)*(x660)))+(((sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(r11)*(x667))));
3064 evalcond[2]=((((r01)*(sj4)*(x660)))+(((r02)*(x666)))+(((IkReal(-1.00000000000000))*(x661)*(x662)))+(((IkReal(-1.00000000000000))*(x651)*(x654)*(x656)))+(((r10)*(x668)))+(((IkReal(-1.00000000000000))*(x658)*(x664))));
3065 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x652)*(x653)))+(((IkReal(-1.00000000000000))*(x654)*(x664)))+(((r02)*(sj4)*(x651)))+(((IkReal(-1.00000000000000))*(x657)*(x660)))+(((x659)*(x662)))+(((x656)*(x666))));
3066 evalcond[4]=((((cj1)*(sj2)))+(((r10)*(sj4)*(x667)))+(((IkReal(-1.00000000000000))*(r01)*(x654)*(x662)))+(((IkReal(-1.00000000000000))*(x660)*(x661)))+(((IkReal(-1.00000000000000))*(r12)*(x651)*(x658)))+(((sj4)*(x652)*(x656)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x658)*(x663))));
3067 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x653)*(x666)))+(((IkReal(-1.00000000000000))*(x652)*(x656)*(x658)))+(((IkReal(-1.00000000000000))*(r12)*(x651)*(x654)))+(((x659)*(x660)))+(((x657)*(x662)))+(((IkReal(-1.00000000000000))*(x654)*(x663))));
3068 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
3075 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3076 vinfos[0].jointtype = 1;
3077 vinfos[0].foffset = j0;
3078 vinfos[0].indices[0] = _ij0[0];
3079 vinfos[0].indices[1] = _ij0[1];
3080 vinfos[0].maxsolutions = _nj0;
3081 vinfos[1].jointtype = 1;
3082 vinfos[1].foffset = j1;
3083 vinfos[1].indices[0] = _ij1[0];
3084 vinfos[1].indices[1] = _ij1[1];
3085 vinfos[1].maxsolutions = _nj1;
3086 vinfos[2].jointtype = 1;
3087 vinfos[2].foffset = j2;
3088 vinfos[2].indices[0] = _ij2[0];
3089 vinfos[2].indices[1] = _ij2[1];
3090 vinfos[2].maxsolutions = _nj2;
3091 vinfos[3].jointtype = 1;
3092 vinfos[3].foffset = j3;
3093 vinfos[3].indices[0] = _ij3[0];
3094 vinfos[3].indices[1] = _ij3[1];
3095 vinfos[3].maxsolutions = _nj3;
3096 vinfos[4].jointtype = 1;
3097 vinfos[4].foffset = j4;
3098 vinfos[4].indices[0] = _ij4[0];
3099 vinfos[4].indices[1] = _ij4[1];
3100 vinfos[4].maxsolutions = _nj4;
3101 vinfos[5].jointtype = 1;
3102 vinfos[5].foffset = j5;
3103 vinfos[5].indices[0] = _ij5[0];
3104 vinfos[5].indices[1] = _ij5[1];
3105 vinfos[5].maxsolutions = _nj5;
3106 std::vector<int> vfree(0);
3124 IkReal x669=((cj5)*(npx));
3125 IkReal x670=((IkReal(1.00000000000000))*(cj4)*(sj5));
3126 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(4.71238898038469))+(j3)), IkReal(6.28318530717959))));
3127 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(0.350000000000000))*(sj2)))+(((npx)*(sj5))));
3128 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((IkReal(-1.00000000000000))*(sj4)*(x669)))+(((npy)*(sj4)*(sj5))));
3129 evalcond[3]=((((cj4)*(x669)))+(((IkReal(-1.00000000000000))*(npy)*(x670)))+(((npz)*(sj4))));
3130 evalcond[4]=((((cj4)*(cj5)*(r20)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(r21)*(x670))));
3131 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 )
3134 IkReal dummyeval[1];
3136 gconst109=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
3137 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
3138 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3141 IkReal dummyeval[1];
3143 IkReal x671=(sj5)*(sj5);
3144 IkReal x672=(cj5)*(cj5);
3145 IkReal x673=((cj5)*(sj4));
3146 IkReal x674=((IkReal(1.00000000000000))*(r02));
3147 IkReal x675=((sj4)*(sj5));
3148 IkReal x676=((cj4)*(r01)*(r10));
3149 IkReal x677=((IkReal(1.00000000000000))*(cj4)*(r00)*(r11));
3150 gconst107=
IKsign(((((r00)*(r12)*(x675)))+(((x672)*(x676)))+(((IkReal(-1.00000000000000))*(r10)*(x674)*(x675)))+(((IkReal(-1.00000000000000))*(x672)*(x677)))+(((x671)*(x676)))+(((IkReal(-1.00000000000000))*(r11)*(x673)*(x674)))+(((IkReal(-1.00000000000000))*(x671)*(x677)))+(((r01)*(r12)*(x673)))));
3151 IkReal x678=(sj5)*(sj5);
3152 IkReal x679=(cj5)*(cj5);
3153 IkReal x680=((cj5)*(sj4));
3154 IkReal x681=((IkReal(1.00000000000000))*(r02));
3155 IkReal x682=((sj4)*(sj5));
3156 IkReal x683=((cj4)*(r01)*(r10));
3158 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(x681)*(x682)))+(((r00)*(r12)*(x682)))+(((IkReal(-1.00000000000000))*(x679)*(x684)))+(((IkReal(-1.00000000000000))*(x678)*(x684)))+(((IkReal(-1.00000000000000))*(r11)*(x680)*(x681)))+(((x679)*(x683)))+(((r01)*(r12)*(x680)))+(((x678)*(x683))));
3159 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3162 IkReal dummyeval[1];
3164 IkReal x685=(cj4)*(cj4);
3165 IkReal x686=(sj4)*(sj4);
3166 IkReal x687=((cj5)*(r10));
3167 IkReal x688=((r01)*(sj5));
3168 IkReal x689=((r12)*(x686));
3169 IkReal x690=((IkReal(1.00000000000000))*(cj5)*(r00));
3170 IkReal x691=((r02)*(x685));
3171 IkReal x692=((IkReal(1.00000000000000))*(r11)*(sj5));
3172 IkReal x693=((r12)*(x685));
3173 IkReal x694=((r02)*(x686));
3174 gconst108=
IKsign(((((x687)*(x694)))+(((x688)*(x689)))+(((IkReal(-1.00000000000000))*(x692)*(x694)))+(((IkReal(-1.00000000000000))*(x689)*(x690)))+(((x687)*(x691)))+(((x688)*(x693)))+(((IkReal(-1.00000000000000))*(x691)*(x692)))+(((IkReal(-1.00000000000000))*(x690)*(x693)))));
3175 IkReal x695=(cj4)*(cj4);
3176 IkReal x696=(sj4)*(sj4);
3177 IkReal x697=((cj5)*(r10));
3178 IkReal x698=((r01)*(sj5));
3179 IkReal x699=((r12)*(x696));
3180 IkReal x700=((IkReal(1.00000000000000))*(cj5)*(r00));
3181 IkReal x701=((r02)*(x695));
3182 IkReal x702=((IkReal(1.00000000000000))*(r11)*(sj5));
3183 IkReal x703=((r12)*(x695));
3184 IkReal x704=((r02)*(x696));
3185 dummyeval[0]=((((IkReal(-1.00000000000000))*(x700)*(x703)))+(((x698)*(x699)))+(((x697)*(x704)))+(((IkReal(-1.00000000000000))*(x701)*(x702)))+(((x697)*(x701)))+(((IkReal(-1.00000000000000))*(x699)*(x700)))+(((IkReal(-1.00000000000000))*(x702)*(x704)))+(((x698)*(x703))));
3186 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3193 IkReal j0array[1], cj0array[1], sj0array[1];
3194 bool j0valid[1]={
false};
3196 IkReal x705=((cj5)*(sj4));
3197 IkReal x706=((IkReal(1.00000000000000))*(cj4));
3198 IkReal x707=((IkReal(1.00000000000000))*(sj4)*(sj5));
3199 if(
IKabs(((gconst108)*(((((IkReal(-1.00000000000000))*(r11)*(x707)))+(((IkReal(-1.00000000000000))*(r12)*(x706)))+(((r10)*(x705))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst108)*(((((r00)*(x705)))+(((IkReal(-1.00000000000000))*(r01)*(x707)))+(((IkReal(-1.00000000000000))*(r02)*(x706))))))) <
IKFAST_ATAN2_MAGTHRESH )
3201 j0array[0]=
IKatan2(((gconst108)*(((((IkReal(-1.00000000000000))*(r11)*(x707)))+(((IkReal(-1.00000000000000))*(r12)*(x706)))+(((r10)*(x705)))))), ((gconst108)*(((((r00)*(x705)))+(((IkReal(-1.00000000000000))*(r01)*(x707)))+(((IkReal(-1.00000000000000))*(r02)*(x706)))))));
3202 sj0array[0]=
IKsin(j0array[0]);
3203 cj0array[0]=
IKcos(j0array[0]);
3204 if( j0array[0] >
IKPI )
3208 else if( j0array[0] < -
IKPI )
3209 { j0array[0]+=
IK2PI;
3212 for(
int ij0 = 0; ij0 < 1; ++ij0)
3218 _ij0[0] = ij0; _ij0[1] = -1;
3219 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
3223 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
3226 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
3229 IkReal x708=
IKsin(j0);
3230 IkReal x709=
IKcos(j0);
3231 IkReal x710=((IkReal(1.00000000000000))*(sj5));
3232 IkReal x711=((cj5)*(r10));
3233 IkReal x712=((IkReal(1.00000000000000))*(r12));
3234 IkReal x713=((cj4)*(cj5));
3235 IkReal x714=((r11)*(sj5));
3236 IkReal x715=((IkReal(1.00000000000000))*(cj5));
3237 IkReal x716=((r00)*(x708));
3238 IkReal x717=((sj4)*(x709));
3239 IkReal x718=((r01)*(x708));
3240 IkReal x719=((sj4)*(x708));
3241 IkReal x720=((cj4)*(x708));
3242 IkReal x721=((cj4)*(x709));
3243 evalcond[0]=((((IkReal(-1.00000000000000))*(r10)*(x709)*(x710)))+(((IkReal(-1.00000000000000))*(r11)*(x709)*(x715)))+(((cj5)*(x718)))+(((sj5)*(x716))));
3244 evalcond[1]=((((IkReal(-1.00000000000000))*(x712)*(x721)))+(((sj4)*(sj5)*(x718)))+(((IkReal(-1.00000000000000))*(r11)*(x710)*(x717)))+(((x711)*(x717)))+(((IkReal(-1.00000000000000))*(sj4)*(x715)*(x716)))+(((r02)*(x720))));
3245 evalcond[2]=((IkReal(-1.00000000000000))+(((r02)*(x719)))+(((IkReal(-1.00000000000000))*(x711)*(x721)))+(((IkReal(-1.00000000000000))*(x712)*(x717)))+(((x713)*(x716)))+(((IkReal(-1.00000000000000))*(cj4)*(x710)*(x718)))+(((x714)*(x721))));
3246 evalcond[3]=((((IkReal(-1.00000000000000))*(x712)*(x719)))+(((IkReal(-1.00000000000000))*(x711)*(x720)))+(((IkReal(-1.00000000000000))*(r02)*(x717)))+(((x714)*(x720)))+(((r01)*(sj5)*(x721)))+(((IkReal(-1.00000000000000))*(r00)*(x709)*(x713))));
3247 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3254 IkReal dummyeval[1];
3256 gconst110=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
3257 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
3258 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3261 IkReal dummyeval[1];
3263 gconst111=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
3264 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
3265 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3272 IkReal j1array[1], cj1array[1], sj1array[1];
3273 bool j1valid[1]={
false};
3275 IkReal x722=((IkReal(1.00000000000000))*(sj2));
3276 IkReal x723=((r20)*(sj5));
3277 IkReal x724=((IkReal(1.00000000000000))*(cj2));
3278 IkReal x725=((cj5)*(r21));
3279 IkReal x726=((cj5)*(r11)*(sj0));
3280 IkReal x727=((r10)*(sj0)*(sj5));
3281 IkReal x728=((cj0)*(r00)*(sj5));
3282 IkReal x729=((cj0)*(cj5)*(r01));
3283 if(
IKabs(((gconst111)*(((((IkReal(-1.00000000000000))*(x723)*(x724)))+(((IkReal(-1.00000000000000))*(x722)*(x726)))+(((IkReal(-1.00000000000000))*(x724)*(x725)))+(((IkReal(-1.00000000000000))*(x722)*(x728)))+(((IkReal(-1.00000000000000))*(x722)*(x727)))+(((IkReal(-1.00000000000000))*(x722)*(x729))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst111)*(((((cj2)*(x727)))+(((cj2)*(x726)))+(((cj2)*(x729)))+(((IkReal(-1.00000000000000))*(x722)*(x723)))+(((IkReal(-1.00000000000000))*(x722)*(x725)))+(((cj2)*(x728))))))) <
IKFAST_ATAN2_MAGTHRESH )
3285 j1array[0]=
IKatan2(((gconst111)*(((((IkReal(-1.00000000000000))*(x723)*(x724)))+(((IkReal(-1.00000000000000))*(x722)*(x726)))+(((IkReal(-1.00000000000000))*(x724)*(x725)))+(((IkReal(-1.00000000000000))*(x722)*(x728)))+(((IkReal(-1.00000000000000))*(x722)*(x727)))+(((IkReal(-1.00000000000000))*(x722)*(x729)))))), ((gconst111)*(((((cj2)*(x727)))+(((cj2)*(x726)))+(((cj2)*(x729)))+(((IkReal(-1.00000000000000))*(x722)*(x723)))+(((IkReal(-1.00000000000000))*(x722)*(x725)))+(((cj2)*(x728)))))));
3286 sj1array[0]=
IKsin(j1array[0]);
3287 cj1array[0]=
IKcos(j1array[0]);
3288 if( j1array[0] >
IKPI )
3292 else if( j1array[0] < -
IKPI )
3293 { j1array[0]+=
IK2PI;
3296 for(
int ij1 = 0; ij1 < 1; ++ij1)
3302 _ij1[0] = ij1; _ij1[1] = -1;
3303 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3307 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3310 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3313 IkReal x730=
IKsin(j1);
3314 IkReal x731=
IKcos(j1);
3315 IkReal x732=((cj5)*(sj0));
3316 IkReal x733=((IkReal(1.00000000000000))*(r11));
3317 IkReal x734=((cj5)*(sj4));
3318 IkReal x735=((cj0)*(r00));
3319 IkReal x736=((IkReal(1.00000000000000))*(sj0));
3320 IkReal x737=((sj4)*(sj5));
3321 IkReal x738=((IkReal(1.00000000000000))*(cj0));
3322 IkReal x739=((sj2)*(x731));
3323 IkReal x740=((cj2)*(x730));
3324 IkReal x741=((cj2)*(x731));
3325 IkReal x742=((sj2)*(x730));
3326 IkReal x743=((x740)+(x739));
3327 evalcond[0]=((x743)+(((r20)*(sj5)))+(((cj5)*(r21))));
3328 evalcond[1]=((((IkReal(-1.00000000000000))*(x741)))+(((IkReal(-1.00000000000000))*(r20)*(x734)))+(((r21)*(x737)))+(x742)+(((cj4)*(r22))));
3329 evalcond[2]=((x741)+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x738)))+(((IkReal(-1.00000000000000))*(sj5)*(x735)))+(((IkReal(-1.00000000000000))*(x742)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x736)))+(((IkReal(-1.00000000000000))*(x732)*(x733))));
3330 evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(r02)*(x738)))+(x743)+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x736)))+(((IkReal(-1.00000000000000))*(sj0)*(x733)*(x737)))+(((x734)*(x735)))+(((IkReal(-1.00000000000000))*(r01)*(x737)*(x738)))+(((r10)*(sj4)*(x732))));
3331 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3338 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3339 vinfos[0].jointtype = 1;
3340 vinfos[0].foffset = j0;
3341 vinfos[0].indices[0] = _ij0[0];
3342 vinfos[0].indices[1] = _ij0[1];
3343 vinfos[0].maxsolutions = _nj0;
3344 vinfos[1].jointtype = 1;
3345 vinfos[1].foffset = j1;
3346 vinfos[1].indices[0] = _ij1[0];
3347 vinfos[1].indices[1] = _ij1[1];
3348 vinfos[1].maxsolutions = _nj1;
3349 vinfos[2].jointtype = 1;
3350 vinfos[2].foffset = j2;
3351 vinfos[2].indices[0] = _ij2[0];
3352 vinfos[2].indices[1] = _ij2[1];
3353 vinfos[2].maxsolutions = _nj2;
3354 vinfos[3].jointtype = 1;
3355 vinfos[3].foffset = j3;
3356 vinfos[3].indices[0] = _ij3[0];
3357 vinfos[3].indices[1] = _ij3[1];
3358 vinfos[3].maxsolutions = _nj3;
3359 vinfos[4].jointtype = 1;
3360 vinfos[4].foffset = j4;
3361 vinfos[4].indices[0] = _ij4[0];
3362 vinfos[4].indices[1] = _ij4[1];
3363 vinfos[4].maxsolutions = _nj4;
3364 vinfos[5].jointtype = 1;
3365 vinfos[5].foffset = j5;
3366 vinfos[5].indices[0] = _ij5[0];
3367 vinfos[5].indices[1] = _ij5[1];
3368 vinfos[5].maxsolutions = _nj5;
3369 std::vector<int> vfree(0);
3382 IkReal j1array[1], cj1array[1], sj1array[1];
3383 bool j1valid[1]={
false};
3385 IkReal x744=((cj4)*(r22));
3386 IkReal x745=((r20)*(sj5));
3387 IkReal x746=((cj5)*(r21));
3388 IkReal x747=((sj2)*(sj4));
3389 IkReal x748=((r21)*(sj5));
3390 IkReal x749=((IkReal(1.00000000000000))*(cj2));
3391 IkReal x750=((cj5)*(r20));
3392 if(
IKabs(((gconst110)*(((((IkReal(-1.00000000000000))*(x747)*(x750)))+(((sj2)*(x744)))+(((x747)*(x748)))+(((cj2)*(x745)))+(((cj2)*(x746))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst110)*(((((cj2)*(sj4)*(x750)))+(((sj2)*(x746)))+(((IkReal(-1.00000000000000))*(sj4)*(x748)*(x749)))+(((IkReal(-1.00000000000000))*(x744)*(x749)))+(((sj2)*(x745))))))) <
IKFAST_ATAN2_MAGTHRESH )
3394 j1array[0]=
IKatan2(((gconst110)*(((((IkReal(-1.00000000000000))*(x747)*(x750)))+(((sj2)*(x744)))+(((x747)*(x748)))+(((cj2)*(x745)))+(((cj2)*(x746)))))), ((gconst110)*(((((cj2)*(sj4)*(x750)))+(((sj2)*(x746)))+(((IkReal(-1.00000000000000))*(sj4)*(x748)*(x749)))+(((IkReal(-1.00000000000000))*(x744)*(x749)))+(((sj2)*(x745)))))));
3395 sj1array[0]=
IKsin(j1array[0]);
3396 cj1array[0]=
IKcos(j1array[0]);
3397 if( j1array[0] >
IKPI )
3401 else if( j1array[0] < -
IKPI )
3402 { j1array[0]+=
IK2PI;
3405 for(
int ij1 = 0; ij1 < 1; ++ij1)
3411 _ij1[0] = ij1; _ij1[1] = -1;
3412 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3416 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3419 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3422 IkReal x751=
IKsin(j1);
3423 IkReal x752=
IKcos(j1);
3424 IkReal x753=((cj5)*(sj0));
3425 IkReal x754=((IkReal(1.00000000000000))*(r11));
3426 IkReal x755=((cj5)*(sj4));
3427 IkReal x756=((cj0)*(r00));
3428 IkReal x757=((IkReal(1.00000000000000))*(sj0));
3429 IkReal x758=((sj4)*(sj5));
3430 IkReal x759=((IkReal(1.00000000000000))*(cj0));
3431 IkReal x760=((sj2)*(x752));
3432 IkReal x761=((cj2)*(x751));
3433 IkReal x762=((cj2)*(x752));
3434 IkReal x763=((sj2)*(x751));
3435 IkReal x764=((x760)+(x761));
3436 evalcond[0]=((x764)+(((r20)*(sj5)))+(((cj5)*(r21))));
3437 evalcond[1]=((((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x762)))+(x763)+(((IkReal(-1.00000000000000))*(r20)*(x755)))+(((r21)*(x758))));
3438 evalcond[2]=((((IkReal(-1.00000000000000))*(cj5)*(r01)*(x759)))+(((IkReal(-1.00000000000000))*(x753)*(x754)))+(x762)+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x757)))+(((IkReal(-1.00000000000000))*(x763)))+(((IkReal(-1.00000000000000))*(sj5)*(x756))));
3439 evalcond[3]=((((IkReal(-1.00000000000000))*(sj0)*(x754)*(x758)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x759)))+(((r10)*(sj4)*(x753)))+(x764)+(((IkReal(-1.00000000000000))*(r01)*(x758)*(x759)))+(((x755)*(x756)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x757))));
3440 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3447 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3448 vinfos[0].jointtype = 1;
3449 vinfos[0].foffset = j0;
3450 vinfos[0].indices[0] = _ij0[0];
3451 vinfos[0].indices[1] = _ij0[1];
3452 vinfos[0].maxsolutions = _nj0;
3453 vinfos[1].jointtype = 1;
3454 vinfos[1].foffset = j1;
3455 vinfos[1].indices[0] = _ij1[0];
3456 vinfos[1].indices[1] = _ij1[1];
3457 vinfos[1].maxsolutions = _nj1;
3458 vinfos[2].jointtype = 1;
3459 vinfos[2].foffset = j2;
3460 vinfos[2].indices[0] = _ij2[0];
3461 vinfos[2].indices[1] = _ij2[1];
3462 vinfos[2].maxsolutions = _nj2;
3463 vinfos[3].jointtype = 1;
3464 vinfos[3].foffset = j3;
3465 vinfos[3].indices[0] = _ij3[0];
3466 vinfos[3].indices[1] = _ij3[1];
3467 vinfos[3].maxsolutions = _nj3;
3468 vinfos[4].jointtype = 1;
3469 vinfos[4].foffset = j4;
3470 vinfos[4].indices[0] = _ij4[0];
3471 vinfos[4].indices[1] = _ij4[1];
3472 vinfos[4].maxsolutions = _nj4;
3473 vinfos[5].jointtype = 1;
3474 vinfos[5].foffset = j5;
3475 vinfos[5].indices[0] = _ij5[0];
3476 vinfos[5].indices[1] = _ij5[1];
3477 vinfos[5].maxsolutions = _nj5;
3478 std::vector<int> vfree(0);
3497 IkReal j0array[1], cj0array[1], sj0array[1];
3498 bool j0valid[1]={
false};
3500 IkReal x765=((IkReal(1.00000000000000))*(cj5));
3501 IkReal x766=((IkReal(1.00000000000000))*(sj5));
3502 if(
IKabs(((gconst107)*(((((IkReal(-1.00000000000000))*(r11)*(x765)))+(((IkReal(-1.00000000000000))*(r10)*(x766))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst107)*(((((IkReal(-1.00000000000000))*(r01)*(x765)))+(((IkReal(-1.00000000000000))*(r00)*(x766))))))) <
IKFAST_ATAN2_MAGTHRESH )
3504 j0array[0]=
IKatan2(((gconst107)*(((((IkReal(-1.00000000000000))*(r11)*(x765)))+(((IkReal(-1.00000000000000))*(r10)*(x766)))))), ((gconst107)*(((((IkReal(-1.00000000000000))*(r01)*(x765)))+(((IkReal(-1.00000000000000))*(r00)*(x766)))))));
3505 sj0array[0]=
IKsin(j0array[0]);
3506 cj0array[0]=
IKcos(j0array[0]);
3507 if( j0array[0] >
IKPI )
3511 else if( j0array[0] < -
IKPI )
3512 { j0array[0]+=
IK2PI;
3515 for(
int ij0 = 0; ij0 < 1; ++ij0)
3521 _ij0[0] = ij0; _ij0[1] = -1;
3522 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
3526 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
3529 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
3532 IkReal x767=
IKsin(j0);
3533 IkReal x768=
IKcos(j0);
3534 IkReal x769=((IkReal(1.00000000000000))*(sj5));
3535 IkReal x770=((cj5)*(r10));
3536 IkReal x771=((IkReal(1.00000000000000))*(r12));
3537 IkReal x772=((cj4)*(cj5));
3538 IkReal x773=((r11)*(sj5));
3539 IkReal x774=((IkReal(1.00000000000000))*(cj5));
3540 IkReal x775=((r00)*(x767));
3541 IkReal x776=((sj4)*(x768));
3542 IkReal x777=((r01)*(x767));
3543 IkReal x778=((sj4)*(x767));
3544 IkReal x779=((cj4)*(x767));
3545 IkReal x780=((cj4)*(x768));
3546 evalcond[0]=((((IkReal(-1.00000000000000))*(r11)*(x768)*(x774)))+(((cj5)*(x777)))+(((IkReal(-1.00000000000000))*(r10)*(x768)*(x769)))+(((sj5)*(x775))));
3547 evalcond[1]=((((r02)*(x779)))+(((IkReal(-1.00000000000000))*(r11)*(x769)*(x776)))+(((sj4)*(sj5)*(x777)))+(((IkReal(-1.00000000000000))*(sj4)*(x774)*(x775)))+(((x770)*(x776)))+(((IkReal(-1.00000000000000))*(x771)*(x780))));
3548 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x771)*(x776)))+(((x773)*(x780)))+(((IkReal(-1.00000000000000))*(x770)*(x780)))+(((IkReal(-1.00000000000000))*(cj4)*(x769)*(x777)))+(((x772)*(x775)))+(((r02)*(x778))));
3549 evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x776)))+(((IkReal(-1.00000000000000))*(r00)*(x768)*(x772)))+(((IkReal(-1.00000000000000))*(x771)*(x778)))+(((IkReal(-1.00000000000000))*(x770)*(x779)))+(((r01)*(sj5)*(x780)))+(((x773)*(x779))));
3550 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3557 IkReal dummyeval[1];
3559 gconst110=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
3560 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
3561 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3564 IkReal dummyeval[1];
3566 gconst111=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
3567 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
3568 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3575 IkReal j1array[1], cj1array[1], sj1array[1];
3576 bool j1valid[1]={
false};
3578 IkReal x781=((IkReal(1.00000000000000))*(sj2));
3579 IkReal x782=((r20)*(sj5));
3580 IkReal x783=((IkReal(1.00000000000000))*(cj2));
3581 IkReal x784=((cj5)*(r21));
3582 IkReal x785=((cj5)*(r11)*(sj0));
3583 IkReal x786=((r10)*(sj0)*(sj5));
3584 IkReal x787=((cj0)*(r00)*(sj5));
3585 IkReal x788=((cj0)*(cj5)*(r01));
3586 if(
IKabs(((gconst111)*(((((IkReal(-1.00000000000000))*(x783)*(x784)))+(((IkReal(-1.00000000000000))*(x781)*(x788)))+(((IkReal(-1.00000000000000))*(x781)*(x785)))+(((IkReal(-1.00000000000000))*(x781)*(x786)))+(((IkReal(-1.00000000000000))*(x782)*(x783)))+(((IkReal(-1.00000000000000))*(x781)*(x787))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst111)*(((((IkReal(-1.00000000000000))*(x781)*(x784)))+(((IkReal(-1.00000000000000))*(x781)*(x782)))+(((cj2)*(x785)))+(((cj2)*(x786)))+(((cj2)*(x788)))+(((cj2)*(x787))))))) <
IKFAST_ATAN2_MAGTHRESH )
3588 j1array[0]=
IKatan2(((gconst111)*(((((IkReal(-1.00000000000000))*(x783)*(x784)))+(((IkReal(-1.00000000000000))*(x781)*(x788)))+(((IkReal(-1.00000000000000))*(x781)*(x785)))+(((IkReal(-1.00000000000000))*(x781)*(x786)))+(((IkReal(-1.00000000000000))*(x782)*(x783)))+(((IkReal(-1.00000000000000))*(x781)*(x787)))))), ((gconst111)*(((((IkReal(-1.00000000000000))*(x781)*(x784)))+(((IkReal(-1.00000000000000))*(x781)*(x782)))+(((cj2)*(x785)))+(((cj2)*(x786)))+(((cj2)*(x788)))+(((cj2)*(x787)))))));
3589 sj1array[0]=
IKsin(j1array[0]);
3590 cj1array[0]=
IKcos(j1array[0]);
3591 if( j1array[0] >
IKPI )
3595 else if( j1array[0] < -
IKPI )
3596 { j1array[0]+=
IK2PI;
3599 for(
int ij1 = 0; ij1 < 1; ++ij1)
3605 _ij1[0] = ij1; _ij1[1] = -1;
3606 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3610 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3613 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3616 IkReal x789=
IKsin(j1);
3617 IkReal x790=
IKcos(j1);
3618 IkReal x791=((cj5)*(sj0));
3619 IkReal x792=((IkReal(1.00000000000000))*(r11));
3620 IkReal x793=((cj5)*(sj4));
3621 IkReal x794=((cj0)*(r00));
3622 IkReal x795=((IkReal(1.00000000000000))*(sj0));
3623 IkReal x796=((sj4)*(sj5));
3624 IkReal x797=((IkReal(1.00000000000000))*(cj0));
3625 IkReal x798=((sj2)*(x790));
3626 IkReal x799=((cj2)*(x789));
3627 IkReal x800=((cj2)*(x790));
3628 IkReal x801=((sj2)*(x789));
3629 IkReal x802=((x799)+(x798));
3630 evalcond[0]=((x802)+(((r20)*(sj5)))+(((cj5)*(r21))));
3631 evalcond[1]=((x801)+(((IkReal(-1.00000000000000))*(r20)*(x793)))+(((IkReal(-1.00000000000000))*(x800)))+(((cj4)*(r22)))+(((r21)*(x796))));
3632 evalcond[2]=((x800)+(((IkReal(-1.00000000000000))*(x801)))+(((IkReal(-1.00000000000000))*(x791)*(x792)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x797)))+(((IkReal(-1.00000000000000))*(sj5)*(x794)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x795))));
3633 evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(r02)*(x797)))+(x802)+(((IkReal(-1.00000000000000))*(sj0)*(x792)*(x796)))+(((IkReal(-1.00000000000000))*(r01)*(x796)*(x797)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x795)))+(((x793)*(x794)))+(((r10)*(sj4)*(x791))));
3634 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3641 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3642 vinfos[0].jointtype = 1;
3643 vinfos[0].foffset = j0;
3644 vinfos[0].indices[0] = _ij0[0];
3645 vinfos[0].indices[1] = _ij0[1];
3646 vinfos[0].maxsolutions = _nj0;
3647 vinfos[1].jointtype = 1;
3648 vinfos[1].foffset = j1;
3649 vinfos[1].indices[0] = _ij1[0];
3650 vinfos[1].indices[1] = _ij1[1];
3651 vinfos[1].maxsolutions = _nj1;
3652 vinfos[2].jointtype = 1;
3653 vinfos[2].foffset = j2;
3654 vinfos[2].indices[0] = _ij2[0];
3655 vinfos[2].indices[1] = _ij2[1];
3656 vinfos[2].maxsolutions = _nj2;
3657 vinfos[3].jointtype = 1;
3658 vinfos[3].foffset = j3;
3659 vinfos[3].indices[0] = _ij3[0];
3660 vinfos[3].indices[1] = _ij3[1];
3661 vinfos[3].maxsolutions = _nj3;
3662 vinfos[4].jointtype = 1;
3663 vinfos[4].foffset = j4;
3664 vinfos[4].indices[0] = _ij4[0];
3665 vinfos[4].indices[1] = _ij4[1];
3666 vinfos[4].maxsolutions = _nj4;
3667 vinfos[5].jointtype = 1;
3668 vinfos[5].foffset = j5;
3669 vinfos[5].indices[0] = _ij5[0];
3670 vinfos[5].indices[1] = _ij5[1];
3671 vinfos[5].maxsolutions = _nj5;
3672 std::vector<int> vfree(0);
3685 IkReal j1array[1], cj1array[1], sj1array[1];
3686 bool j1valid[1]={
false};
3688 IkReal x803=((cj4)*(r22));
3689 IkReal x804=((r20)*(sj5));
3690 IkReal x805=((cj5)*(r21));
3691 IkReal x806=((sj2)*(sj4));
3692 IkReal x807=((r21)*(sj5));
3693 IkReal x808=((IkReal(1.00000000000000))*(cj2));
3694 IkReal x809=((cj5)*(r20));
3695 if(
IKabs(((gconst110)*(((((cj2)*(x804)))+(((IkReal(-1.00000000000000))*(x806)*(x809)))+(((cj2)*(x805)))+(((sj2)*(x803)))+(((x806)*(x807))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst110)*(((((cj2)*(sj4)*(x809)))+(((sj2)*(x804)))+(((sj2)*(x805)))+(((IkReal(-1.00000000000000))*(x803)*(x808)))+(((IkReal(-1.00000000000000))*(sj4)*(x807)*(x808))))))) <
IKFAST_ATAN2_MAGTHRESH )
3697 j1array[0]=
IKatan2(((gconst110)*(((((cj2)*(x804)))+(((IkReal(-1.00000000000000))*(x806)*(x809)))+(((cj2)*(x805)))+(((sj2)*(x803)))+(((x806)*(x807)))))), ((gconst110)*(((((cj2)*(sj4)*(x809)))+(((sj2)*(x804)))+(((sj2)*(x805)))+(((IkReal(-1.00000000000000))*(x803)*(x808)))+(((IkReal(-1.00000000000000))*(sj4)*(x807)*(x808)))))));
3698 sj1array[0]=
IKsin(j1array[0]);
3699 cj1array[0]=
IKcos(j1array[0]);
3700 if( j1array[0] >
IKPI )
3704 else if( j1array[0] < -
IKPI )
3705 { j1array[0]+=
IK2PI;
3708 for(
int ij1 = 0; ij1 < 1; ++ij1)
3714 _ij1[0] = ij1; _ij1[1] = -1;
3715 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3719 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3722 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3725 IkReal x810=
IKsin(j1);
3726 IkReal x811=
IKcos(j1);
3727 IkReal x812=((cj5)*(sj0));
3728 IkReal x813=((IkReal(1.00000000000000))*(r11));
3729 IkReal x814=((cj5)*(sj4));
3730 IkReal x815=((cj0)*(r00));
3731 IkReal x816=((IkReal(1.00000000000000))*(sj0));
3732 IkReal x817=((sj4)*(sj5));
3733 IkReal x818=((IkReal(1.00000000000000))*(cj0));
3734 IkReal x819=((sj2)*(x811));
3735 IkReal x820=((cj2)*(x810));
3736 IkReal x821=((cj2)*(x811));
3737 IkReal x822=((sj2)*(x810));
3738 IkReal x823=((x820)+(x819));
3739 evalcond[0]=((x823)+(((r20)*(sj5)))+(((cj5)*(r21))));
3740 evalcond[1]=((x822)+(((IkReal(-1.00000000000000))*(x821)))+(((cj4)*(r22)))+(((r21)*(x817)))+(((IkReal(-1.00000000000000))*(r20)*(x814))));
3741 evalcond[2]=((x821)+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x816)))+(((IkReal(-1.00000000000000))*(x822)))+(((IkReal(-1.00000000000000))*(sj5)*(x815)))+(((IkReal(-1.00000000000000))*(x812)*(x813)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x818))));
3742 evalcond[3]=((x823)+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x818)))+(((IkReal(-1.00000000000000))*(sj0)*(x813)*(x817)))+(((r10)*(sj4)*(x812)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x816)))+(((x814)*(x815)))+(((IkReal(-1.00000000000000))*(r01)*(x817)*(x818))));
3743 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
3750 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3751 vinfos[0].jointtype = 1;
3752 vinfos[0].foffset = j0;
3753 vinfos[0].indices[0] = _ij0[0];
3754 vinfos[0].indices[1] = _ij0[1];
3755 vinfos[0].maxsolutions = _nj0;
3756 vinfos[1].jointtype = 1;
3757 vinfos[1].foffset = j1;
3758 vinfos[1].indices[0] = _ij1[0];
3759 vinfos[1].indices[1] = _ij1[1];
3760 vinfos[1].maxsolutions = _nj1;
3761 vinfos[2].jointtype = 1;
3762 vinfos[2].foffset = j2;
3763 vinfos[2].indices[0] = _ij2[0];
3764 vinfos[2].indices[1] = _ij2[1];
3765 vinfos[2].maxsolutions = _nj2;
3766 vinfos[3].jointtype = 1;
3767 vinfos[3].foffset = j3;
3768 vinfos[3].indices[0] = _ij3[0];
3769 vinfos[3].indices[1] = _ij3[1];
3770 vinfos[3].maxsolutions = _nj3;
3771 vinfos[4].jointtype = 1;
3772 vinfos[4].foffset = j4;
3773 vinfos[4].indices[0] = _ij4[0];
3774 vinfos[4].indices[1] = _ij4[1];
3775 vinfos[4].maxsolutions = _nj4;
3776 vinfos[5].jointtype = 1;
3777 vinfos[5].foffset = j5;
3778 vinfos[5].indices[0] = _ij5[0];
3779 vinfos[5].indices[1] = _ij5[1];
3780 vinfos[5].maxsolutions = _nj5;
3781 std::vector<int> vfree(0);
3800 IkReal j1array[1], cj1array[1], sj1array[1];
3801 bool j1valid[1]={
false};
3803 IkReal x824=((cj4)*(r22));
3804 IkReal x825=((cj2)*(sj5));
3805 IkReal x826=((r21)*(sj4));
3806 IkReal x827=((cj2)*(cj5));
3807 IkReal x828=((sj2)*(sj5));
3808 IkReal x829=((r20)*(sj4));
3809 IkReal x830=((cj5)*(sj2));
3810 if(
IKabs(((gconst109)*(((((x826)*(x828)))+(((r21)*(x827)))+(((sj2)*(x824)))+(((r20)*(x825)))+(((IkReal(-1.00000000000000))*(x829)*(x830))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst109)*(((((x827)*(x829)))+(((IkReal(-1.00000000000000))*(cj2)*(x824)))+(((IkReal(-1.00000000000000))*(x825)*(x826)))+(((r21)*(x830)))+(((r20)*(x828))))))) <
IKFAST_ATAN2_MAGTHRESH )
3812 j1array[0]=
IKatan2(((gconst109)*(((((x826)*(x828)))+(((r21)*(x827)))+(((sj2)*(x824)))+(((r20)*(x825)))+(((IkReal(-1.00000000000000))*(x829)*(x830)))))), ((gconst109)*(((((x827)*(x829)))+(((IkReal(-1.00000000000000))*(cj2)*(x824)))+(((IkReal(-1.00000000000000))*(x825)*(x826)))+(((r21)*(x830)))+(((r20)*(x828)))))));
3813 sj1array[0]=
IKsin(j1array[0]);
3814 cj1array[0]=
IKcos(j1array[0]);
3815 if( j1array[0] >
IKPI )
3819 else if( j1array[0] < -
IKPI )
3820 { j1array[0]+=
IK2PI;
3823 for(
int ij1 = 0; ij1 < 1; ++ij1)
3829 _ij1[0] = ij1; _ij1[1] = -1;
3830 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3834 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3837 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3840 IkReal x831=
IKsin(j1);
3841 IkReal x832=
IKcos(j1);
3842 evalcond[0]=((((cj2)*(x831)))+(((sj2)*(x832)))+(((r20)*(sj5)))+(((cj5)*(r21))));
3843 evalcond[1]=((((IkReal(-1.00000000000000))*(cj5)*(r20)*(sj4)))+(((sj2)*(x831)))+(((IkReal(-1.00000000000000))*(cj2)*(x832)))+(((cj4)*(r22)))+(((r21)*(sj4)*(sj5))));
3844 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
3851 IkReal dummyeval[1];
3853 IkReal x833=(sj5)*(sj5);
3854 IkReal x834=(cj5)*(cj5);
3855 IkReal x835=((cj5)*(sj4));
3856 IkReal x836=((IkReal(1.00000000000000))*(r02));
3857 IkReal x837=((sj4)*(sj5));
3858 IkReal x838=((cj4)*(r01)*(r10));
3859 IkReal x839=((IkReal(1.00000000000000))*(cj4)*(r00)*(r11));
3860 gconst113=
IKsign(((((IkReal(-1.00000000000000))*(x834)*(x839)))+(((x833)*(x838)))+(((IkReal(-1.00000000000000))*(r10)*(x836)*(x837)))+(((IkReal(-1.00000000000000))*(r11)*(x835)*(x836)))+(((x834)*(x838)))+(((IkReal(-1.00000000000000))*(x833)*(x839)))+(((r00)*(r12)*(x837)))+(((r01)*(r12)*(x835)))));
3861 IkReal x840=(sj5)*(sj5);
3862 IkReal x841=(cj5)*(cj5);
3863 IkReal x842=((cj5)*(sj4));
3864 IkReal x843=((IkReal(1.00000000000000))*(r02));
3865 IkReal x844=((sj4)*(sj5));
3866 IkReal x845=((cj4)*(r01)*(r10));
3868 dummyeval[0]=((((x841)*(x845)))+(((r00)*(r12)*(x844)))+(((IkReal(-1.00000000000000))*(r11)*(x842)*(x843)))+(((IkReal(-1.00000000000000))*(x841)*(x846)))+(((r01)*(r12)*(x842)))+(((IkReal(-1.00000000000000))*(r10)*(x843)*(x844)))+(((x840)*(x845)))+(((IkReal(-1.00000000000000))*(x840)*(x846))));
3869 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3872 IkReal dummyeval[1];
3874 IkReal x847=(cj5)*(cj5);
3875 IkReal x848=(sj5)*(sj5);
3876 IkReal x849=((IkReal(1.00000000000000))*(x848));
3877 IkReal x850=((IkReal(2.00000000000000))*(cj5)*(sj5));
3878 IkReal x851=((IkReal(1.00000000000000))*(x847));
3879 gconst112=
IKsign(((((IkReal(-1.00000000000000))*(x851)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x849)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x849)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x850)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x850)))+(((IkReal(-1.00000000000000))*(x851)*((r01)*(r01))))));
3880 IkReal x852=(cj5)*(cj5);
3881 IkReal x853=(sj5)*(sj5);
3882 IkReal x854=((IkReal(1.00000000000000))*(x853));
3883 IkReal x855=((IkReal(2.00000000000000))*(cj5)*(sj5));
3884 IkReal x856=((IkReal(1.00000000000000))*(x852));
3885 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x855)))+(((IkReal(-1.00000000000000))*(x854)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x854)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x855)))+(((IkReal(-1.00000000000000))*(x856)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x856)*((r01)*(r01)))));
3886 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
3893 IkReal j0array[1], cj0array[1], sj0array[1];
3894 bool j0valid[1]={
false};
3896 IkReal x857=((sj1)*(sj2));
3897 IkReal x858=((IkReal(1.00000000000000))*(cj1)*(cj2)*(cj5));
3898 IkReal x859=((IkReal(1.00000000000000))*(cj1)*(cj2)*(sj5));
3899 if(
IKabs(((gconst112)*(((((cj5)*(r11)*(x857)))+(((IkReal(-1.00000000000000))*(r11)*(x858)))+(((IkReal(-1.00000000000000))*(r10)*(x859)))+(((r10)*(sj5)*(x857))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst112)*(((((r00)*(sj5)*(x857)))+(((IkReal(-1.00000000000000))*(r01)*(x858)))+(((cj5)*(r01)*(x857)))+(((IkReal(-1.00000000000000))*(r00)*(x859))))))) <
IKFAST_ATAN2_MAGTHRESH )
3901 j0array[0]=
IKatan2(((gconst112)*(((((cj5)*(r11)*(x857)))+(((IkReal(-1.00000000000000))*(r11)*(x858)))+(((IkReal(-1.00000000000000))*(r10)*(x859)))+(((r10)*(sj5)*(x857)))))), ((gconst112)*(((((r00)*(sj5)*(x857)))+(((IkReal(-1.00000000000000))*(r01)*(x858)))+(((cj5)*(r01)*(x857)))+(((IkReal(-1.00000000000000))*(r00)*(x859)))))));
3902 sj0array[0]=
IKsin(j0array[0]);
3903 cj0array[0]=
IKcos(j0array[0]);
3904 if( j0array[0] >
IKPI )
3908 else if( j0array[0] < -
IKPI )
3909 { j0array[0]+=
IK2PI;
3912 for(
int ij0 = 0; ij0 < 1; ++ij0)
3918 _ij0[0] = ij0; _ij0[1] = -1;
3919 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
3923 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
3926 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
3929 IkReal x860=
IKsin(j0);
3930 IkReal x861=
IKcos(j0);
3931 IkReal x862=((IkReal(1.00000000000000))*(r10));
3932 IkReal x863=((IkReal(1.00000000000000))*(sj4));
3933 IkReal x864=((cj5)*(r01));
3934 IkReal x865=((cj5)*(r00));
3935 IkReal x866=((cj4)*(r01));
3936 IkReal x867=((IkReal(1.00000000000000))*(cj4));
3937 IkReal x868=((cj4)*(r11));
3938 IkReal x869=((sj5)*(x860));
3939 IkReal x870=((r11)*(x863));
3940 IkReal x871=((sj5)*(x861));
3941 IkReal x872=((r02)*(x861));
3942 IkReal x873=((r12)*(x861));
3943 IkReal x874=((IkReal(1.00000000000000))*(x861));
3944 IkReal x875=((cj4)*(x860));
3945 IkReal x876=((cj5)*(x860));
3946 IkReal x877=((cj5)*(sj4)*(x861));
3947 evalcond[0]=((((IkReal(-1.00000000000000))*(cj5)*(r11)*(x874)))+(((r00)*(x869)))+(((IkReal(-1.00000000000000))*(x862)*(x871)))+(((x860)*(x864))));
3948 evalcond[1]=((((IkReal(-1.00000000000000))*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(r00)*(x871)))+(((IkReal(-1.00000000000000))*(r11)*(x876)))+(((cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x864)*(x874)))+(((IkReal(-1.00000000000000))*(x862)*(x869))));
3949 evalcond[2]=((((IkReal(-1.00000000000000))*(x860)*(x863)*(x865)))+(((IkReal(-1.00000000000000))*(x870)*(x871)))+(((r10)*(x877)))+(((r01)*(sj4)*(x869)))+(((IkReal(-1.00000000000000))*(x867)*(x873)))+(((r02)*(x875))));
3950 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x866)*(x869)))+(((x868)*(x871)))+(((r02)*(sj4)*(x860)))+(((IkReal(-1.00000000000000))*(x863)*(x873)))+(((x865)*(x875)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x861)*(x862))));
3951 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x869)*(x870)))+(((IkReal(-1.00000000000000))*(r01)*(x863)*(x871)))+(((r10)*(sj4)*(x876)))+(((IkReal(-1.00000000000000))*(x867)*(x872)))+(((sj4)*(x861)*(x865)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(r12)*(x860)*(x867))));
3952 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x862)*(x875)))+(((x866)*(x871)))+(((x868)*(x869)))+(((IkReal(-1.00000000000000))*(x861)*(x865)*(x867)))+(((IkReal(-1.00000000000000))*(x863)*(x872)))+(((IkReal(-1.00000000000000))*(r12)*(x860)*(x863))));
3953 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
3960 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3961 vinfos[0].jointtype = 1;
3962 vinfos[0].foffset = j0;
3963 vinfos[0].indices[0] = _ij0[0];
3964 vinfos[0].indices[1] = _ij0[1];
3965 vinfos[0].maxsolutions = _nj0;
3966 vinfos[1].jointtype = 1;
3967 vinfos[1].foffset = j1;
3968 vinfos[1].indices[0] = _ij1[0];
3969 vinfos[1].indices[1] = _ij1[1];
3970 vinfos[1].maxsolutions = _nj1;
3971 vinfos[2].jointtype = 1;
3972 vinfos[2].foffset = j2;
3973 vinfos[2].indices[0] = _ij2[0];
3974 vinfos[2].indices[1] = _ij2[1];
3975 vinfos[2].maxsolutions = _nj2;
3976 vinfos[3].jointtype = 1;
3977 vinfos[3].foffset = j3;
3978 vinfos[3].indices[0] = _ij3[0];
3979 vinfos[3].indices[1] = _ij3[1];
3980 vinfos[3].maxsolutions = _nj3;
3981 vinfos[4].jointtype = 1;
3982 vinfos[4].foffset = j4;
3983 vinfos[4].indices[0] = _ij4[0];
3984 vinfos[4].indices[1] = _ij4[1];
3985 vinfos[4].maxsolutions = _nj4;
3986 vinfos[5].jointtype = 1;
3987 vinfos[5].foffset = j5;
3988 vinfos[5].indices[0] = _ij5[0];
3989 vinfos[5].indices[1] = _ij5[1];
3990 vinfos[5].maxsolutions = _nj5;
3991 std::vector<int> vfree(0);
4004 IkReal j0array[1], cj0array[1], sj0array[1];
4005 bool j0valid[1]={
false};
4007 IkReal x878=((IkReal(1.00000000000000))*(cj5));
4008 IkReal x879=((IkReal(1.00000000000000))*(sj5));
4009 if(
IKabs(((gconst113)*(((((IkReal(-1.00000000000000))*(r10)*(x879)))+(((IkReal(-1.00000000000000))*(r11)*(x878))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst113)*(((((IkReal(-1.00000000000000))*(r00)*(x879)))+(((IkReal(-1.00000000000000))*(r01)*(x878))))))) <
IKFAST_ATAN2_MAGTHRESH )
4011 j0array[0]=
IKatan2(((gconst113)*(((((IkReal(-1.00000000000000))*(r10)*(x879)))+(((IkReal(-1.00000000000000))*(r11)*(x878)))))), ((gconst113)*(((((IkReal(-1.00000000000000))*(r00)*(x879)))+(((IkReal(-1.00000000000000))*(r01)*(x878)))))));
4012 sj0array[0]=
IKsin(j0array[0]);
4013 cj0array[0]=
IKcos(j0array[0]);
4014 if( j0array[0] >
IKPI )
4018 else if( j0array[0] < -
IKPI )
4019 { j0array[0]+=
IK2PI;
4022 for(
int ij0 = 0; ij0 < 1; ++ij0)
4028 _ij0[0] = ij0; _ij0[1] = -1;
4029 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
4033 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
4036 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
4039 IkReal x880=
IKsin(j0);
4040 IkReal x881=
IKcos(j0);
4041 IkReal x882=((IkReal(1.00000000000000))*(r10));
4042 IkReal x883=((IkReal(1.00000000000000))*(sj4));
4043 IkReal x884=((cj5)*(r01));
4044 IkReal x885=((cj5)*(r00));
4045 IkReal x886=((cj4)*(r01));
4046 IkReal x887=((IkReal(1.00000000000000))*(cj4));
4047 IkReal x888=((cj4)*(r11));
4048 IkReal x889=((sj5)*(x880));
4049 IkReal x890=((r11)*(x883));
4050 IkReal x891=((sj5)*(x881));
4051 IkReal x892=((r02)*(x881));
4052 IkReal x893=((r12)*(x881));
4053 IkReal x894=((IkReal(1.00000000000000))*(x881));
4054 IkReal x895=((cj4)*(x880));
4055 IkReal x896=((cj5)*(x880));
4056 IkReal x897=((cj5)*(sj4)*(x881));
4057 evalcond[0]=((((r00)*(x889)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x894)))+(((IkReal(-1.00000000000000))*(x882)*(x891)))+(((x880)*(x884))));
4058 evalcond[1]=((((IkReal(-1.00000000000000))*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x884)*(x894)))+(((IkReal(-1.00000000000000))*(r11)*(x896)))+(((IkReal(-1.00000000000000))*(x882)*(x889)))+(((IkReal(-1.00000000000000))*(r00)*(x891)))+(((cj1)*(cj2))));
4059 evalcond[2]=((((IkReal(-1.00000000000000))*(x887)*(x893)))+(((IkReal(-1.00000000000000))*(x880)*(x883)*(x885)))+(((r02)*(x895)))+(((r01)*(sj4)*(x889)))+(((IkReal(-1.00000000000000))*(x890)*(x891)))+(((r10)*(x897))));
4060 evalcond[3]=((IkReal(-1.00000000000000))+(((x888)*(x891)))+(((IkReal(-1.00000000000000))*(x886)*(x889)))+(((IkReal(-1.00000000000000))*(x883)*(x893)))+(((r02)*(sj4)*(x880)))+(((x885)*(x895)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x881)*(x882))));
4061 evalcond[4]=((((cj1)*(sj2)))+(((sj4)*(x881)*(x885)))+(((IkReal(-1.00000000000000))*(x887)*(x892)))+(((IkReal(-1.00000000000000))*(r12)*(x880)*(x887)))+(((r10)*(sj4)*(x896)))+(((IkReal(-1.00000000000000))*(x889)*(x890)))+(((IkReal(-1.00000000000000))*(r01)*(x883)*(x891)))+(((cj2)*(sj1))));
4062 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x882)*(x895)))+(((x888)*(x889)))+(((x886)*(x891)))+(((IkReal(-1.00000000000000))*(x881)*(x885)*(x887)))+(((IkReal(-1.00000000000000))*(r12)*(x880)*(x883)))+(((IkReal(-1.00000000000000))*(x883)*(x892))));
4063 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
4070 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4071 vinfos[0].jointtype = 1;
4072 vinfos[0].foffset = j0;
4073 vinfos[0].indices[0] = _ij0[0];
4074 vinfos[0].indices[1] = _ij0[1];
4075 vinfos[0].maxsolutions = _nj0;
4076 vinfos[1].jointtype = 1;
4077 vinfos[1].foffset = j1;
4078 vinfos[1].indices[0] = _ij1[0];
4079 vinfos[1].indices[1] = _ij1[1];
4080 vinfos[1].maxsolutions = _nj1;
4081 vinfos[2].jointtype = 1;
4082 vinfos[2].foffset = j2;
4083 vinfos[2].indices[0] = _ij2[0];
4084 vinfos[2].indices[1] = _ij2[1];
4085 vinfos[2].maxsolutions = _nj2;
4086 vinfos[3].jointtype = 1;
4087 vinfos[3].foffset = j3;
4088 vinfos[3].indices[0] = _ij3[0];
4089 vinfos[3].indices[1] = _ij3[1];
4090 vinfos[3].maxsolutions = _nj3;
4091 vinfos[4].jointtype = 1;
4092 vinfos[4].foffset = j4;
4093 vinfos[4].indices[0] = _ij4[0];
4094 vinfos[4].indices[1] = _ij4[1];
4095 vinfos[4].maxsolutions = _nj4;
4096 vinfos[5].jointtype = 1;
4097 vinfos[5].foffset = j5;
4098 vinfos[5].indices[0] = _ij5[0];
4099 vinfos[5].indices[1] = _ij5[1];
4100 vinfos[5].maxsolutions = _nj5;
4101 std::vector<int> vfree(0);
4119 IkReal x898=((cj5)*(npx));
4120 IkReal x899=((npy)*(sj5));
4121 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(3.14159265358979))+(j3)), IkReal(6.28318530717959))));
4122 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
4123 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(sj4)*(x898)))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((sj4)*(x899))));
4124 evalcond[3]=((((IkReal(0.350000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj4)*(x899)))+(((cj4)*(x898)))+(((npz)*(sj4))));
4125 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
4126 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 )
4129 IkReal dummyeval[1];
4131 IkReal x900=(sj5)*(sj5);
4132 IkReal x901=(cj5)*(cj5);
4133 IkReal x902=((IkReal(2.00000000000000))*(cj5)*(sj5));
4134 gconst114=
IKsign(((((r00)*(r01)*(x902)))+(((x901)*((r11)*(r11))))+(((x900)*((r00)*(r00))))+(((r10)*(r11)*(x902)))+(((x901)*((r01)*(r01))))+(((x900)*((r10)*(r10))))));
4135 IkReal x903=(sj5)*(sj5);
4136 IkReal x904=(cj5)*(cj5);
4137 IkReal x905=((IkReal(2.00000000000000))*(cj5)*(sj5));
4138 dummyeval[0]=((((x904)*((r11)*(r11))))+(((r10)*(r11)*(x905)))+(((x903)*((r10)*(r10))))+(((r00)*(r01)*(x905)))+(((x904)*((r01)*(r01))))+(((x903)*((r00)*(r00)))));
4139 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4142 IkReal dummyeval[1];
4144 gconst116=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
4145 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
4146 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4149 IkReal dummyeval[1];
4151 IkReal x906=(cj5)*(cj5);
4152 IkReal x907=(sj5)*(sj5);
4153 IkReal x908=((IkReal(1.00000000000000))*(r10));
4154 IkReal x909=((cj4)*(sj5));
4155 IkReal x910=((r00)*(r11));
4156 IkReal x911=((cj4)*(cj5));
4157 IkReal x912=((sj4)*(x906));
4158 IkReal x913=((sj4)*(x907));
4159 gconst115=
IKsign(((((IkReal(-1.00000000000000))*(r01)*(x908)*(x913)))+(((IkReal(-1.00000000000000))*(r01)*(x908)*(x912)))+(((x910)*(x913)))+(((IkReal(-1.00000000000000))*(r02)*(x908)*(x909)))+(((x910)*(x912)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x911)))+(((r00)*(r12)*(x909)))+(((r01)*(r12)*(x911)))));
4160 IkReal x914=(cj5)*(cj5);
4161 IkReal x915=(sj5)*(sj5);
4162 IkReal x916=((IkReal(1.00000000000000))*(r10));
4163 IkReal x917=((cj4)*(sj5));
4164 IkReal x918=((r00)*(r11));
4165 IkReal x919=((cj4)*(cj5));
4166 IkReal x920=((sj4)*(x914));
4167 IkReal x921=((sj4)*(x915));
4168 dummyeval[0]=((((x918)*(x921)))+(((IkReal(-1.00000000000000))*(r01)*(x916)*(x920)))+(((IkReal(-1.00000000000000))*(r02)*(x916)*(x917)))+(((r00)*(r12)*(x917)))+(((r01)*(r12)*(x919)))+(((IkReal(-1.00000000000000))*(r01)*(x916)*(x921)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x919)))+(((x918)*(x920))));
4169 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4176 IkReal j0array[1], cj0array[1], sj0array[1];
4177 bool j0valid[1]={
false};
4179 IkReal x922=((cj5)*(sj4));
4180 IkReal x923=((IkReal(1.00000000000000))*(cj4));
4181 IkReal x924=((IkReal(1.00000000000000))*(sj4)*(sj5));
4182 if(
IKabs(((gconst115)*(((((IkReal(-1.00000000000000))*(r11)*(x924)))+(((IkReal(-1.00000000000000))*(r12)*(x923)))+(((r10)*(x922))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst115)*(((((IkReal(-1.00000000000000))*(r01)*(x924)))+(((r00)*(x922)))+(((IkReal(-1.00000000000000))*(r02)*(x923))))))) <
IKFAST_ATAN2_MAGTHRESH )
4184 j0array[0]=
IKatan2(((gconst115)*(((((IkReal(-1.00000000000000))*(r11)*(x924)))+(((IkReal(-1.00000000000000))*(r12)*(x923)))+(((r10)*(x922)))))), ((gconst115)*(((((IkReal(-1.00000000000000))*(r01)*(x924)))+(((r00)*(x922)))+(((IkReal(-1.00000000000000))*(r02)*(x923)))))));
4185 sj0array[0]=
IKsin(j0array[0]);
4186 cj0array[0]=
IKcos(j0array[0]);
4187 if( j0array[0] >
IKPI )
4191 else if( j0array[0] < -
IKPI )
4192 { j0array[0]+=
IK2PI;
4195 for(
int ij0 = 0; ij0 < 1; ++ij0)
4201 _ij0[0] = ij0; _ij0[1] = -1;
4202 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
4206 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
4209 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
4212 IkReal x925=
IKsin(j0);
4213 IkReal x926=
IKcos(j0);
4214 IkReal x927=((IkReal(1.00000000000000))*(cj5));
4215 IkReal x928=((r01)*(sj5));
4216 IkReal x929=((IkReal(1.00000000000000))*(r12));
4217 IkReal x930=((IkReal(1.00000000000000))*(r10));
4218 IkReal x931=((r11)*(sj5));
4219 IkReal x932=((cj4)*(x925));
4220 IkReal x933=((sj4)*(x925));
4221 IkReal x934=((sj4)*(x926));
4222 IkReal x935=((sj5)*(x925));
4223 IkReal x936=((cj4)*(x926));
4224 IkReal x937=((sj5)*(x926));
4225 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x930)*(x937)))+(((r00)*(x935)))+(((IkReal(-1.00000000000000))*(r11)*(x926)*(x927)))+(((cj5)*(r01)*(x925))));
4226 evalcond[1]=((((IkReal(-1.00000000000000))*(r01)*(x926)*(x927)))+(((IkReal(-1.00000000000000))*(r00)*(x937)))+(((IkReal(-1.00000000000000))*(r11)*(x925)*(x927)))+(((IkReal(-1.00000000000000))*(x930)*(x935))));
4227 evalcond[2]=((((IkReal(-1.00000000000000))*(x931)*(x934)))+(((x928)*(x933)))+(((IkReal(-1.00000000000000))*(r00)*(x927)*(x933)))+(((IkReal(-1.00000000000000))*(x929)*(x936)))+(((r02)*(x932)))+(((cj5)*(r10)*(x934))));
4228 evalcond[3]=((((x931)*(x936)))+(((IkReal(-1.00000000000000))*(r10)*(x927)*(x936)))+(((r02)*(x933)))+(((IkReal(-1.00000000000000))*(x928)*(x932)))+(((IkReal(-1.00000000000000))*(x929)*(x934)))+(((cj5)*(r00)*(x932))));
4229 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
4236 IkReal dummyeval[1];
4238 gconst117=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
4239 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
4240 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4243 IkReal dummyeval[1];
4245 gconst118=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
4246 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
4247 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4254 IkReal j1array[1], cj1array[1], sj1array[1];
4255 bool j1valid[1]={
false};
4257 IkReal x938=((cj4)*(r22));
4258 IkReal x939=((r11)*(sj0));
4259 IkReal x940=((IkReal(1.00000000000000))*(cj2));
4260 IkReal x941=((sj2)*(sj4));
4261 IkReal x942=((IkReal(1.00000000000000))*(sj2));
4262 IkReal x943=((cj2)*(sj4));
4263 IkReal x944=((cj0)*(r01));
4264 IkReal x945=((cj5)*(r20));
4265 IkReal x946=((IkReal(1.00000000000000))*(sj4)*(sj5));
4266 IkReal x947=((cj0)*(cj4)*(r02));
4267 IkReal x948=((cj4)*(r12)*(sj0));
4268 IkReal x949=((cj5)*(r10)*(sj0));
4269 IkReal x950=((cj0)*(cj5)*(r00));
4270 if(
IKabs(((gconst118)*(((((r21)*(sj5)*(x941)))+(((IkReal(-1.00000000000000))*(x941)*(x945)))+(((IkReal(-1.00000000000000))*(x940)*(x948)))+(((IkReal(-1.00000000000000))*(x940)*(x947)))+(((x943)*(x949)))+(((sj2)*(x938)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x940)*(x944)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x939)*(x940)))+(((x943)*(x950))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst118)*(((((IkReal(-1.00000000000000))*(sj5)*(x939)*(x941)))+(((IkReal(-1.00000000000000))*(sj5)*(x941)*(x944)))+(((x941)*(x949)))+(((IkReal(-1.00000000000000))*(x942)*(x947)))+(((x943)*(x945)))+(((IkReal(-1.00000000000000))*(x942)*(x948)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((x941)*(x950)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x940))))))) <
IKFAST_ATAN2_MAGTHRESH )
4272 j1array[0]=
IKatan2(((gconst118)*(((((r21)*(sj5)*(x941)))+(((IkReal(-1.00000000000000))*(x941)*(x945)))+(((IkReal(-1.00000000000000))*(x940)*(x948)))+(((IkReal(-1.00000000000000))*(x940)*(x947)))+(((x943)*(x949)))+(((sj2)*(x938)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x940)*(x944)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x939)*(x940)))+(((x943)*(x950)))))), ((gconst118)*(((((IkReal(-1.00000000000000))*(sj5)*(x939)*(x941)))+(((IkReal(-1.00000000000000))*(sj5)*(x941)*(x944)))+(((x941)*(x949)))+(((IkReal(-1.00000000000000))*(x942)*(x947)))+(((x943)*(x945)))+(((IkReal(-1.00000000000000))*(x942)*(x948)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((x941)*(x950)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x940)))))));
4273 sj1array[0]=
IKsin(j1array[0]);
4274 cj1array[0]=
IKcos(j1array[0]);
4275 if( j1array[0] >
IKPI )
4279 else if( j1array[0] < -
IKPI )
4280 { j1array[0]+=
IK2PI;
4283 for(
int ij1 = 0; ij1 < 1; ++ij1)
4289 _ij1[0] = ij1; _ij1[1] = -1;
4290 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4294 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4297 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4300 IkReal x951=
IKcos(j1);
4301 IkReal x952=
IKsin(j1);
4302 IkReal x953=((IkReal(1.00000000000000))*(sj4));
4303 IkReal x954=((cj5)*(r00));
4304 IkReal x955=((cj5)*(r20));
4305 IkReal x956=((r12)*(sj0));
4306 IkReal x957=((r21)*(sj5));
4307 IkReal x958=((IkReal(1.00000000000000))*(cj4));
4308 IkReal x959=((sj2)*(x951));
4309 IkReal x960=((cj2)*(x952));
4310 IkReal x961=((cj2)*(x951));
4311 IkReal x962=((r11)*(sj0)*(sj5));
4312 IkReal x963=((cj0)*(x958));
4313 IkReal x964=((cj0)*(r01)*(sj5));
4314 IkReal x965=((cj5)*(r10)*(sj0));
4315 IkReal x966=((sj2)*(x952));
4316 IkReal x967=((x959)+(x960));
4317 evalcond[0]=((((IkReal(-1.00000000000000))*(x961)))+(((cj4)*(r22)))+(((sj4)*(x957)))+(x966)+(((IkReal(-1.00000000000000))*(x953)*(x955))));
4318 evalcond[1]=((((IkReal(-1.00000000000000))*(x957)*(x958)))+(((r22)*(sj4)))+(((cj4)*(x955)))+(x967));
4319 evalcond[2]=((((IkReal(-1.00000000000000))*(x953)*(x962)))+(((IkReal(-1.00000000000000))*(x953)*(x964)))+(((IkReal(-1.00000000000000))*(x956)*(x958)))+(((cj0)*(sj4)*(x954)))+(x967)+(((sj4)*(x965)))+(((IkReal(-1.00000000000000))*(r02)*(x963))));
4320 evalcond[3]=((((cj4)*(x962)))+(((IkReal(-1.00000000000000))*(x958)*(x965)))+(((IkReal(-1.00000000000000))*(x966)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x953)))+(((IkReal(-1.00000000000000))*(x953)*(x956)))+(((IkReal(-1.00000000000000))*(x954)*(x963)))+(x961)+(((cj4)*(x964))));
4321 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
4328 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4329 vinfos[0].jointtype = 1;
4330 vinfos[0].foffset = j0;
4331 vinfos[0].indices[0] = _ij0[0];
4332 vinfos[0].indices[1] = _ij0[1];
4333 vinfos[0].maxsolutions = _nj0;
4334 vinfos[1].jointtype = 1;
4335 vinfos[1].foffset = j1;
4336 vinfos[1].indices[0] = _ij1[0];
4337 vinfos[1].indices[1] = _ij1[1];
4338 vinfos[1].maxsolutions = _nj1;
4339 vinfos[2].jointtype = 1;
4340 vinfos[2].foffset = j2;
4341 vinfos[2].indices[0] = _ij2[0];
4342 vinfos[2].indices[1] = _ij2[1];
4343 vinfos[2].maxsolutions = _nj2;
4344 vinfos[3].jointtype = 1;
4345 vinfos[3].foffset = j3;
4346 vinfos[3].indices[0] = _ij3[0];
4347 vinfos[3].indices[1] = _ij3[1];
4348 vinfos[3].maxsolutions = _nj3;
4349 vinfos[4].jointtype = 1;
4350 vinfos[4].foffset = j4;
4351 vinfos[4].indices[0] = _ij4[0];
4352 vinfos[4].indices[1] = _ij4[1];
4353 vinfos[4].maxsolutions = _nj4;
4354 vinfos[5].jointtype = 1;
4355 vinfos[5].foffset = j5;
4356 vinfos[5].indices[0] = _ij5[0];
4357 vinfos[5].indices[1] = _ij5[1];
4358 vinfos[5].maxsolutions = _nj5;
4359 std::vector<int> vfree(0);
4372 IkReal j1array[1], cj1array[1], sj1array[1];
4373 bool j1valid[1]={
false};
4375 IkReal x968=((cj4)*(r22));
4376 IkReal x969=((cj2)*(sj4));
4377 IkReal x970=((sj2)*(sj4));
4378 IkReal x971=((r21)*(sj5));
4379 IkReal x972=((cj5)*(r20));
4380 IkReal x973=((cj4)*(sj2));
4381 IkReal x974=((IkReal(1.00000000000000))*(cj2)*(x971));
4382 if(
IKabs(((gconst117)*(((((sj2)*(x968)))+(((IkReal(-1.00000000000000))*(x970)*(x972)))+(((cj2)*(cj4)*(x972)))+(((r22)*(x969)))+(((IkReal(-1.00000000000000))*(cj4)*(x974)))+(((x970)*(x971))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst117)*(((((IkReal(-1.00000000000000))*(cj2)*(x968)))+(((r22)*(x970)))+(((x972)*(x973)))+(((IkReal(-1.00000000000000))*(x971)*(x973)))+(((x969)*(x972)))+(((IkReal(-1.00000000000000))*(x969)*(x971))))))) <
IKFAST_ATAN2_MAGTHRESH )
4384 j1array[0]=
IKatan2(((gconst117)*(((((sj2)*(x968)))+(((IkReal(-1.00000000000000))*(x970)*(x972)))+(((cj2)*(cj4)*(x972)))+(((r22)*(x969)))+(((IkReal(-1.00000000000000))*(cj4)*(x974)))+(((x970)*(x971)))))), ((gconst117)*(((((IkReal(-1.00000000000000))*(cj2)*(x968)))+(((r22)*(x970)))+(((x972)*(x973)))+(((IkReal(-1.00000000000000))*(x971)*(x973)))+(((x969)*(x972)))+(((IkReal(-1.00000000000000))*(x969)*(x971)))))));
4385 sj1array[0]=
IKsin(j1array[0]);
4386 cj1array[0]=
IKcos(j1array[0]);
4387 if( j1array[0] >
IKPI )
4391 else if( j1array[0] < -
IKPI )
4392 { j1array[0]+=
IK2PI;
4395 for(
int ij1 = 0; ij1 < 1; ++ij1)
4401 _ij1[0] = ij1; _ij1[1] = -1;
4402 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4406 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4409 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4412 IkReal x975=
IKcos(j1);
4413 IkReal x976=
IKsin(j1);
4414 IkReal x977=((IkReal(1.00000000000000))*(sj4));
4415 IkReal x978=((cj5)*(r00));
4416 IkReal x979=((cj5)*(r20));
4417 IkReal x980=((r12)*(sj0));
4418 IkReal x981=((r21)*(sj5));
4419 IkReal x982=((IkReal(1.00000000000000))*(cj4));
4420 IkReal x983=((sj2)*(x975));
4421 IkReal x984=((cj2)*(x976));
4422 IkReal x985=((cj2)*(x975));
4423 IkReal x986=((r11)*(sj0)*(sj5));
4424 IkReal x987=((cj0)*(x982));
4425 IkReal x988=((cj0)*(r01)*(sj5));
4426 IkReal x989=((cj5)*(r10)*(sj0));
4427 IkReal x990=((sj2)*(x976));
4428 IkReal x991=((x984)+(x983));
4429 evalcond[0]=((((sj4)*(x981)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x985)))+(((IkReal(-1.00000000000000))*(x977)*(x979)))+(x990));
4430 evalcond[1]=((((cj4)*(x979)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(x981)*(x982)))+(x991));
4431 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x987)))+(((IkReal(-1.00000000000000))*(x977)*(x986)))+(((sj4)*(x989)))+(((cj0)*(sj4)*(x978)))+(((IkReal(-1.00000000000000))*(x980)*(x982)))+(((IkReal(-1.00000000000000))*(x977)*(x988)))+(x991));
4432 evalcond[3]=((((cj4)*(x986)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x977)))+(((IkReal(-1.00000000000000))*(x978)*(x987)))+(x985)+(((IkReal(-1.00000000000000))*(x977)*(x980)))+(((IkReal(-1.00000000000000))*(x982)*(x989)))+(((IkReal(-1.00000000000000))*(x990)))+(((cj4)*(x988))));
4433 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
4440 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4441 vinfos[0].jointtype = 1;
4442 vinfos[0].foffset = j0;
4443 vinfos[0].indices[0] = _ij0[0];
4444 vinfos[0].indices[1] = _ij0[1];
4445 vinfos[0].maxsolutions = _nj0;
4446 vinfos[1].jointtype = 1;
4447 vinfos[1].foffset = j1;
4448 vinfos[1].indices[0] = _ij1[0];
4449 vinfos[1].indices[1] = _ij1[1];
4450 vinfos[1].maxsolutions = _nj1;
4451 vinfos[2].jointtype = 1;
4452 vinfos[2].foffset = j2;
4453 vinfos[2].indices[0] = _ij2[0];
4454 vinfos[2].indices[1] = _ij2[1];
4455 vinfos[2].maxsolutions = _nj2;
4456 vinfos[3].jointtype = 1;
4457 vinfos[3].foffset = j3;
4458 vinfos[3].indices[0] = _ij3[0];
4459 vinfos[3].indices[1] = _ij3[1];
4460 vinfos[3].maxsolutions = _nj3;
4461 vinfos[4].jointtype = 1;
4462 vinfos[4].foffset = j4;
4463 vinfos[4].indices[0] = _ij4[0];
4464 vinfos[4].indices[1] = _ij4[1];
4465 vinfos[4].maxsolutions = _nj4;
4466 vinfos[5].jointtype = 1;
4467 vinfos[5].foffset = j5;
4468 vinfos[5].indices[0] = _ij5[0];
4469 vinfos[5].indices[1] = _ij5[1];
4470 vinfos[5].maxsolutions = _nj5;
4471 std::vector<int> vfree(0);
4490 IkReal j1array[1], cj1array[1], sj1array[1];
4491 bool j1valid[1]={
false};
4493 IkReal x992=((cj4)*(r22));
4494 IkReal x993=((cj2)*(sj4));
4495 IkReal x994=((sj2)*(sj4));
4496 IkReal x995=((r21)*(sj5));
4497 IkReal x996=((cj5)*(r20));
4498 IkReal x997=((cj4)*(sj2));
4499 IkReal x998=((IkReal(1.00000000000000))*(cj2)*(x995));
4500 if(
IKabs(((gconst116)*(((((cj2)*(cj4)*(x996)))+(((x994)*(x995)))+(((r22)*(x993)))+(((sj2)*(x992)))+(((IkReal(-1.00000000000000))*(cj4)*(x998)))+(((IkReal(-1.00000000000000))*(x994)*(x996))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst116)*(((((r22)*(x994)))+(((IkReal(-1.00000000000000))*(x993)*(x995)))+(((IkReal(-1.00000000000000))*(x995)*(x997)))+(((IkReal(-1.00000000000000))*(cj2)*(x992)))+(((x996)*(x997)))+(((x993)*(x996))))))) <
IKFAST_ATAN2_MAGTHRESH )
4502 j1array[0]=
IKatan2(((gconst116)*(((((cj2)*(cj4)*(x996)))+(((x994)*(x995)))+(((r22)*(x993)))+(((sj2)*(x992)))+(((IkReal(-1.00000000000000))*(cj4)*(x998)))+(((IkReal(-1.00000000000000))*(x994)*(x996)))))), ((gconst116)*(((((r22)*(x994)))+(((IkReal(-1.00000000000000))*(x993)*(x995)))+(((IkReal(-1.00000000000000))*(x995)*(x997)))+(((IkReal(-1.00000000000000))*(cj2)*(x992)))+(((x996)*(x997)))+(((x993)*(x996)))))));
4503 sj1array[0]=
IKsin(j1array[0]);
4504 cj1array[0]=
IKcos(j1array[0]);
4505 if( j1array[0] >
IKPI )
4509 else if( j1array[0] < -
IKPI )
4510 { j1array[0]+=
IK2PI;
4513 for(
int ij1 = 0; ij1 < 1; ++ij1)
4519 _ij1[0] = ij1; _ij1[1] = -1;
4520 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4524 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4527 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4530 IkReal x999=
IKcos(j1);
4531 IkReal x1000=
IKsin(j1);
4532 IkReal x1001=((cj5)*(r20));
4533 IkReal x1002=((r21)*(sj5));
4534 evalcond[0]=((((sj4)*(x1002)))+(((cj4)*(r22)))+(((sj2)*(x1000)))+(((IkReal(-1.00000000000000))*(cj2)*(x999)))+(((IkReal(-1.00000000000000))*(sj4)*(x1001))));
4535 evalcond[1]=((((cj4)*(x1001)))+(((r22)*(sj4)))+(((cj2)*(x1000)))+(((sj2)*(x999)))+(((IkReal(-1.00000000000000))*(cj4)*(x1002))));
4536 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
4543 IkReal dummyeval[1];
4545 IkReal x1003=(sj5)*(sj5);
4546 IkReal x1004=(cj5)*(cj5);
4547 IkReal x1005=((IkReal(2.00000000000000))*(cj5)*(sj5));
4548 gconst119=
IKsign(((((x1003)*((r10)*(r10))))+(((x1004)*((r11)*(r11))))+(((x1003)*((r00)*(r00))))+(((x1004)*((r01)*(r01))))+(((r00)*(r01)*(x1005)))+(((r10)*(r11)*(x1005)))));
4549 IkReal x1006=(sj5)*(sj5);
4550 IkReal x1007=(cj5)*(cj5);
4551 IkReal x1008=((IkReal(2.00000000000000))*(cj5)*(sj5));
4552 dummyeval[0]=((((x1006)*((r10)*(r10))))+(((x1006)*((r00)*(r00))))+(((x1007)*((r11)*(r11))))+(((r10)*(r11)*(x1008)))+(((r00)*(r01)*(x1008)))+(((x1007)*((r01)*(r01)))));
4553 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4556 IkReal dummyeval[1];
4558 IkReal x1009=(cj5)*(cj5);
4559 IkReal x1010=(sj5)*(sj5);
4560 IkReal x1011=((IkReal(1.00000000000000))*(r10));
4561 IkReal x1012=((cj4)*(sj5));
4562 IkReal x1013=((r00)*(r11));
4563 IkReal x1014=((cj4)*(cj5));
4564 IkReal x1015=((sj4)*(x1009));
4565 IkReal x1016=((sj4)*(x1010));
4566 gconst120=
IKsign(((((x1013)*(x1015)))+(((x1013)*(x1016)))+(((r01)*(r12)*(x1014)))+(((r00)*(r12)*(x1012)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1014)))+(((IkReal(-1.00000000000000))*(r01)*(x1011)*(x1016)))+(((IkReal(-1.00000000000000))*(r01)*(x1011)*(x1015)))+(((IkReal(-1.00000000000000))*(r02)*(x1011)*(x1012)))));
4567 IkReal x1017=(cj5)*(cj5);
4568 IkReal x1018=(sj5)*(sj5);
4569 IkReal x1019=((IkReal(1.00000000000000))*(r10));
4570 IkReal x1020=((cj4)*(sj5));
4571 IkReal x1021=((r00)*(r11));
4572 IkReal x1022=((cj4)*(cj5));
4573 IkReal x1023=((sj4)*(x1017));
4574 IkReal x1024=((sj4)*(x1018));
4575 dummyeval[0]=((((x1021)*(x1024)))+(((IkReal(-1.00000000000000))*(r01)*(x1019)*(x1024)))+(((r00)*(r12)*(x1020)))+(((IkReal(-1.00000000000000))*(r02)*(x1019)*(x1020)))+(((r01)*(r12)*(x1022)))+(((x1021)*(x1023)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1022)))+(((IkReal(-1.00000000000000))*(r01)*(x1019)*(x1023))));
4576 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4583 IkReal j0array[1], cj0array[1], sj0array[1];
4584 bool j0valid[1]={
false};
4586 IkReal x1025=((cj5)*(sj4));
4587 IkReal x1026=((IkReal(1.00000000000000))*(cj4));
4588 IkReal x1027=((IkReal(1.00000000000000))*(sj4)*(sj5));
4589 if(
IKabs(((gconst120)*(((((IkReal(-1.00000000000000))*(r11)*(x1027)))+(((IkReal(-1.00000000000000))*(r12)*(x1026)))+(((r10)*(x1025))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst120)*(((((IkReal(-1.00000000000000))*(r01)*(x1027)))+(((IkReal(-1.00000000000000))*(r02)*(x1026)))+(((r00)*(x1025))))))) <
IKFAST_ATAN2_MAGTHRESH )
4591 j0array[0]=
IKatan2(((gconst120)*(((((IkReal(-1.00000000000000))*(r11)*(x1027)))+(((IkReal(-1.00000000000000))*(r12)*(x1026)))+(((r10)*(x1025)))))), ((gconst120)*(((((IkReal(-1.00000000000000))*(r01)*(x1027)))+(((IkReal(-1.00000000000000))*(r02)*(x1026)))+(((r00)*(x1025)))))));
4592 sj0array[0]=
IKsin(j0array[0]);
4593 cj0array[0]=
IKcos(j0array[0]);
4594 if( j0array[0] >
IKPI )
4598 else if( j0array[0] < -
IKPI )
4599 { j0array[0]+=
IK2PI;
4602 for(
int ij0 = 0; ij0 < 1; ++ij0)
4608 _ij0[0] = ij0; _ij0[1] = -1;
4609 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
4613 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
4616 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
4619 IkReal x1028=
IKsin(j0);
4620 IkReal x1029=
IKcos(j0);
4621 IkReal x1030=((IkReal(1.00000000000000))*(r10));
4622 IkReal x1031=((IkReal(1.00000000000000))*(sj4));
4623 IkReal x1032=((cj5)*(r01));
4624 IkReal x1033=((cj5)*(r00));
4625 IkReal x1034=((cj4)*(r01));
4626 IkReal x1035=((IkReal(1.00000000000000))*(cj4));
4627 IkReal x1036=((cj4)*(r11));
4628 IkReal x1037=((sj5)*(x1028));
4629 IkReal x1038=((r11)*(x1031));
4630 IkReal x1039=((sj5)*(x1029));
4631 IkReal x1040=((r02)*(x1029));
4632 IkReal x1041=((r12)*(x1029));
4633 IkReal x1042=((IkReal(1.00000000000000))*(x1029));
4634 IkReal x1043=((cj4)*(x1028));
4635 IkReal x1044=((cj5)*(x1028));
4636 IkReal x1045=((cj5)*(sj4)*(x1029));
4637 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1030)*(x1039)))+(((r00)*(x1037)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1042)))+(((x1028)*(x1032))));
4638 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x1044)))+(((IkReal(-1.00000000000000))*(x1032)*(x1042)))+(((IkReal(-1.00000000000000))*(r00)*(x1039)))+(((IkReal(-1.00000000000000))*(x1030)*(x1037))));
4639 evalcond[2]=((((r01)*(sj4)*(x1037)))+(((r02)*(x1043)))+(((IkReal(-1.00000000000000))*(x1028)*(x1031)*(x1033)))+(((IkReal(-1.00000000000000))*(x1035)*(x1041)))+(((IkReal(-1.00000000000000))*(x1038)*(x1039)))+(((r10)*(x1045))));
4640 evalcond[3]=((((x1036)*(x1039)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x1029)*(x1030)))+(((IkReal(-1.00000000000000))*(x1031)*(x1041)))+(((x1033)*(x1043)))+(((r02)*(sj4)*(x1028)))+(((IkReal(-1.00000000000000))*(x1034)*(x1037))));
4641 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1035)*(x1040)))+(((sj4)*(x1029)*(x1033)))+(((IkReal(-1.00000000000000))*(x1037)*(x1038)))+(((IkReal(-1.00000000000000))*(r01)*(x1031)*(x1039)))+(((IkReal(-1.00000000000000))*(r12)*(x1028)*(x1035)))+(((cj2)*(sj1)))+(((r10)*(sj4)*(x1044))));
4642 evalcond[5]=((((IkReal(-1.00000000000000))*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1029)*(x1033)*(x1035)))+(((x1034)*(x1039)))+(((IkReal(-1.00000000000000))*(r12)*(x1028)*(x1031)))+(((IkReal(-1.00000000000000))*(cj5)*(x1030)*(x1043)))+(((IkReal(-1.00000000000000))*(x1031)*(x1040)))+(((cj1)*(cj2)))+(((x1036)*(x1037))));
4643 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
4650 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4651 vinfos[0].jointtype = 1;
4652 vinfos[0].foffset = j0;
4653 vinfos[0].indices[0] = _ij0[0];
4654 vinfos[0].indices[1] = _ij0[1];
4655 vinfos[0].maxsolutions = _nj0;
4656 vinfos[1].jointtype = 1;
4657 vinfos[1].foffset = j1;
4658 vinfos[1].indices[0] = _ij1[0];
4659 vinfos[1].indices[1] = _ij1[1];
4660 vinfos[1].maxsolutions = _nj1;
4661 vinfos[2].jointtype = 1;
4662 vinfos[2].foffset = j2;
4663 vinfos[2].indices[0] = _ij2[0];
4664 vinfos[2].indices[1] = _ij2[1];
4665 vinfos[2].maxsolutions = _nj2;
4666 vinfos[3].jointtype = 1;
4667 vinfos[3].foffset = j3;
4668 vinfos[3].indices[0] = _ij3[0];
4669 vinfos[3].indices[1] = _ij3[1];
4670 vinfos[3].maxsolutions = _nj3;
4671 vinfos[4].jointtype = 1;
4672 vinfos[4].foffset = j4;
4673 vinfos[4].indices[0] = _ij4[0];
4674 vinfos[4].indices[1] = _ij4[1];
4675 vinfos[4].maxsolutions = _nj4;
4676 vinfos[5].jointtype = 1;
4677 vinfos[5].foffset = j5;
4678 vinfos[5].indices[0] = _ij5[0];
4679 vinfos[5].indices[1] = _ij5[1];
4680 vinfos[5].maxsolutions = _nj5;
4681 std::vector<int> vfree(0);
4694 IkReal j0array[1], cj0array[1], sj0array[1];
4695 bool j0valid[1]={
false};
4699 j0array[0]=
IKatan2(((gconst119)*(((((IkReal(-1.00000000000000))*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)))))), ((gconst119)*(((((cj5)*(r11)))+(((r10)*(sj5)))))));
4700 sj0array[0]=
IKsin(j0array[0]);
4701 cj0array[0]=
IKcos(j0array[0]);
4702 if( j0array[0] >
IKPI )
4706 else if( j0array[0] < -
IKPI )
4707 { j0array[0]+=
IK2PI;
4710 for(
int ij0 = 0; ij0 < 1; ++ij0)
4716 _ij0[0] = ij0; _ij0[1] = -1;
4717 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
4721 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
4724 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
4727 IkReal x1046=
IKsin(j0);
4728 IkReal x1047=
IKcos(j0);
4729 IkReal x1048=((IkReal(1.00000000000000))*(r10));
4730 IkReal x1049=((IkReal(1.00000000000000))*(sj4));
4731 IkReal x1050=((cj5)*(r01));
4732 IkReal x1051=((cj5)*(r00));
4733 IkReal x1052=((cj4)*(r01));
4734 IkReal x1053=((IkReal(1.00000000000000))*(cj4));
4735 IkReal x1054=((cj4)*(r11));
4736 IkReal x1055=((sj5)*(x1046));
4737 IkReal x1056=((r11)*(x1049));
4738 IkReal x1057=((sj5)*(x1047));
4739 IkReal x1058=((r02)*(x1047));
4740 IkReal x1059=((r12)*(x1047));
4741 IkReal x1060=((IkReal(1.00000000000000))*(x1047));
4742 IkReal x1061=((cj4)*(x1046));
4743 IkReal x1062=((cj5)*(x1046));
4744 IkReal x1063=((cj5)*(sj4)*(x1047));
4745 evalcond[0]=((IkReal(1.00000000000000))+(((x1046)*(x1050)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1060)))+(((r00)*(x1055)))+(((IkReal(-1.00000000000000))*(x1048)*(x1057))));
4746 evalcond[1]=((((IkReal(-1.00000000000000))*(x1050)*(x1060)))+(((IkReal(-1.00000000000000))*(r00)*(x1057)))+(((IkReal(-1.00000000000000))*(x1048)*(x1055)))+(((IkReal(-1.00000000000000))*(r11)*(x1062))));
4747 evalcond[2]=((((IkReal(-1.00000000000000))*(x1046)*(x1049)*(x1051)))+(((IkReal(-1.00000000000000))*(x1056)*(x1057)))+(((IkReal(-1.00000000000000))*(x1053)*(x1059)))+(((r02)*(x1061)))+(((r10)*(x1063)))+(((r01)*(sj4)*(x1055))));
4748 evalcond[3]=((((x1054)*(x1057)))+(((x1051)*(x1061)))+(((r02)*(sj4)*(x1046)))+(((IkReal(-1.00000000000000))*(x1052)*(x1055)))+(((IkReal(-1.00000000000000))*(x1049)*(x1059)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x1047)*(x1048))));
4749 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(r01)*(x1049)*(x1057)))+(((IkReal(-1.00000000000000))*(x1053)*(x1058)))+(((IkReal(-1.00000000000000))*(r12)*(x1046)*(x1053)))+(((IkReal(-1.00000000000000))*(x1055)*(x1056)))+(((sj4)*(x1047)*(x1051)))+(((r10)*(sj4)*(x1062)))+(((cj2)*(sj1))));
4750 evalcond[5]=((((IkReal(-1.00000000000000))*(sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1049)*(x1058)))+(((IkReal(-1.00000000000000))*(cj5)*(x1048)*(x1061)))+(((IkReal(-1.00000000000000))*(r12)*(x1046)*(x1049)))+(((IkReal(-1.00000000000000))*(x1047)*(x1051)*(x1053)))+(((cj1)*(cj2)))+(((x1054)*(x1055)))+(((x1052)*(x1057))));
4751 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
4758 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4759 vinfos[0].jointtype = 1;
4760 vinfos[0].foffset = j0;
4761 vinfos[0].indices[0] = _ij0[0];
4762 vinfos[0].indices[1] = _ij0[1];
4763 vinfos[0].maxsolutions = _nj0;
4764 vinfos[1].jointtype = 1;
4765 vinfos[1].foffset = j1;
4766 vinfos[1].indices[0] = _ij1[0];
4767 vinfos[1].indices[1] = _ij1[1];
4768 vinfos[1].maxsolutions = _nj1;
4769 vinfos[2].jointtype = 1;
4770 vinfos[2].foffset = j2;
4771 vinfos[2].indices[0] = _ij2[0];
4772 vinfos[2].indices[1] = _ij2[1];
4773 vinfos[2].maxsolutions = _nj2;
4774 vinfos[3].jointtype = 1;
4775 vinfos[3].foffset = j3;
4776 vinfos[3].indices[0] = _ij3[0];
4777 vinfos[3].indices[1] = _ij3[1];
4778 vinfos[3].maxsolutions = _nj3;
4779 vinfos[4].jointtype = 1;
4780 vinfos[4].foffset = j4;
4781 vinfos[4].indices[0] = _ij4[0];
4782 vinfos[4].indices[1] = _ij4[1];
4783 vinfos[4].maxsolutions = _nj4;
4784 vinfos[5].jointtype = 1;
4785 vinfos[5].foffset = j5;
4786 vinfos[5].indices[0] = _ij5[0];
4787 vinfos[5].indices[1] = _ij5[1];
4788 vinfos[5].maxsolutions = _nj5;
4789 std::vector<int> vfree(0);
4808 IkReal j0array[1], cj0array[1], sj0array[1];
4809 bool j0valid[1]={
false};
4813 j0array[0]=
IKatan2(((gconst114)*(((((IkReal(-1.00000000000000))*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)))))), ((gconst114)*(((((cj5)*(r11)))+(((r10)*(sj5)))))));
4814 sj0array[0]=
IKsin(j0array[0]);
4815 cj0array[0]=
IKcos(j0array[0]);
4816 if( j0array[0] >
IKPI )
4820 else if( j0array[0] < -
IKPI )
4821 { j0array[0]+=
IK2PI;
4824 for(
int ij0 = 0; ij0 < 1; ++ij0)
4830 _ij0[0] = ij0; _ij0[1] = -1;
4831 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
4835 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
4838 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
4841 IkReal x1064=
IKsin(j0);
4842 IkReal x1065=
IKcos(j0);
4843 IkReal x1066=((IkReal(1.00000000000000))*(cj5));
4844 IkReal x1067=((r01)*(sj5));
4845 IkReal x1068=((IkReal(1.00000000000000))*(r12));
4846 IkReal x1069=((IkReal(1.00000000000000))*(r10));
4847 IkReal x1070=((r11)*(sj5));
4848 IkReal x1071=((cj4)*(x1064));
4849 IkReal x1072=((sj4)*(x1064));
4850 IkReal x1073=((sj4)*(x1065));
4851 IkReal x1074=((sj5)*(x1064));
4852 IkReal x1075=((cj4)*(x1065));
4853 IkReal x1076=((sj5)*(x1065));
4854 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1069)*(x1076)))+(((IkReal(-1.00000000000000))*(r11)*(x1065)*(x1066)))+(((cj5)*(r01)*(x1064)))+(((r00)*(x1074))));
4855 evalcond[1]=((((IkReal(-1.00000000000000))*(r11)*(x1064)*(x1066)))+(((IkReal(-1.00000000000000))*(r00)*(x1076)))+(((IkReal(-1.00000000000000))*(x1069)*(x1074)))+(((IkReal(-1.00000000000000))*(r01)*(x1065)*(x1066))));
4856 evalcond[2]=((((r02)*(x1071)))+(((IkReal(-1.00000000000000))*(x1068)*(x1075)))+(((x1067)*(x1072)))+(((IkReal(-1.00000000000000))*(x1070)*(x1073)))+(((cj5)*(r10)*(x1073)))+(((IkReal(-1.00000000000000))*(r00)*(x1066)*(x1072))));
4857 evalcond[3]=((((r02)*(x1072)))+(((x1070)*(x1075)))+(((cj5)*(r00)*(x1071)))+(((IkReal(-1.00000000000000))*(r10)*(x1066)*(x1075)))+(((IkReal(-1.00000000000000))*(x1067)*(x1071)))+(((IkReal(-1.00000000000000))*(x1068)*(x1073))));
4858 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
4865 IkReal dummyeval[1];
4867 gconst117=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
4868 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
4869 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4872 IkReal dummyeval[1];
4874 gconst118=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
4875 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
4876 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
4883 IkReal j1array[1], cj1array[1], sj1array[1];
4884 bool j1valid[1]={
false};
4886 IkReal x1077=((cj4)*(r22));
4887 IkReal x1078=((r11)*(sj0));
4888 IkReal x1079=((IkReal(1.00000000000000))*(cj2));
4889 IkReal x1080=((sj2)*(sj4));
4890 IkReal x1081=((IkReal(1.00000000000000))*(sj2));
4891 IkReal x1082=((cj2)*(sj4));
4892 IkReal x1083=((cj0)*(r01));
4893 IkReal x1084=((cj5)*(r20));
4894 IkReal x1085=((IkReal(1.00000000000000))*(sj4)*(sj5));
4895 IkReal x1086=((cj0)*(cj4)*(r02));
4896 IkReal x1087=((cj4)*(r12)*(sj0));
4897 IkReal x1088=((cj5)*(r10)*(sj0));
4898 IkReal x1089=((cj0)*(cj5)*(r00));
4899 if(
IKabs(((gconst118)*(((((IkReal(-1.00000000000000))*(x1079)*(x1087)))+(((x1082)*(x1089)))+(((sj2)*(x1077)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1078)*(x1079)))+(((IkReal(-1.00000000000000))*(x1080)*(x1084)))+(((IkReal(-1.00000000000000))*(x1079)*(x1086)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1079)*(x1083)))+(((r21)*(sj5)*(x1080)))+(((x1082)*(x1088))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst118)*(((((IkReal(-1.00000000000000))*(sj5)*(x1080)*(x1083)))+(((IkReal(-1.00000000000000))*(x1081)*(x1087)))+(((IkReal(-1.00000000000000))*(sj5)*(x1078)*(x1080)))+(((x1082)*(x1084)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1079)))+(((IkReal(-1.00000000000000))*(x1077)*(x1079)))+(((IkReal(-1.00000000000000))*(x1081)*(x1086)))+(((x1080)*(x1088)))+(((x1080)*(x1089))))))) <
IKFAST_ATAN2_MAGTHRESH )
4901 j1array[0]=
IKatan2(((gconst118)*(((((IkReal(-1.00000000000000))*(x1079)*(x1087)))+(((x1082)*(x1089)))+(((sj2)*(x1077)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1078)*(x1079)))+(((IkReal(-1.00000000000000))*(x1080)*(x1084)))+(((IkReal(-1.00000000000000))*(x1079)*(x1086)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1079)*(x1083)))+(((r21)*(sj5)*(x1080)))+(((x1082)*(x1088)))))), ((gconst118)*(((((IkReal(-1.00000000000000))*(sj5)*(x1080)*(x1083)))+(((IkReal(-1.00000000000000))*(x1081)*(x1087)))+(((IkReal(-1.00000000000000))*(sj5)*(x1078)*(x1080)))+(((x1082)*(x1084)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1079)))+(((IkReal(-1.00000000000000))*(x1077)*(x1079)))+(((IkReal(-1.00000000000000))*(x1081)*(x1086)))+(((x1080)*(x1088)))+(((x1080)*(x1089)))))));
4902 sj1array[0]=
IKsin(j1array[0]);
4903 cj1array[0]=
IKcos(j1array[0]);
4904 if( j1array[0] >
IKPI )
4908 else if( j1array[0] < -
IKPI )
4909 { j1array[0]+=
IK2PI;
4912 for(
int ij1 = 0; ij1 < 1; ++ij1)
4918 _ij1[0] = ij1; _ij1[1] = -1;
4919 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
4923 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
4926 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
4929 IkReal x1090=
IKcos(j1);
4930 IkReal x1091=
IKsin(j1);
4931 IkReal x1092=((IkReal(1.00000000000000))*(sj4));
4932 IkReal x1093=((cj5)*(r00));
4933 IkReal x1094=((cj5)*(r20));
4934 IkReal x1095=((r12)*(sj0));
4935 IkReal x1096=((r21)*(sj5));
4936 IkReal x1097=((IkReal(1.00000000000000))*(cj4));
4937 IkReal x1098=((sj2)*(x1090));
4938 IkReal x1099=((cj2)*(x1091));
4939 IkReal x1100=((cj2)*(x1090));
4940 IkReal x1101=((r11)*(sj0)*(sj5));
4941 IkReal x1102=((cj0)*(x1097));
4942 IkReal x1103=((cj0)*(r01)*(sj5));
4943 IkReal x1104=((cj5)*(r10)*(sj0));
4944 IkReal x1105=((sj2)*(x1091));
4945 IkReal x1106=((x1098)+(x1099));
4946 evalcond[0]=((x1105)+(((IkReal(-1.00000000000000))*(x1100)))+(((sj4)*(x1096)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x1092)*(x1094))));
4947 evalcond[1]=((x1106)+(((r22)*(sj4)))+(((cj4)*(x1094)))+(((IkReal(-1.00000000000000))*(x1096)*(x1097))));
4948 evalcond[2]=((((IkReal(-1.00000000000000))*(x1095)*(x1097)))+(x1106)+(((sj4)*(x1104)))+(((IkReal(-1.00000000000000))*(x1092)*(x1101)))+(((IkReal(-1.00000000000000))*(x1092)*(x1103)))+(((cj0)*(sj4)*(x1093)))+(((IkReal(-1.00000000000000))*(r02)*(x1102))));
4949 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1092)))+(x1100)+(((cj4)*(x1101)))+(((IkReal(-1.00000000000000))*(x1093)*(x1102)))+(((IkReal(-1.00000000000000))*(x1092)*(x1095)))+(((IkReal(-1.00000000000000))*(x1097)*(x1104)))+(((cj4)*(x1103)))+(((IkReal(-1.00000000000000))*(x1105))));
4950 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
4957 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4958 vinfos[0].jointtype = 1;
4959 vinfos[0].foffset = j0;
4960 vinfos[0].indices[0] = _ij0[0];
4961 vinfos[0].indices[1] = _ij0[1];
4962 vinfos[0].maxsolutions = _nj0;
4963 vinfos[1].jointtype = 1;
4964 vinfos[1].foffset = j1;
4965 vinfos[1].indices[0] = _ij1[0];
4966 vinfos[1].indices[1] = _ij1[1];
4967 vinfos[1].maxsolutions = _nj1;
4968 vinfos[2].jointtype = 1;
4969 vinfos[2].foffset = j2;
4970 vinfos[2].indices[0] = _ij2[0];
4971 vinfos[2].indices[1] = _ij2[1];
4972 vinfos[2].maxsolutions = _nj2;
4973 vinfos[3].jointtype = 1;
4974 vinfos[3].foffset = j3;
4975 vinfos[3].indices[0] = _ij3[0];
4976 vinfos[3].indices[1] = _ij3[1];
4977 vinfos[3].maxsolutions = _nj3;
4978 vinfos[4].jointtype = 1;
4979 vinfos[4].foffset = j4;
4980 vinfos[4].indices[0] = _ij4[0];
4981 vinfos[4].indices[1] = _ij4[1];
4982 vinfos[4].maxsolutions = _nj4;
4983 vinfos[5].jointtype = 1;
4984 vinfos[5].foffset = j5;
4985 vinfos[5].indices[0] = _ij5[0];
4986 vinfos[5].indices[1] = _ij5[1];
4987 vinfos[5].maxsolutions = _nj5;
4988 std::vector<int> vfree(0);
5001 IkReal j1array[1], cj1array[1], sj1array[1];
5002 bool j1valid[1]={
false};
5004 IkReal x1107=((cj4)*(r22));
5005 IkReal x1108=((cj2)*(sj4));
5006 IkReal x1109=((sj2)*(sj4));
5007 IkReal x1110=((r21)*(sj5));
5008 IkReal x1111=((cj5)*(r20));
5009 IkReal x1112=((cj4)*(sj2));
5010 IkReal x1113=((IkReal(1.00000000000000))*(cj2)*(x1110));
5011 if(
IKabs(((gconst117)*(((((cj2)*(cj4)*(x1111)))+(((IkReal(-1.00000000000000))*(x1109)*(x1111)))+(((IkReal(-1.00000000000000))*(cj4)*(x1113)))+(((sj2)*(x1107)))+(((r22)*(x1108)))+(((x1109)*(x1110))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst117)*(((((IkReal(-1.00000000000000))*(x1108)*(x1110)))+(((IkReal(-1.00000000000000))*(x1110)*(x1112)))+(((IkReal(-1.00000000000000))*(cj2)*(x1107)))+(((x1111)*(x1112)))+(((r22)*(x1109)))+(((x1108)*(x1111))))))) <
IKFAST_ATAN2_MAGTHRESH )
5013 j1array[0]=
IKatan2(((gconst117)*(((((cj2)*(cj4)*(x1111)))+(((IkReal(-1.00000000000000))*(x1109)*(x1111)))+(((IkReal(-1.00000000000000))*(cj4)*(x1113)))+(((sj2)*(x1107)))+(((r22)*(x1108)))+(((x1109)*(x1110)))))), ((gconst117)*(((((IkReal(-1.00000000000000))*(x1108)*(x1110)))+(((IkReal(-1.00000000000000))*(x1110)*(x1112)))+(((IkReal(-1.00000000000000))*(cj2)*(x1107)))+(((x1111)*(x1112)))+(((r22)*(x1109)))+(((x1108)*(x1111)))))));
5014 sj1array[0]=
IKsin(j1array[0]);
5015 cj1array[0]=
IKcos(j1array[0]);
5016 if( j1array[0] >
IKPI )
5020 else if( j1array[0] < -
IKPI )
5021 { j1array[0]+=
IK2PI;
5024 for(
int ij1 = 0; ij1 < 1; ++ij1)
5030 _ij1[0] = ij1; _ij1[1] = -1;
5031 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
5035 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
5038 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
5041 IkReal x1114=
IKcos(j1);
5042 IkReal x1115=
IKsin(j1);
5043 IkReal x1116=((IkReal(1.00000000000000))*(sj4));
5044 IkReal x1117=((cj5)*(r00));
5045 IkReal x1118=((cj5)*(r20));
5046 IkReal x1119=((r12)*(sj0));
5047 IkReal x1120=((r21)*(sj5));
5048 IkReal x1121=((IkReal(1.00000000000000))*(cj4));
5049 IkReal x1122=((sj2)*(x1114));
5050 IkReal x1123=((cj2)*(x1115));
5051 IkReal x1124=((cj2)*(x1114));
5052 IkReal x1125=((r11)*(sj0)*(sj5));
5053 IkReal x1126=((cj0)*(x1121));
5054 IkReal x1127=((cj0)*(r01)*(sj5));
5055 IkReal x1128=((cj5)*(r10)*(sj0));
5056 IkReal x1129=((sj2)*(x1115));
5057 IkReal x1130=((x1122)+(x1123));
5058 evalcond[0]=((((IkReal(-1.00000000000000))*(x1116)*(x1118)))+(((cj4)*(r22)))+(x1129)+(((sj4)*(x1120)))+(((IkReal(-1.00000000000000))*(x1124))));
5059 evalcond[1]=((((IkReal(-1.00000000000000))*(x1120)*(x1121)))+(x1130)+(((r22)*(sj4)))+(((cj4)*(x1118))));
5060 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x1126)))+(((IkReal(-1.00000000000000))*(x1119)*(x1121)))+(((cj0)*(sj4)*(x1117)))+(x1130)+(((IkReal(-1.00000000000000))*(x1116)*(x1127)))+(((sj4)*(x1128)))+(((IkReal(-1.00000000000000))*(x1116)*(x1125))));
5061 evalcond[3]=((((IkReal(-1.00000000000000))*(x1116)*(x1119)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1116)))+(((cj4)*(x1125)))+(((cj4)*(x1127)))+(((IkReal(-1.00000000000000))*(x1117)*(x1126)))+(((IkReal(-1.00000000000000))*(x1129)))+(x1124)+(((IkReal(-1.00000000000000))*(x1121)*(x1128))));
5062 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5069 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5070 vinfos[0].jointtype = 1;
5071 vinfos[0].foffset = j0;
5072 vinfos[0].indices[0] = _ij0[0];
5073 vinfos[0].indices[1] = _ij0[1];
5074 vinfos[0].maxsolutions = _nj0;
5075 vinfos[1].jointtype = 1;
5076 vinfos[1].foffset = j1;
5077 vinfos[1].indices[0] = _ij1[0];
5078 vinfos[1].indices[1] = _ij1[1];
5079 vinfos[1].maxsolutions = _nj1;
5080 vinfos[2].jointtype = 1;
5081 vinfos[2].foffset = j2;
5082 vinfos[2].indices[0] = _ij2[0];
5083 vinfos[2].indices[1] = _ij2[1];
5084 vinfos[2].maxsolutions = _nj2;
5085 vinfos[3].jointtype = 1;
5086 vinfos[3].foffset = j3;
5087 vinfos[3].indices[0] = _ij3[0];
5088 vinfos[3].indices[1] = _ij3[1];
5089 vinfos[3].maxsolutions = _nj3;
5090 vinfos[4].jointtype = 1;
5091 vinfos[4].foffset = j4;
5092 vinfos[4].indices[0] = _ij4[0];
5093 vinfos[4].indices[1] = _ij4[1];
5094 vinfos[4].maxsolutions = _nj4;
5095 vinfos[5].jointtype = 1;
5096 vinfos[5].foffset = j5;
5097 vinfos[5].indices[0] = _ij5[0];
5098 vinfos[5].indices[1] = _ij5[1];
5099 vinfos[5].maxsolutions = _nj5;
5100 std::vector<int> vfree(0);
5118 IkReal x1131=((cj5)*(npx));
5119 IkReal x1132=((npy)*(sj5));
5120 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(j3, IkReal(6.28318530717959))));
5121 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
5122 evalcond[2]=((IkReal(-0.350000000000000))+(((sj4)*(x1132)))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((IkReal(-1.00000000000000))*(sj4)*(x1131))));
5123 evalcond[3]=((((cj4)*(x1131)))+(((IkReal(-0.350000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj4)*(x1132)))+(((npz)*(sj4))));
5124 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
5125 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 )
5128 IkReal dummyeval[1];
5130 IkReal x1133=(cj5)*(cj5);
5131 IkReal x1134=(sj5)*(sj5);
5132 IkReal x1135=((IkReal(1.00000000000000))*(x1134));
5133 IkReal x1136=((IkReal(2.00000000000000))*(cj5)*(sj5));
5134 IkReal x1137=((IkReal(1.00000000000000))*(x1133));
5135 gconst121=
IKsign(((((IkReal(-1.00000000000000))*(x1135)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1137)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1136)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1136)))+(((IkReal(-1.00000000000000))*(x1137)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1135)*((r00)*(r00))))));
5136 IkReal x1138=(cj5)*(cj5);
5137 IkReal x1139=(sj5)*(sj5);
5138 IkReal x1140=((IkReal(1.00000000000000))*(x1139));
5139 IkReal x1141=((IkReal(2.00000000000000))*(cj5)*(sj5));
5140 IkReal x1142=((IkReal(1.00000000000000))*(x1138));
5141 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1140)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1142)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1140)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1142)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1141)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1141))));
5142 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5145 IkReal dummyeval[1];
5147 gconst123=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
5148 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
5149 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5152 IkReal dummyeval[1];
5154 IkReal x1143=(sj5)*(sj5);
5155 IkReal x1144=(cj5)*(cj5);
5156 IkReal x1145=((cj4)*(cj5));
5157 IkReal x1146=((IkReal(1.00000000000000))*(r12));
5158 IkReal x1147=((cj4)*(sj5));
5159 IkReal x1148=((r01)*(r10));
5160 IkReal x1149=((sj4)*(x1144));
5161 IkReal x1150=((IkReal(1.00000000000000))*(r00)*(r11));
5162 IkReal x1151=((sj4)*(x1143));
5163 gconst122=
IKsign(((((IkReal(-1.00000000000000))*(r01)*(x1145)*(x1146)))+(((IkReal(-1.00000000000000))*(x1150)*(x1151)))+(((r02)*(r10)*(x1147)))+(((r02)*(r11)*(x1145)))+(((x1148)*(x1149)))+(((IkReal(-1.00000000000000))*(x1149)*(x1150)))+(((IkReal(-1.00000000000000))*(r00)*(x1146)*(x1147)))+(((x1148)*(x1151)))));
5164 IkReal x1152=(sj5)*(sj5);
5165 IkReal x1153=(cj5)*(cj5);
5166 IkReal x1154=((cj4)*(cj5));
5167 IkReal x1155=((IkReal(1.00000000000000))*(r12));
5168 IkReal x1156=((cj4)*(sj5));
5169 IkReal x1157=((r01)*(r10));
5170 IkReal x1158=((sj4)*(x1153));
5171 IkReal x1159=((IkReal(1.00000000000000))*(r00)*(r11));
5172 IkReal x1160=((sj4)*(x1152));
5173 dummyeval[0]=((((x1157)*(x1160)))+(((r02)*(r10)*(x1156)))+(((r02)*(r11)*(x1154)))+(((IkReal(-1.00000000000000))*(r01)*(x1154)*(x1155)))+(((IkReal(-1.00000000000000))*(x1159)*(x1160)))+(((x1157)*(x1158)))+(((IkReal(-1.00000000000000))*(r00)*(x1155)*(x1156)))+(((IkReal(-1.00000000000000))*(x1158)*(x1159))));
5174 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5181 IkReal j0array[1], cj0array[1], sj0array[1];
5182 bool j0valid[1]={
false};
5184 IkReal x1161=((cj5)*(sj4));
5185 IkReal x1162=((IkReal(1.00000000000000))*(cj4));
5186 IkReal x1163=((IkReal(1.00000000000000))*(sj4)*(sj5));
5187 if(
IKabs(((gconst122)*(((((r10)*(x1161)))+(((IkReal(-1.00000000000000))*(r11)*(x1163)))+(((IkReal(-1.00000000000000))*(r12)*(x1162))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst122)*(((((IkReal(-1.00000000000000))*(r01)*(x1163)))+(((r00)*(x1161)))+(((IkReal(-1.00000000000000))*(r02)*(x1162))))))) <
IKFAST_ATAN2_MAGTHRESH )
5189 j0array[0]=
IKatan2(((gconst122)*(((((r10)*(x1161)))+(((IkReal(-1.00000000000000))*(r11)*(x1163)))+(((IkReal(-1.00000000000000))*(r12)*(x1162)))))), ((gconst122)*(((((IkReal(-1.00000000000000))*(r01)*(x1163)))+(((r00)*(x1161)))+(((IkReal(-1.00000000000000))*(r02)*(x1162)))))));
5190 sj0array[0]=
IKsin(j0array[0]);
5191 cj0array[0]=
IKcos(j0array[0]);
5192 if( j0array[0] >
IKPI )
5196 else if( j0array[0] < -
IKPI )
5197 { j0array[0]+=
IK2PI;
5200 for(
int ij0 = 0; ij0 < 1; ++ij0)
5206 _ij0[0] = ij0; _ij0[1] = -1;
5207 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
5211 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
5214 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
5217 IkReal x1164=
IKsin(j0);
5218 IkReal x1165=
IKcos(j0);
5219 IkReal x1166=((IkReal(1.00000000000000))*(cj5));
5220 IkReal x1167=((r01)*(sj5));
5221 IkReal x1168=((IkReal(1.00000000000000))*(r12));
5222 IkReal x1169=((IkReal(1.00000000000000))*(r10));
5223 IkReal x1170=((r11)*(sj5));
5224 IkReal x1171=((cj4)*(x1164));
5225 IkReal x1172=((sj4)*(x1164));
5226 IkReal x1173=((sj4)*(x1165));
5227 IkReal x1174=((sj5)*(x1164));
5228 IkReal x1175=((cj4)*(x1165));
5229 IkReal x1176=((sj5)*(x1165));
5230 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1169)*(x1176)))+(((IkReal(-1.00000000000000))*(r11)*(x1165)*(x1166)))+(((r00)*(x1174)))+(((cj5)*(r01)*(x1164))));
5231 evalcond[1]=((((IkReal(-1.00000000000000))*(x1169)*(x1174)))+(((IkReal(-1.00000000000000))*(r00)*(x1176)))+(((IkReal(-1.00000000000000))*(r11)*(x1164)*(x1166)))+(((IkReal(-1.00000000000000))*(r01)*(x1165)*(x1166))));
5232 evalcond[2]=((((IkReal(-1.00000000000000))*(x1170)*(x1173)))+(((IkReal(-1.00000000000000))*(x1168)*(x1175)))+(((r02)*(x1171)))+(((cj5)*(r10)*(x1173)))+(((IkReal(-1.00000000000000))*(r00)*(x1166)*(x1172)))+(((x1167)*(x1172))));
5233 evalcond[3]=((((x1170)*(x1175)))+(((IkReal(-1.00000000000000))*(r10)*(x1166)*(x1175)))+(((cj5)*(r00)*(x1171)))+(((IkReal(-1.00000000000000))*(x1168)*(x1173)))+(((IkReal(-1.00000000000000))*(x1167)*(x1171)))+(((r02)*(x1172))));
5234 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5241 IkReal dummyeval[1];
5243 gconst124=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
5244 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
5245 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5248 IkReal dummyeval[1];
5250 gconst125=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
5251 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
5252 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5259 IkReal j1array[1], cj1array[1], sj1array[1];
5260 bool j1valid[1]={
false};
5262 IkReal x1177=((cj4)*(r22));
5263 IkReal x1178=((r11)*(sj0));
5264 IkReal x1179=((IkReal(1.00000000000000))*(cj2));
5265 IkReal x1180=((sj2)*(sj4));
5266 IkReal x1181=((IkReal(1.00000000000000))*(sj2));
5267 IkReal x1182=((cj2)*(sj4));
5268 IkReal x1183=((cj0)*(r01));
5269 IkReal x1184=((cj5)*(r20));
5270 IkReal x1185=((IkReal(1.00000000000000))*(sj4)*(sj5));
5271 IkReal x1186=((cj0)*(cj4)*(r02));
5272 IkReal x1187=((cj4)*(r12)*(sj0));
5273 IkReal x1188=((cj5)*(r10)*(sj0));
5274 IkReal x1189=((cj0)*(cj5)*(r00));
5275 if(
IKabs(((gconst125)*(((((sj2)*(x1177)))+(((IkReal(-1.00000000000000))*(x1179)*(x1187)))+(((r21)*(sj5)*(x1180)))+(((x1182)*(x1188)))+(((IkReal(-1.00000000000000))*(x1180)*(x1184)))+(((x1182)*(x1189)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1179)*(x1183)))+(((IkReal(-1.00000000000000))*(x1179)*(x1186)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1178)*(x1179))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst125)*(((((IkReal(-1.00000000000000))*(sj5)*(x1178)*(x1180)))+(((IkReal(-1.00000000000000))*(x1181)*(x1187)))+(((IkReal(-1.00000000000000))*(x1177)*(x1179)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1179)))+(((x1180)*(x1189)))+(((x1182)*(x1184)))+(((IkReal(-1.00000000000000))*(x1181)*(x1186)))+(((IkReal(-1.00000000000000))*(sj5)*(x1180)*(x1183)))+(((x1180)*(x1188))))))) <
IKFAST_ATAN2_MAGTHRESH )
5277 j1array[0]=
IKatan2(((gconst125)*(((((sj2)*(x1177)))+(((IkReal(-1.00000000000000))*(x1179)*(x1187)))+(((r21)*(sj5)*(x1180)))+(((x1182)*(x1188)))+(((IkReal(-1.00000000000000))*(x1180)*(x1184)))+(((x1182)*(x1189)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1179)*(x1183)))+(((IkReal(-1.00000000000000))*(x1179)*(x1186)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1178)*(x1179)))))), ((gconst125)*(((((IkReal(-1.00000000000000))*(sj5)*(x1178)*(x1180)))+(((IkReal(-1.00000000000000))*(x1181)*(x1187)))+(((IkReal(-1.00000000000000))*(x1177)*(x1179)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1179)))+(((x1180)*(x1189)))+(((x1182)*(x1184)))+(((IkReal(-1.00000000000000))*(x1181)*(x1186)))+(((IkReal(-1.00000000000000))*(sj5)*(x1180)*(x1183)))+(((x1180)*(x1188)))))));
5278 sj1array[0]=
IKsin(j1array[0]);
5279 cj1array[0]=
IKcos(j1array[0]);
5280 if( j1array[0] >
IKPI )
5284 else if( j1array[0] < -
IKPI )
5285 { j1array[0]+=
IK2PI;
5288 for(
int ij1 = 0; ij1 < 1; ++ij1)
5294 _ij1[0] = ij1; _ij1[1] = -1;
5295 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
5299 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
5302 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
5305 IkReal x1190=
IKcos(j1);
5306 IkReal x1191=
IKsin(j1);
5307 IkReal x1192=((IkReal(1.00000000000000))*(sj4));
5308 IkReal x1193=((cj5)*(r00));
5309 IkReal x1194=((cj5)*(r20));
5310 IkReal x1195=((r12)*(sj0));
5311 IkReal x1196=((r21)*(sj5));
5312 IkReal x1197=((IkReal(1.00000000000000))*(cj4));
5313 IkReal x1198=((sj2)*(x1191));
5314 IkReal x1199=((IkReal(1.00000000000000))*(x1190));
5315 IkReal x1200=((r11)*(sj0)*(sj5));
5316 IkReal x1201=((cj0)*(x1197));
5317 IkReal x1202=((cj0)*(r01)*(sj5));
5318 IkReal x1203=((cj2)*(x1191));
5319 IkReal x1204=((cj5)*(r10)*(sj0));
5320 IkReal x1205=((cj2)*(x1199));
5321 evalcond[0]=((((IkReal(-1.00000000000000))*(x1205)))+(((sj4)*(x1196)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x1192)*(x1194)))+(x1198));
5322 evalcond[1]=((((IkReal(-1.00000000000000))*(x1196)*(x1197)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(x1203)))+(((IkReal(-1.00000000000000))*(sj2)*(x1199)))+(((cj4)*(x1194))));
5323 evalcond[2]=((((IkReal(-1.00000000000000))*(x1195)*(x1197)))+(((IkReal(-1.00000000000000))*(r02)*(x1201)))+(((sj2)*(x1190)))+(((IkReal(-1.00000000000000))*(x1192)*(x1200)))+(((sj4)*(x1204)))+(x1203)+(((cj0)*(sj4)*(x1193)))+(((IkReal(-1.00000000000000))*(x1192)*(x1202))));
5324 evalcond[3]=((((IkReal(-1.00000000000000))*(x1205)))+(((cj4)*(x1200)))+(((IkReal(-1.00000000000000))*(x1197)*(x1204)))+(x1198)+(((IkReal(-1.00000000000000))*(x1193)*(x1201)))+(((IkReal(-1.00000000000000))*(x1192)*(x1195)))+(((cj4)*(x1202)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1192))));
5325 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5332 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5333 vinfos[0].jointtype = 1;
5334 vinfos[0].foffset = j0;
5335 vinfos[0].indices[0] = _ij0[0];
5336 vinfos[0].indices[1] = _ij0[1];
5337 vinfos[0].maxsolutions = _nj0;
5338 vinfos[1].jointtype = 1;
5339 vinfos[1].foffset = j1;
5340 vinfos[1].indices[0] = _ij1[0];
5341 vinfos[1].indices[1] = _ij1[1];
5342 vinfos[1].maxsolutions = _nj1;
5343 vinfos[2].jointtype = 1;
5344 vinfos[2].foffset = j2;
5345 vinfos[2].indices[0] = _ij2[0];
5346 vinfos[2].indices[1] = _ij2[1];
5347 vinfos[2].maxsolutions = _nj2;
5348 vinfos[3].jointtype = 1;
5349 vinfos[3].foffset = j3;
5350 vinfos[3].indices[0] = _ij3[0];
5351 vinfos[3].indices[1] = _ij3[1];
5352 vinfos[3].maxsolutions = _nj3;
5353 vinfos[4].jointtype = 1;
5354 vinfos[4].foffset = j4;
5355 vinfos[4].indices[0] = _ij4[0];
5356 vinfos[4].indices[1] = _ij4[1];
5357 vinfos[4].maxsolutions = _nj4;
5358 vinfos[5].jointtype = 1;
5359 vinfos[5].foffset = j5;
5360 vinfos[5].indices[0] = _ij5[0];
5361 vinfos[5].indices[1] = _ij5[1];
5362 vinfos[5].maxsolutions = _nj5;
5363 std::vector<int> vfree(0);
5376 IkReal j1array[1], cj1array[1], sj1array[1];
5377 bool j1valid[1]={
false};
5379 IkReal x1206=((IkReal(1.00000000000000))*(cj2));
5380 IkReal x1207=((cj2)*(r22));
5381 IkReal x1208=((r21)*(sj5));
5382 IkReal x1209=((r22)*(sj2));
5383 IkReal x1210=((cj5)*(r20)*(sj4));
5384 IkReal x1211=((cj4)*(cj5)*(r20));
5385 IkReal x1212=((IkReal(1.00000000000000))*(sj2)*(x1208));
5386 if(
IKabs(((gconst124)*(((((IkReal(-1.00000000000000))*(sj4)*(x1212)))+(((sj2)*(x1210)))+(((IkReal(-1.00000000000000))*(cj4)*(x1206)*(x1208)))+(((sj4)*(x1207)))+(((IkReal(-1.00000000000000))*(cj4)*(x1209)))+(((cj2)*(x1211))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst124)*(((((sj4)*(x1209)))+(((cj4)*(x1207)))+(((sj2)*(x1211)))+(((IkReal(-1.00000000000000))*(x1206)*(x1210)))+(((IkReal(-1.00000000000000))*(cj4)*(x1212)))+(((cj2)*(sj4)*(x1208))))))) <
IKFAST_ATAN2_MAGTHRESH )
5388 j1array[0]=
IKatan2(((gconst124)*(((((IkReal(-1.00000000000000))*(sj4)*(x1212)))+(((sj2)*(x1210)))+(((IkReal(-1.00000000000000))*(cj4)*(x1206)*(x1208)))+(((sj4)*(x1207)))+(((IkReal(-1.00000000000000))*(cj4)*(x1209)))+(((cj2)*(x1211)))))), ((gconst124)*(((((sj4)*(x1209)))+(((cj4)*(x1207)))+(((sj2)*(x1211)))+(((IkReal(-1.00000000000000))*(x1206)*(x1210)))+(((IkReal(-1.00000000000000))*(cj4)*(x1212)))+(((cj2)*(sj4)*(x1208)))))));
5389 sj1array[0]=
IKsin(j1array[0]);
5390 cj1array[0]=
IKcos(j1array[0]);
5391 if( j1array[0] >
IKPI )
5395 else if( j1array[0] < -
IKPI )
5396 { j1array[0]+=
IK2PI;
5399 for(
int ij1 = 0; ij1 < 1; ++ij1)
5405 _ij1[0] = ij1; _ij1[1] = -1;
5406 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
5410 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
5413 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
5416 IkReal x1213=
IKcos(j1);
5417 IkReal x1214=
IKsin(j1);
5418 IkReal x1215=((IkReal(1.00000000000000))*(sj4));
5419 IkReal x1216=((cj5)*(r00));
5420 IkReal x1217=((cj5)*(r20));
5421 IkReal x1218=((r12)*(sj0));
5422 IkReal x1219=((r21)*(sj5));
5423 IkReal x1220=((IkReal(1.00000000000000))*(cj4));
5424 IkReal x1221=((sj2)*(x1214));
5425 IkReal x1222=((IkReal(1.00000000000000))*(x1213));
5426 IkReal x1223=((r11)*(sj0)*(sj5));
5427 IkReal x1224=((cj0)*(x1220));
5428 IkReal x1225=((cj0)*(r01)*(sj5));
5429 IkReal x1226=((cj2)*(x1214));
5430 IkReal x1227=((cj5)*(r10)*(sj0));
5431 IkReal x1228=((cj2)*(x1222));
5432 evalcond[0]=((((sj4)*(x1219)))+(x1221)+(((IkReal(-1.00000000000000))*(x1228)))+(((IkReal(-1.00000000000000))*(x1215)*(x1217)))+(((cj4)*(r22))));
5433 evalcond[1]=((((IkReal(-1.00000000000000))*(sj2)*(x1222)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(x1226)))+(((cj4)*(x1217)))+(((IkReal(-1.00000000000000))*(x1219)*(x1220))));
5434 evalcond[2]=((((IkReal(-1.00000000000000))*(x1218)*(x1220)))+(x1226)+(((IkReal(-1.00000000000000))*(x1215)*(x1225)))+(((sj4)*(x1227)))+(((IkReal(-1.00000000000000))*(r02)*(x1224)))+(((sj2)*(x1213)))+(((IkReal(-1.00000000000000))*(x1215)*(x1223)))+(((cj0)*(sj4)*(x1216))));
5435 evalcond[3]=((((cj4)*(x1223)))+(((IkReal(-1.00000000000000))*(x1215)*(x1218)))+(x1221)+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1215)))+(((IkReal(-1.00000000000000))*(x1216)*(x1224)))+(((IkReal(-1.00000000000000))*(x1228)))+(((IkReal(-1.00000000000000))*(x1220)*(x1227)))+(((cj4)*(x1225))));
5436 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5443 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5444 vinfos[0].jointtype = 1;
5445 vinfos[0].foffset = j0;
5446 vinfos[0].indices[0] = _ij0[0];
5447 vinfos[0].indices[1] = _ij0[1];
5448 vinfos[0].maxsolutions = _nj0;
5449 vinfos[1].jointtype = 1;
5450 vinfos[1].foffset = j1;
5451 vinfos[1].indices[0] = _ij1[0];
5452 vinfos[1].indices[1] = _ij1[1];
5453 vinfos[1].maxsolutions = _nj1;
5454 vinfos[2].jointtype = 1;
5455 vinfos[2].foffset = j2;
5456 vinfos[2].indices[0] = _ij2[0];
5457 vinfos[2].indices[1] = _ij2[1];
5458 vinfos[2].maxsolutions = _nj2;
5459 vinfos[3].jointtype = 1;
5460 vinfos[3].foffset = j3;
5461 vinfos[3].indices[0] = _ij3[0];
5462 vinfos[3].indices[1] = _ij3[1];
5463 vinfos[3].maxsolutions = _nj3;
5464 vinfos[4].jointtype = 1;
5465 vinfos[4].foffset = j4;
5466 vinfos[4].indices[0] = _ij4[0];
5467 vinfos[4].indices[1] = _ij4[1];
5468 vinfos[4].maxsolutions = _nj4;
5469 vinfos[5].jointtype = 1;
5470 vinfos[5].foffset = j5;
5471 vinfos[5].indices[0] = _ij5[0];
5472 vinfos[5].indices[1] = _ij5[1];
5473 vinfos[5].maxsolutions = _nj5;
5474 std::vector<int> vfree(0);
5493 IkReal j1array[1], cj1array[1], sj1array[1];
5494 bool j1valid[1]={
false};
5496 IkReal x1229=((IkReal(1.00000000000000))*(cj2));
5497 IkReal x1230=((cj2)*(r22));
5498 IkReal x1231=((r21)*(sj5));
5499 IkReal x1232=((r22)*(sj2));
5500 IkReal x1233=((cj5)*(r20)*(sj4));
5501 IkReal x1234=((cj4)*(cj5)*(r20));
5502 IkReal x1235=((IkReal(1.00000000000000))*(sj2)*(x1231));
5503 if(
IKabs(((gconst123)*(((((IkReal(-1.00000000000000))*(cj4)*(x1229)*(x1231)))+(((cj2)*(x1234)))+(((IkReal(-1.00000000000000))*(cj4)*(x1232)))+(((sj4)*(x1230)))+(((IkReal(-1.00000000000000))*(sj4)*(x1235)))+(((sj2)*(x1233))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst123)*(((((IkReal(-1.00000000000000))*(cj4)*(x1235)))+(((IkReal(-1.00000000000000))*(x1229)*(x1233)))+(((sj4)*(x1232)))+(((cj2)*(sj4)*(x1231)))+(((sj2)*(x1234)))+(((cj4)*(x1230))))))) <
IKFAST_ATAN2_MAGTHRESH )
5505 j1array[0]=
IKatan2(((gconst123)*(((((IkReal(-1.00000000000000))*(cj4)*(x1229)*(x1231)))+(((cj2)*(x1234)))+(((IkReal(-1.00000000000000))*(cj4)*(x1232)))+(((sj4)*(x1230)))+(((IkReal(-1.00000000000000))*(sj4)*(x1235)))+(((sj2)*(x1233)))))), ((gconst123)*(((((IkReal(-1.00000000000000))*(cj4)*(x1235)))+(((IkReal(-1.00000000000000))*(x1229)*(x1233)))+(((sj4)*(x1232)))+(((cj2)*(sj4)*(x1231)))+(((sj2)*(x1234)))+(((cj4)*(x1230)))))));
5506 sj1array[0]=
IKsin(j1array[0]);
5507 cj1array[0]=
IKcos(j1array[0]);
5508 if( j1array[0] >
IKPI )
5512 else if( j1array[0] < -
IKPI )
5513 { j1array[0]+=
IK2PI;
5516 for(
int ij1 = 0; ij1 < 1; ++ij1)
5522 _ij1[0] = ij1; _ij1[1] = -1;
5523 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
5527 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
5530 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
5533 IkReal x1236=
IKcos(j1);
5534 IkReal x1237=
IKsin(j1);
5535 IkReal x1238=((cj5)*(r20));
5536 IkReal x1239=((r21)*(sj5));
5537 IkReal x1240=((IkReal(1.00000000000000))*(x1236));
5538 evalcond[0]=((((IkReal(-1.00000000000000))*(sj4)*(x1238)))+(((IkReal(-1.00000000000000))*(cj2)*(x1240)))+(((cj4)*(r22)))+(((sj4)*(x1239)))+(((sj2)*(x1237))));
5539 evalcond[1]=((((IkReal(-1.00000000000000))*(cj2)*(x1237)))+(((cj4)*(x1238)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(sj2)*(x1240)))+(((IkReal(-1.00000000000000))*(cj4)*(x1239))));
5540 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 )
5547 IkReal dummyeval[1];
5549 IkReal x1241=(cj5)*(cj5);
5550 IkReal x1242=(sj5)*(sj5);
5551 IkReal x1243=((IkReal(1.00000000000000))*(x1242));
5552 IkReal x1244=((IkReal(2.00000000000000))*(cj5)*(sj5));
5553 IkReal x1245=((IkReal(1.00000000000000))*(x1241));
5554 gconst126=
IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1244)))+(((IkReal(-1.00000000000000))*(x1245)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1243)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1244)))+(((IkReal(-1.00000000000000))*(x1245)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1243)*((r10)*(r10))))));
5555 IkReal x1246=(cj5)*(cj5);
5556 IkReal x1247=(sj5)*(sj5);
5557 IkReal x1248=((IkReal(1.00000000000000))*(x1247));
5558 IkReal x1249=((IkReal(2.00000000000000))*(cj5)*(sj5));
5559 IkReal x1250=((IkReal(1.00000000000000))*(x1246));
5560 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r01)*(x1249)))+(((IkReal(-1.00000000000000))*(x1248)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1249)))+(((IkReal(-1.00000000000000))*(x1248)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1250)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1250)*((r11)*(r11)))));
5561 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5564 IkReal dummyeval[1];
5566 IkReal x1251=(sj5)*(sj5);
5567 IkReal x1252=(cj5)*(cj5);
5568 IkReal x1253=((cj4)*(cj5));
5569 IkReal x1254=((IkReal(1.00000000000000))*(r12));
5570 IkReal x1255=((cj4)*(sj5));
5571 IkReal x1256=((r01)*(r10));
5572 IkReal x1257=((sj4)*(x1252));
5573 IkReal x1258=((IkReal(1.00000000000000))*(r00)*(r11));
5574 IkReal x1259=((sj4)*(x1251));
5575 gconst127=
IKsign(((((IkReal(-1.00000000000000))*(x1257)*(x1258)))+(((x1256)*(x1259)))+(((IkReal(-1.00000000000000))*(x1258)*(x1259)))+(((r02)*(r11)*(x1253)))+(((r02)*(r10)*(x1255)))+(((x1256)*(x1257)))+(((IkReal(-1.00000000000000))*(r00)*(x1254)*(x1255)))+(((IkReal(-1.00000000000000))*(r01)*(x1253)*(x1254)))));
5576 IkReal x1260=(sj5)*(sj5);
5577 IkReal x1261=(cj5)*(cj5);
5578 IkReal x1262=((cj4)*(cj5));
5579 IkReal x1263=((IkReal(1.00000000000000))*(r12));
5580 IkReal x1264=((cj4)*(sj5));
5581 IkReal x1265=((r01)*(r10));
5582 IkReal x1266=((sj4)*(x1261));
5583 IkReal x1267=((IkReal(1.00000000000000))*(r00)*(r11));
5584 IkReal x1268=((sj4)*(x1260));
5585 dummyeval[0]=((((IkReal(-1.00000000000000))*(r01)*(x1262)*(x1263)))+(((IkReal(-1.00000000000000))*(r00)*(x1263)*(x1264)))+(((IkReal(-1.00000000000000))*(x1266)*(x1267)))+(((r02)*(r11)*(x1262)))+(((IkReal(-1.00000000000000))*(x1267)*(x1268)))+(((x1265)*(x1268)))+(((r02)*(r10)*(x1264)))+(((x1265)*(x1266))));
5586 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5593 IkReal j0array[1], cj0array[1], sj0array[1];
5594 bool j0valid[1]={
false};
5596 IkReal x1269=((cj5)*(sj4));
5597 IkReal x1270=((IkReal(1.00000000000000))*(cj4));
5598 IkReal x1271=((IkReal(1.00000000000000))*(sj4)*(sj5));
5599 if(
IKabs(((gconst127)*(((((IkReal(-1.00000000000000))*(r11)*(x1271)))+(((r10)*(x1269)))+(((IkReal(-1.00000000000000))*(r12)*(x1270))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst127)*(((((IkReal(-1.00000000000000))*(r02)*(x1270)))+(((r00)*(x1269)))+(((IkReal(-1.00000000000000))*(r01)*(x1271))))))) <
IKFAST_ATAN2_MAGTHRESH )
5601 j0array[0]=
IKatan2(((gconst127)*(((((IkReal(-1.00000000000000))*(r11)*(x1271)))+(((r10)*(x1269)))+(((IkReal(-1.00000000000000))*(r12)*(x1270)))))), ((gconst127)*(((((IkReal(-1.00000000000000))*(r02)*(x1270)))+(((r00)*(x1269)))+(((IkReal(-1.00000000000000))*(r01)*(x1271)))))));
5602 sj0array[0]=
IKsin(j0array[0]);
5603 cj0array[0]=
IKcos(j0array[0]);
5604 if( j0array[0] >
IKPI )
5608 else if( j0array[0] < -
IKPI )
5609 { j0array[0]+=
IK2PI;
5612 for(
int ij0 = 0; ij0 < 1; ++ij0)
5618 _ij0[0] = ij0; _ij0[1] = -1;
5619 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
5623 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
5626 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
5629 IkReal x1272=
IKsin(j0);
5630 IkReal x1273=
IKcos(j0);
5631 IkReal x1274=((IkReal(1.00000000000000))*(r10));
5632 IkReal x1275=((IkReal(1.00000000000000))*(sj4));
5633 IkReal x1276=((cj5)*(r01));
5634 IkReal x1277=((cj5)*(r00));
5635 IkReal x1278=((cj4)*(r01));
5636 IkReal x1279=((IkReal(1.00000000000000))*(cj4));
5637 IkReal x1280=((cj4)*(r11));
5638 IkReal x1281=((sj5)*(x1272));
5639 IkReal x1282=((r11)*(x1275));
5640 IkReal x1283=((sj5)*(x1273));
5641 IkReal x1284=((r02)*(x1273));
5642 IkReal x1285=((r12)*(x1273));
5643 IkReal x1286=((IkReal(1.00000000000000))*(x1273));
5644 IkReal x1287=((cj4)*(x1272));
5645 IkReal x1288=((cj5)*(x1272));
5646 IkReal x1289=((cj5)*(sj4)*(x1273));
5647 evalcond[0]=((IkReal(-1.00000000000000))+(((r00)*(x1281)))+(((IkReal(-1.00000000000000))*(x1274)*(x1283)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1286)))+(((x1272)*(x1276))));
5648 evalcond[1]=((((IkReal(-1.00000000000000))*(x1274)*(x1281)))+(((IkReal(-1.00000000000000))*(x1276)*(x1286)))+(((IkReal(-1.00000000000000))*(r00)*(x1283)))+(((IkReal(-1.00000000000000))*(r11)*(x1288))));
5649 evalcond[2]=((((IkReal(-1.00000000000000))*(x1282)*(x1283)))+(((r10)*(x1289)))+(((r02)*(x1287)))+(((r01)*(sj4)*(x1281)))+(((IkReal(-1.00000000000000))*(x1279)*(x1285)))+(((IkReal(-1.00000000000000))*(x1272)*(x1275)*(x1277))));
5650 evalcond[3]=((((x1277)*(x1287)))+(((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x1273)*(x1274)))+(((r02)*(sj4)*(x1272)))+(((IkReal(-1.00000000000000))*(x1275)*(x1285)))+(((x1280)*(x1283)))+(((IkReal(-1.00000000000000))*(x1278)*(x1281))));
5651 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1279)*(x1284)))+(((r10)*(sj4)*(x1288)))+(((IkReal(-1.00000000000000))*(x1281)*(x1282)))+(((sj4)*(x1273)*(x1277)))+(((IkReal(-1.00000000000000))*(r12)*(x1272)*(x1279)))+(((IkReal(-1.00000000000000))*(r01)*(x1275)*(x1283)))+(((cj2)*(sj1))));
5652 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x1274)*(x1287)))+(((IkReal(-1.00000000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x1273)*(x1277)*(x1279)))+(((IkReal(-1.00000000000000))*(x1275)*(x1284)))+(((IkReal(-1.00000000000000))*(r12)*(x1272)*(x1275)))+(((x1278)*(x1283)))+(((sj1)*(sj2)))+(((x1280)*(x1281))));
5653 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
5660 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5661 vinfos[0].jointtype = 1;
5662 vinfos[0].foffset = j0;
5663 vinfos[0].indices[0] = _ij0[0];
5664 vinfos[0].indices[1] = _ij0[1];
5665 vinfos[0].maxsolutions = _nj0;
5666 vinfos[1].jointtype = 1;
5667 vinfos[1].foffset = j1;
5668 vinfos[1].indices[0] = _ij1[0];
5669 vinfos[1].indices[1] = _ij1[1];
5670 vinfos[1].maxsolutions = _nj1;
5671 vinfos[2].jointtype = 1;
5672 vinfos[2].foffset = j2;
5673 vinfos[2].indices[0] = _ij2[0];
5674 vinfos[2].indices[1] = _ij2[1];
5675 vinfos[2].maxsolutions = _nj2;
5676 vinfos[3].jointtype = 1;
5677 vinfos[3].foffset = j3;
5678 vinfos[3].indices[0] = _ij3[0];
5679 vinfos[3].indices[1] = _ij3[1];
5680 vinfos[3].maxsolutions = _nj3;
5681 vinfos[4].jointtype = 1;
5682 vinfos[4].foffset = j4;
5683 vinfos[4].indices[0] = _ij4[0];
5684 vinfos[4].indices[1] = _ij4[1];
5685 vinfos[4].maxsolutions = _nj4;
5686 vinfos[5].jointtype = 1;
5687 vinfos[5].foffset = j5;
5688 vinfos[5].indices[0] = _ij5[0];
5689 vinfos[5].indices[1] = _ij5[1];
5690 vinfos[5].maxsolutions = _nj5;
5691 std::vector<int> vfree(0);
5704 IkReal j0array[1], cj0array[1], sj0array[1];
5705 bool j0valid[1]={
false};
5709 j0array[0]=
IKatan2(((gconst126)*(((((IkReal(-1.00000000000000))*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)))))), ((gconst126)*(((((cj5)*(r11)))+(((r10)*(sj5)))))));
5710 sj0array[0]=
IKsin(j0array[0]);
5711 cj0array[0]=
IKcos(j0array[0]);
5712 if( j0array[0] >
IKPI )
5716 else if( j0array[0] < -
IKPI )
5717 { j0array[0]+=
IK2PI;
5720 for(
int ij0 = 0; ij0 < 1; ++ij0)
5726 _ij0[0] = ij0; _ij0[1] = -1;
5727 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
5731 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
5734 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
5737 IkReal x1290=
IKsin(j0);
5738 IkReal x1291=
IKcos(j0);
5739 IkReal x1292=((IkReal(1.00000000000000))*(r10));
5740 IkReal x1293=((IkReal(1.00000000000000))*(sj4));
5741 IkReal x1294=((cj5)*(r01));
5742 IkReal x1295=((cj5)*(r00));
5743 IkReal x1296=((cj4)*(r01));
5744 IkReal x1297=((IkReal(1.00000000000000))*(cj4));
5745 IkReal x1298=((cj4)*(r11));
5746 IkReal x1299=((sj5)*(x1290));
5747 IkReal x1300=((r11)*(x1293));
5748 IkReal x1301=((sj5)*(x1291));
5749 IkReal x1302=((r02)*(x1291));
5750 IkReal x1303=((r12)*(x1291));
5751 IkReal x1304=((IkReal(1.00000000000000))*(x1291));
5752 IkReal x1305=((cj4)*(x1290));
5753 IkReal x1306=((cj5)*(x1290));
5754 IkReal x1307=((cj5)*(sj4)*(x1291));
5755 evalcond[0]=((IkReal(-1.00000000000000))+(((r00)*(x1299)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1304)))+(((x1290)*(x1294)))+(((IkReal(-1.00000000000000))*(x1292)*(x1301))));
5756 evalcond[1]=((((IkReal(-1.00000000000000))*(x1292)*(x1299)))+(((IkReal(-1.00000000000000))*(r11)*(x1306)))+(((IkReal(-1.00000000000000))*(x1294)*(x1304)))+(((IkReal(-1.00000000000000))*(r00)*(x1301))));
5757 evalcond[2]=((((r01)*(sj4)*(x1299)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)))+(((IkReal(-1.00000000000000))*(x1290)*(x1293)*(x1295)))+(((r02)*(x1305)))+(((r10)*(x1307)))+(((IkReal(-1.00000000000000))*(x1297)*(x1303))));
5758 evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(cj5)*(x1291)*(x1292)))+(((x1295)*(x1305)))+(((IkReal(-1.00000000000000))*(x1296)*(x1299)))+(((IkReal(-1.00000000000000))*(x1293)*(x1303)))+(((r02)*(sj4)*(x1290)))+(((x1298)*(x1301))));
5759 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(r01)*(x1293)*(x1301)))+(((sj4)*(x1291)*(x1295)))+(((IkReal(-1.00000000000000))*(x1299)*(x1300)))+(((IkReal(-1.00000000000000))*(r12)*(x1290)*(x1297)))+(((r10)*(sj4)*(x1306)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x1297)*(x1302))));
5760 evalcond[5]=((((IkReal(-1.00000000000000))*(cj1)*(cj2)))+(((IkReal(-1.00000000000000))*(x1293)*(x1302)))+(((IkReal(-1.00000000000000))*(r12)*(x1290)*(x1293)))+(((x1296)*(x1301)))+(((sj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1291)*(x1295)*(x1297)))+(((IkReal(-1.00000000000000))*(cj5)*(x1292)*(x1305)))+(((x1298)*(x1299))));
5761 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
5768 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5769 vinfos[0].jointtype = 1;
5770 vinfos[0].foffset = j0;
5771 vinfos[0].indices[0] = _ij0[0];
5772 vinfos[0].indices[1] = _ij0[1];
5773 vinfos[0].maxsolutions = _nj0;
5774 vinfos[1].jointtype = 1;
5775 vinfos[1].foffset = j1;
5776 vinfos[1].indices[0] = _ij1[0];
5777 vinfos[1].indices[1] = _ij1[1];
5778 vinfos[1].maxsolutions = _nj1;
5779 vinfos[2].jointtype = 1;
5780 vinfos[2].foffset = j2;
5781 vinfos[2].indices[0] = _ij2[0];
5782 vinfos[2].indices[1] = _ij2[1];
5783 vinfos[2].maxsolutions = _nj2;
5784 vinfos[3].jointtype = 1;
5785 vinfos[3].foffset = j3;
5786 vinfos[3].indices[0] = _ij3[0];
5787 vinfos[3].indices[1] = _ij3[1];
5788 vinfos[3].maxsolutions = _nj3;
5789 vinfos[4].jointtype = 1;
5790 vinfos[4].foffset = j4;
5791 vinfos[4].indices[0] = _ij4[0];
5792 vinfos[4].indices[1] = _ij4[1];
5793 vinfos[4].maxsolutions = _nj4;
5794 vinfos[5].jointtype = 1;
5795 vinfos[5].foffset = j5;
5796 vinfos[5].indices[0] = _ij5[0];
5797 vinfos[5].indices[1] = _ij5[1];
5798 vinfos[5].maxsolutions = _nj5;
5799 std::vector<int> vfree(0);
5818 IkReal j0array[1], cj0array[1], sj0array[1];
5819 bool j0valid[1]={
false};
5823 j0array[0]=
IKatan2(((gconst121)*(((((IkReal(-1.00000000000000))*(r00)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)))))), ((gconst121)*(((((cj5)*(r11)))+(((r10)*(sj5)))))));
5824 sj0array[0]=
IKsin(j0array[0]);
5825 cj0array[0]=
IKcos(j0array[0]);
5826 if( j0array[0] >
IKPI )
5830 else if( j0array[0] < -
IKPI )
5831 { j0array[0]+=
IK2PI;
5834 for(
int ij0 = 0; ij0 < 1; ++ij0)
5840 _ij0[0] = ij0; _ij0[1] = -1;
5841 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
5845 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
5848 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
5851 IkReal x1308=
IKsin(j0);
5852 IkReal x1309=
IKcos(j0);
5853 IkReal x1310=((IkReal(1.00000000000000))*(cj5));
5854 IkReal x1311=((r01)*(sj5));
5855 IkReal x1312=((IkReal(1.00000000000000))*(r12));
5856 IkReal x1313=((IkReal(1.00000000000000))*(r10));
5857 IkReal x1314=((r11)*(sj5));
5858 IkReal x1315=((cj4)*(x1308));
5859 IkReal x1316=((sj4)*(x1308));
5860 IkReal x1317=((sj4)*(x1309));
5861 IkReal x1318=((sj5)*(x1308));
5862 IkReal x1319=((cj4)*(x1309));
5863 IkReal x1320=((sj5)*(x1309));
5864 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1313)*(x1320)))+(((r00)*(x1318)))+(((IkReal(-1.00000000000000))*(r11)*(x1309)*(x1310)))+(((cj5)*(r01)*(x1308))));
5865 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(x1320)))+(((IkReal(-1.00000000000000))*(x1313)*(x1318)))+(((IkReal(-1.00000000000000))*(r11)*(x1308)*(x1310)))+(((IkReal(-1.00000000000000))*(r01)*(x1309)*(x1310))));
5866 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1310)*(x1316)))+(((x1311)*(x1316)))+(((cj5)*(r10)*(x1317)))+(((IkReal(-1.00000000000000))*(x1314)*(x1317)))+(((r02)*(x1315)))+(((IkReal(-1.00000000000000))*(x1312)*(x1319))));
5867 evalcond[3]=((((IkReal(-1.00000000000000))*(x1312)*(x1317)))+(((cj5)*(r00)*(x1315)))+(((r02)*(x1316)))+(((IkReal(-1.00000000000000))*(r10)*(x1310)*(x1319)))+(((x1314)*(x1319)))+(((IkReal(-1.00000000000000))*(x1311)*(x1315))));
5868 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5875 IkReal dummyeval[1];
5877 gconst124=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
5878 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
5879 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5882 IkReal dummyeval[1];
5884 gconst125=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
5885 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
5886 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
5893 IkReal j1array[1], cj1array[1], sj1array[1];
5894 bool j1valid[1]={
false};
5896 IkReal x1321=((cj4)*(r22));
5897 IkReal x1322=((r11)*(sj0));
5898 IkReal x1323=((IkReal(1.00000000000000))*(cj2));
5899 IkReal x1324=((sj2)*(sj4));
5900 IkReal x1325=((IkReal(1.00000000000000))*(sj2));
5901 IkReal x1326=((cj2)*(sj4));
5902 IkReal x1327=((cj0)*(r01));
5903 IkReal x1328=((cj5)*(r20));
5904 IkReal x1329=((IkReal(1.00000000000000))*(sj4)*(sj5));
5905 IkReal x1330=((cj0)*(cj4)*(r02));
5906 IkReal x1331=((cj4)*(r12)*(sj0));
5907 IkReal x1332=((cj5)*(r10)*(sj0));
5908 IkReal x1333=((cj0)*(cj5)*(r00));
5909 if(
IKabs(((gconst125)*(((((sj2)*(x1321)))+(((r21)*(sj5)*(x1324)))+(((IkReal(-1.00000000000000))*(x1323)*(x1331)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1322)*(x1323)))+(((x1326)*(x1333)))+(((IkReal(-1.00000000000000))*(x1323)*(x1330)))+(((x1326)*(x1332)))+(((IkReal(-1.00000000000000))*(x1324)*(x1328)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1323)*(x1327))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst125)*(((((IkReal(-1.00000000000000))*(sj5)*(x1324)*(x1327)))+(((IkReal(-1.00000000000000))*(x1325)*(x1330)))+(((x1324)*(x1332)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1323)))+(((IkReal(-1.00000000000000))*(sj5)*(x1322)*(x1324)))+(((x1326)*(x1328)))+(((IkReal(-1.00000000000000))*(x1321)*(x1323)))+(((x1324)*(x1333)))+(((IkReal(-1.00000000000000))*(x1325)*(x1331))))))) <
IKFAST_ATAN2_MAGTHRESH )
5911 j1array[0]=
IKatan2(((gconst125)*(((((sj2)*(x1321)))+(((r21)*(sj5)*(x1324)))+(((IkReal(-1.00000000000000))*(x1323)*(x1331)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1322)*(x1323)))+(((x1326)*(x1333)))+(((IkReal(-1.00000000000000))*(x1323)*(x1330)))+(((x1326)*(x1332)))+(((IkReal(-1.00000000000000))*(x1324)*(x1328)))+(((IkReal(-1.00000000000000))*(sj4)*(sj5)*(x1323)*(x1327)))))), ((gconst125)*(((((IkReal(-1.00000000000000))*(sj5)*(x1324)*(x1327)))+(((IkReal(-1.00000000000000))*(x1325)*(x1330)))+(((x1324)*(x1332)))+(((IkReal(-1.00000000000000))*(r21)*(sj4)*(sj5)*(x1323)))+(((IkReal(-1.00000000000000))*(sj5)*(x1322)*(x1324)))+(((x1326)*(x1328)))+(((IkReal(-1.00000000000000))*(x1321)*(x1323)))+(((x1324)*(x1333)))+(((IkReal(-1.00000000000000))*(x1325)*(x1331)))))));
5912 sj1array[0]=
IKsin(j1array[0]);
5913 cj1array[0]=
IKcos(j1array[0]);
5914 if( j1array[0] >
IKPI )
5918 else if( j1array[0] < -
IKPI )
5919 { j1array[0]+=
IK2PI;
5922 for(
int ij1 = 0; ij1 < 1; ++ij1)
5928 _ij1[0] = ij1; _ij1[1] = -1;
5929 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
5933 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
5936 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
5939 IkReal x1334=
IKcos(j1);
5940 IkReal x1335=
IKsin(j1);
5941 IkReal x1336=((IkReal(1.00000000000000))*(sj4));
5942 IkReal x1337=((cj5)*(r00));
5943 IkReal x1338=((cj5)*(r20));
5944 IkReal x1339=((r12)*(sj0));
5945 IkReal x1340=((r21)*(sj5));
5946 IkReal x1341=((IkReal(1.00000000000000))*(cj4));
5947 IkReal x1342=((sj2)*(x1335));
5948 IkReal x1343=((IkReal(1.00000000000000))*(x1334));
5949 IkReal x1344=((r11)*(sj0)*(sj5));
5950 IkReal x1345=((cj0)*(x1341));
5951 IkReal x1346=((cj0)*(r01)*(sj5));
5952 IkReal x1347=((cj2)*(x1335));
5953 IkReal x1348=((cj5)*(r10)*(sj0));
5954 IkReal x1349=((cj2)*(x1343));
5955 evalcond[0]=((((sj4)*(x1340)))+(((IkReal(-1.00000000000000))*(x1336)*(x1338)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x1349)))+(x1342));
5956 evalcond[1]=((((cj4)*(x1338)))+(((IkReal(-1.00000000000000))*(x1340)*(x1341)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(sj2)*(x1343)))+(((IkReal(-1.00000000000000))*(x1347))));
5957 evalcond[2]=((((sj2)*(x1334)))+(((IkReal(-1.00000000000000))*(x1339)*(x1341)))+(((cj0)*(sj4)*(x1337)))+(((sj4)*(x1348)))+(((IkReal(-1.00000000000000))*(r02)*(x1345)))+(x1347)+(((IkReal(-1.00000000000000))*(x1336)*(x1344)))+(((IkReal(-1.00000000000000))*(x1336)*(x1346))));
5958 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1336)))+(((cj4)*(x1344)))+(((IkReal(-1.00000000000000))*(x1336)*(x1339)))+(((IkReal(-1.00000000000000))*(x1337)*(x1345)))+(((IkReal(-1.00000000000000))*(x1341)*(x1348)))+(((cj4)*(x1346)))+(((IkReal(-1.00000000000000))*(x1349)))+(x1342));
5959 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
5966 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5967 vinfos[0].jointtype = 1;
5968 vinfos[0].foffset = j0;
5969 vinfos[0].indices[0] = _ij0[0];
5970 vinfos[0].indices[1] = _ij0[1];
5971 vinfos[0].maxsolutions = _nj0;
5972 vinfos[1].jointtype = 1;
5973 vinfos[1].foffset = j1;
5974 vinfos[1].indices[0] = _ij1[0];
5975 vinfos[1].indices[1] = _ij1[1];
5976 vinfos[1].maxsolutions = _nj1;
5977 vinfos[2].jointtype = 1;
5978 vinfos[2].foffset = j2;
5979 vinfos[2].indices[0] = _ij2[0];
5980 vinfos[2].indices[1] = _ij2[1];
5981 vinfos[2].maxsolutions = _nj2;
5982 vinfos[3].jointtype = 1;
5983 vinfos[3].foffset = j3;
5984 vinfos[3].indices[0] = _ij3[0];
5985 vinfos[3].indices[1] = _ij3[1];
5986 vinfos[3].maxsolutions = _nj3;
5987 vinfos[4].jointtype = 1;
5988 vinfos[4].foffset = j4;
5989 vinfos[4].indices[0] = _ij4[0];
5990 vinfos[4].indices[1] = _ij4[1];
5991 vinfos[4].maxsolutions = _nj4;
5992 vinfos[5].jointtype = 1;
5993 vinfos[5].foffset = j5;
5994 vinfos[5].indices[0] = _ij5[0];
5995 vinfos[5].indices[1] = _ij5[1];
5996 vinfos[5].maxsolutions = _nj5;
5997 std::vector<int> vfree(0);
6010 IkReal j1array[1], cj1array[1], sj1array[1];
6011 bool j1valid[1]={
false};
6013 IkReal x1350=((IkReal(1.00000000000000))*(cj2));
6014 IkReal x1351=((cj2)*(r22));
6015 IkReal x1352=((r21)*(sj5));
6016 IkReal x1353=((r22)*(sj2));
6017 IkReal x1354=((cj5)*(r20)*(sj4));
6018 IkReal x1355=((cj4)*(cj5)*(r20));
6019 IkReal x1356=((IkReal(1.00000000000000))*(sj2)*(x1352));
6020 if(
IKabs(((gconst124)*(((((IkReal(-1.00000000000000))*(cj4)*(x1350)*(x1352)))+(((sj4)*(x1351)))+(((cj2)*(x1355)))+(((IkReal(-1.00000000000000))*(cj4)*(x1353)))+(((IkReal(-1.00000000000000))*(sj4)*(x1356)))+(((sj2)*(x1354))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst124)*(((((cj4)*(x1351)))+(((IkReal(-1.00000000000000))*(cj4)*(x1356)))+(((IkReal(-1.00000000000000))*(x1350)*(x1354)))+(((cj2)*(sj4)*(x1352)))+(((sj2)*(x1355)))+(((sj4)*(x1353))))))) <
IKFAST_ATAN2_MAGTHRESH )
6022 j1array[0]=
IKatan2(((gconst124)*(((((IkReal(-1.00000000000000))*(cj4)*(x1350)*(x1352)))+(((sj4)*(x1351)))+(((cj2)*(x1355)))+(((IkReal(-1.00000000000000))*(cj4)*(x1353)))+(((IkReal(-1.00000000000000))*(sj4)*(x1356)))+(((sj2)*(x1354)))))), ((gconst124)*(((((cj4)*(x1351)))+(((IkReal(-1.00000000000000))*(cj4)*(x1356)))+(((IkReal(-1.00000000000000))*(x1350)*(x1354)))+(((cj2)*(sj4)*(x1352)))+(((sj2)*(x1355)))+(((sj4)*(x1353)))))));
6023 sj1array[0]=
IKsin(j1array[0]);
6024 cj1array[0]=
IKcos(j1array[0]);
6025 if( j1array[0] >
IKPI )
6029 else if( j1array[0] < -
IKPI )
6030 { j1array[0]+=
IK2PI;
6033 for(
int ij1 = 0; ij1 < 1; ++ij1)
6039 _ij1[0] = ij1; _ij1[1] = -1;
6040 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6044 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6047 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6050 IkReal x1357=
IKcos(j1);
6051 IkReal x1358=
IKsin(j1);
6052 IkReal x1359=((IkReal(1.00000000000000))*(sj4));
6053 IkReal x1360=((cj5)*(r00));
6054 IkReal x1361=((cj5)*(r20));
6055 IkReal x1362=((r12)*(sj0));
6056 IkReal x1363=((r21)*(sj5));
6057 IkReal x1364=((IkReal(1.00000000000000))*(cj4));
6058 IkReal x1365=((sj2)*(x1358));
6059 IkReal x1366=((IkReal(1.00000000000000))*(x1357));
6060 IkReal x1367=((r11)*(sj0)*(sj5));
6061 IkReal x1368=((cj0)*(x1364));
6062 IkReal x1369=((cj0)*(r01)*(sj5));
6063 IkReal x1370=((cj2)*(x1358));
6064 IkReal x1371=((cj5)*(r10)*(sj0));
6065 IkReal x1372=((cj2)*(x1366));
6066 evalcond[0]=((x1365)+(((IkReal(-1.00000000000000))*(x1359)*(x1361)))+(((IkReal(-1.00000000000000))*(x1372)))+(((cj4)*(r22)))+(((sj4)*(x1363))));
6067 evalcond[1]=((((r22)*(sj4)))+(((cj4)*(x1361)))+(((IkReal(-1.00000000000000))*(x1370)))+(((IkReal(-1.00000000000000))*(x1363)*(x1364)))+(((IkReal(-1.00000000000000))*(sj2)*(x1366))));
6068 evalcond[2]=((((IkReal(-1.00000000000000))*(x1362)*(x1364)))+(((IkReal(-1.00000000000000))*(r02)*(x1368)))+(((sj4)*(x1371)))+(((cj0)*(sj4)*(x1360)))+(((IkReal(-1.00000000000000))*(x1359)*(x1367)))+(x1370)+(((IkReal(-1.00000000000000))*(x1359)*(x1369)))+(((sj2)*(x1357))));
6069 evalcond[3]=((((IkReal(-1.00000000000000))*(x1359)*(x1362)))+(x1365)+(((IkReal(-1.00000000000000))*(x1372)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1359)))+(((cj4)*(x1367)))+(((IkReal(-1.00000000000000))*(x1364)*(x1371)))+(((IkReal(-1.00000000000000))*(x1360)*(x1368)))+(((cj4)*(x1369))));
6070 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6077 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6078 vinfos[0].jointtype = 1;
6079 vinfos[0].foffset = j0;
6080 vinfos[0].indices[0] = _ij0[0];
6081 vinfos[0].indices[1] = _ij0[1];
6082 vinfos[0].maxsolutions = _nj0;
6083 vinfos[1].jointtype = 1;
6084 vinfos[1].foffset = j1;
6085 vinfos[1].indices[0] = _ij1[0];
6086 vinfos[1].indices[1] = _ij1[1];
6087 vinfos[1].maxsolutions = _nj1;
6088 vinfos[2].jointtype = 1;
6089 vinfos[2].foffset = j2;
6090 vinfos[2].indices[0] = _ij2[0];
6091 vinfos[2].indices[1] = _ij2[1];
6092 vinfos[2].maxsolutions = _nj2;
6093 vinfos[3].jointtype = 1;
6094 vinfos[3].foffset = j3;
6095 vinfos[3].indices[0] = _ij3[0];
6096 vinfos[3].indices[1] = _ij3[1];
6097 vinfos[3].maxsolutions = _nj3;
6098 vinfos[4].jointtype = 1;
6099 vinfos[4].foffset = j4;
6100 vinfos[4].indices[0] = _ij4[0];
6101 vinfos[4].indices[1] = _ij4[1];
6102 vinfos[4].maxsolutions = _nj4;
6103 vinfos[5].jointtype = 1;
6104 vinfos[5].foffset = j5;
6105 vinfos[5].indices[0] = _ij5[0];
6106 vinfos[5].indices[1] = _ij5[1];
6107 vinfos[5].maxsolutions = _nj5;
6108 std::vector<int> vfree(0);
6142 IkReal j0array[1], cj0array[1], sj0array[1];
6143 bool j0valid[1]={
false};
6145 IkReal x1373=((cj3)*(sj4));
6146 IkReal x1374=((IkReal(1.00000000000000))*(sj5));
6147 IkReal x1375=((IkReal(1.00000000000000))*(cj3)*(cj4));
6148 if(
IKabs(((gconst84)*(((((IkReal(-1.00000000000000))*(r12)*(x1375)))+(((cj5)*(r10)*(x1373)))+(((IkReal(-1.00000000000000))*(r11)*(x1373)*(x1374))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst84)*(((((cj5)*(r00)*(x1373)))+(((IkReal(-1.00000000000000))*(r01)*(x1373)*(x1374)))+(((IkReal(-1.00000000000000))*(r02)*(x1375))))))) <
IKFAST_ATAN2_MAGTHRESH )
6150 j0array[0]=
IKatan2(((gconst84)*(((((IkReal(-1.00000000000000))*(r12)*(x1375)))+(((cj5)*(r10)*(x1373)))+(((IkReal(-1.00000000000000))*(r11)*(x1373)*(x1374)))))), ((gconst84)*(((((cj5)*(r00)*(x1373)))+(((IkReal(-1.00000000000000))*(r01)*(x1373)*(x1374)))+(((IkReal(-1.00000000000000))*(r02)*(x1375)))))));
6151 sj0array[0]=
IKsin(j0array[0]);
6152 cj0array[0]=
IKcos(j0array[0]);
6153 if( j0array[0] >
IKPI )
6157 else if( j0array[0] < -
IKPI )
6158 { j0array[0]+=
IK2PI;
6161 for(
int ij0 = 0; ij0 < 1; ++ij0)
6167 _ij0[0] = ij0; _ij0[1] = -1;
6168 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
6172 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
6175 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
6178 IkReal x1376=
IKsin(j0);
6179 IkReal x1377=
IKcos(j0);
6180 IkReal x1378=((cj5)*(r00));
6181 IkReal x1379=((r01)*(sj5));
6182 IkReal x1380=((cj5)*(r10));
6183 IkReal x1381=((IkReal(1.00000000000000))*(r12));
6184 IkReal x1382=((r11)*(sj5));
6185 IkReal x1383=((cj4)*(x1376));
6186 IkReal x1384=((sj4)*(x1376));
6187 IkReal x1385=((sj4)*(x1377));
6188 IkReal x1386=((cj4)*(x1377));
6189 IkReal x1387=((IkReal(1.00000000000000))*(x1377));
6190 evalcond[0]=((((r00)*(sj5)*(x1376)))+(cj3)+(((cj5)*(r01)*(x1376)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1387)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1387))));
6191 evalcond[1]=((((IkReal(-1.00000000000000))*(x1378)*(x1384)))+(((x1379)*(x1384)))+(((IkReal(-1.00000000000000))*(x1382)*(x1385)))+(((x1380)*(x1385)))+(((r02)*(x1383)))+(((IkReal(-1.00000000000000))*(x1381)*(x1386))));
6192 evalcond[2]=((((r02)*(x1384)))+(sj3)+(((IkReal(-1.00000000000000))*(x1381)*(x1385)))+(((IkReal(-1.00000000000000))*(x1380)*(x1386)))+(((x1378)*(x1383)))+(((x1382)*(x1386)))+(((IkReal(-1.00000000000000))*(x1379)*(x1383))));
6193 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 )
6200 IkReal dummyeval[1];
6202 gconst88=
IKsign(((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2))))));
6203 dummyeval[0]=((((sj3)*((cj2)*(cj2))))+(((sj3)*((sj2)*(sj2)))));
6204 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6207 IkReal dummyeval[1];
6209 gconst89=
IKsign(((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2))))));
6210 dummyeval[0]=((((cj3)*((sj2)*(sj2))))+(((cj3)*((cj2)*(cj2)))));
6211 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6215 IkReal x1388=((sj0)*(sj4));
6216 IkReal x1389=((IkReal(1.00000000000000))*(r12));
6217 IkReal x1390=((cj4)*(cj5));
6218 IkReal x1391=((IkReal(1.00000000000000))*(cj0));
6219 IkReal x1392=((cj4)*(sj0));
6220 IkReal x1393=((IkReal(1.00000000000000))*(sj5));
6221 IkReal x1394=((cj0)*(cj4));
6222 IkReal x1395=((r00)*(sj0));
6223 IkReal x1396=((sj4)*(sj5));
6224 IkReal x1397=((IkReal(1.00000000000000))*(cj5));
6225 IkReal x1398=((r01)*(sj5));
6226 IkReal x1399=((cj0)*(sj4));
6227 IkReal x1400=((r11)*(sj5));
6228 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(1.57079632679490))+(j3)), IkReal(6.28318530717959))));
6229 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(-0.350000000000000))*(sj2)))+(((npx)*(sj5))));
6230 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x1397)))+(((npy)*(x1396))));
6231 evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(npy)*(x1393)))+(((npz)*(sj4)))+(((npx)*(x1390))));
6232 evalcond[4]=((((cj5)*(r01)*(sj0)))+(((sj5)*(x1395)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1391)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1391))));
6233 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(r21)*(x1393)))+(((r22)*(sj4)))+(((r20)*(x1390))));
6234 evalcond[6]=((((IkReal(-1.00000000000000))*(x1389)*(x1394)))+(((cj5)*(r10)*(x1399)))+(((IkReal(-1.00000000000000))*(r00)*(x1388)*(x1397)))+(((r02)*(x1392)))+(((x1388)*(x1398)))+(((IkReal(-1.00000000000000))*(r11)*(x1391)*(x1396))));
6235 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1389)*(x1399)))+(((x1390)*(x1395)))+(((IkReal(-1.00000000000000))*(r01)*(x1392)*(x1393)))+(((r02)*(x1388)))+(((x1394)*(x1400)))+(((IkReal(-1.00000000000000))*(r10)*(x1390)*(x1391))));
6236 evalcond[8]=((((x1392)*(x1400)))+(((IkReal(-1.00000000000000))*(x1388)*(x1389)))+(((IkReal(-1.00000000000000))*(r00)*(x1390)*(x1391)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1390)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1391)))+(((x1394)*(x1398))));
6237 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
6240 IkReal dummyeval[1];
6242 gconst90=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
6243 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
6244 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6247 IkReal dummyeval[1];
6249 gconst91=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
6250 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
6251 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6258 IkReal j1array[1], cj1array[1], sj1array[1];
6259 bool j1valid[1]={
false};
6261 IkReal x1401=((cj2)*(sj5));
6262 IkReal x1402=((r10)*(sj0));
6263 IkReal x1403=((cj2)*(cj5));
6264 IkReal x1404=((cj0)*(r00));
6265 IkReal x1405=((sj2)*(sj5));
6266 IkReal x1406=((r11)*(sj0));
6267 IkReal x1407=((cj5)*(sj2));
6268 IkReal x1408=((cj0)*(r01));
6269 if(
IKabs(((gconst91)*(((((r20)*(x1401)))+(((x1406)*(x1407)))+(((x1404)*(x1405)))+(((x1407)*(x1408)))+(((r21)*(x1403)))+(((x1402)*(x1405))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst91)*(((((IkReal(-1.00000000000000))*(x1403)*(x1408)))+(((r20)*(x1405)))+(((IkReal(-1.00000000000000))*(x1401)*(x1404)))+(((IkReal(-1.00000000000000))*(x1401)*(x1402)))+(((r21)*(x1407)))+(((IkReal(-1.00000000000000))*(x1403)*(x1406))))))) <
IKFAST_ATAN2_MAGTHRESH )
6271 j1array[0]=
IKatan2(((gconst91)*(((((r20)*(x1401)))+(((x1406)*(x1407)))+(((x1404)*(x1405)))+(((x1407)*(x1408)))+(((r21)*(x1403)))+(((x1402)*(x1405)))))), ((gconst91)*(((((IkReal(-1.00000000000000))*(x1403)*(x1408)))+(((r20)*(x1405)))+(((IkReal(-1.00000000000000))*(x1401)*(x1404)))+(((IkReal(-1.00000000000000))*(x1401)*(x1402)))+(((r21)*(x1407)))+(((IkReal(-1.00000000000000))*(x1403)*(x1406)))))));
6272 sj1array[0]=
IKsin(j1array[0]);
6273 cj1array[0]=
IKcos(j1array[0]);
6274 if( j1array[0] >
IKPI )
6278 else if( j1array[0] < -
IKPI )
6279 { j1array[0]+=
IK2PI;
6282 for(
int ij1 = 0; ij1 < 1; ++ij1)
6288 _ij1[0] = ij1; _ij1[1] = -1;
6289 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6293 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6296 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6299 IkReal x1409=
IKsin(j1);
6300 IkReal x1410=
IKcos(j1);
6301 IkReal x1411=((cj0)*(sj4));
6302 IkReal x1412=((IkReal(1.00000000000000))*(sj5));
6303 IkReal x1413=((cj5)*(sj0));
6304 IkReal x1414=((IkReal(1.00000000000000))*(cj4));
6305 IkReal x1415=((IkReal(1.00000000000000))*(cj5));
6306 IkReal x1416=((sj2)*(x1409));
6307 IkReal x1417=((IkReal(1.00000000000000))*(x1410));
6308 IkReal x1418=((cj2)*(x1409));
6309 IkReal x1419=((cj2)*(x1417));
6310 evalcond[0]=((((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(x1418)))+(((cj5)*(r21)))+(((IkReal(-1.00000000000000))*(sj2)*(x1417))));
6311 evalcond[1]=((x1416)+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x1415)))+(((IkReal(-1.00000000000000))*(x1419)))+(((cj4)*(r22)))+(((r21)*(sj4)*(sj5))));
6312 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x1413)))+(x1416)+(((IkReal(-1.00000000000000))*(cj0)*(r01)*(x1415)))+(((IkReal(-1.00000000000000))*(x1419)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1412)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1412))));
6313 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x1411)*(x1412)))+(((IkReal(-1.00000000000000))*(r12)*(sj0)*(x1414)))+(x1418)+(((sj2)*(x1410)))+(((r10)*(sj4)*(x1413)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1414)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x1412)))+(((cj5)*(r00)*(x1411))));
6314 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6321 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6322 vinfos[0].jointtype = 1;
6323 vinfos[0].foffset = j0;
6324 vinfos[0].indices[0] = _ij0[0];
6325 vinfos[0].indices[1] = _ij0[1];
6326 vinfos[0].maxsolutions = _nj0;
6327 vinfos[1].jointtype = 1;
6328 vinfos[1].foffset = j1;
6329 vinfos[1].indices[0] = _ij1[0];
6330 vinfos[1].indices[1] = _ij1[1];
6331 vinfos[1].maxsolutions = _nj1;
6332 vinfos[2].jointtype = 1;
6333 vinfos[2].foffset = j2;
6334 vinfos[2].indices[0] = _ij2[0];
6335 vinfos[2].indices[1] = _ij2[1];
6336 vinfos[2].maxsolutions = _nj2;
6337 vinfos[3].jointtype = 1;
6338 vinfos[3].foffset = j3;
6339 vinfos[3].indices[0] = _ij3[0];
6340 vinfos[3].indices[1] = _ij3[1];
6341 vinfos[3].maxsolutions = _nj3;
6342 vinfos[4].jointtype = 1;
6343 vinfos[4].foffset = j4;
6344 vinfos[4].indices[0] = _ij4[0];
6345 vinfos[4].indices[1] = _ij4[1];
6346 vinfos[4].maxsolutions = _nj4;
6347 vinfos[5].jointtype = 1;
6348 vinfos[5].foffset = j5;
6349 vinfos[5].indices[0] = _ij5[0];
6350 vinfos[5].indices[1] = _ij5[1];
6351 vinfos[5].maxsolutions = _nj5;
6352 std::vector<int> vfree(0);
6365 IkReal j1array[1], cj1array[1], sj1array[1];
6366 bool j1valid[1]={
false};
6368 IkReal x1420=((r20)*(sj5));
6369 IkReal x1421=((cj4)*(r22));
6370 IkReal x1422=((sj2)*(sj4));
6371 IkReal x1423=((cj5)*(r20));
6372 IkReal x1424=((cj5)*(r21));
6373 IkReal x1425=((r21)*(sj5));
6374 IkReal x1426=((cj2)*(sj4));
6375 if(
IKabs(((gconst90)*(((((IkReal(-1.00000000000000))*(sj2)*(x1421)))+(((cj2)*(x1420)))+(((x1422)*(x1423)))+(((cj2)*(x1424)))+(((IkReal(-1.00000000000000))*(x1422)*(x1425))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst90)*(((((sj2)*(x1424)))+(((sj2)*(x1420)))+(((cj2)*(x1421)))+(((x1425)*(x1426)))+(((IkReal(-1.00000000000000))*(x1423)*(x1426))))))) <
IKFAST_ATAN2_MAGTHRESH )
6377 j1array[0]=
IKatan2(((gconst90)*(((((IkReal(-1.00000000000000))*(sj2)*(x1421)))+(((cj2)*(x1420)))+(((x1422)*(x1423)))+(((cj2)*(x1424)))+(((IkReal(-1.00000000000000))*(x1422)*(x1425)))))), ((gconst90)*(((((sj2)*(x1424)))+(((sj2)*(x1420)))+(((cj2)*(x1421)))+(((x1425)*(x1426)))+(((IkReal(-1.00000000000000))*(x1423)*(x1426)))))));
6378 sj1array[0]=
IKsin(j1array[0]);
6379 cj1array[0]=
IKcos(j1array[0]);
6380 if( j1array[0] >
IKPI )
6384 else if( j1array[0] < -
IKPI )
6385 { j1array[0]+=
IK2PI;
6388 for(
int ij1 = 0; ij1 < 1; ++ij1)
6394 _ij1[0] = ij1; _ij1[1] = -1;
6395 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6399 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6402 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6405 IkReal x1427=
IKsin(j1);
6406 IkReal x1428=
IKcos(j1);
6407 IkReal x1429=((cj0)*(sj4));
6408 IkReal x1430=((IkReal(1.00000000000000))*(sj5));
6409 IkReal x1431=((cj5)*(sj0));
6410 IkReal x1432=((IkReal(1.00000000000000))*(cj4));
6411 IkReal x1433=((IkReal(1.00000000000000))*(cj5));
6412 IkReal x1434=((sj2)*(x1427));
6413 IkReal x1435=((IkReal(1.00000000000000))*(x1428));
6414 IkReal x1436=((cj2)*(x1427));
6415 IkReal x1437=((cj2)*(x1435));
6416 evalcond[0]=((((IkReal(-1.00000000000000))*(x1436)))+(((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(sj2)*(x1435)))+(((cj5)*(r21))));
6417 evalcond[1]=((x1434)+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x1437)))+(((IkReal(-1.00000000000000))*(r20)*(sj4)*(x1433)))+(((r21)*(sj4)*(sj5))));
6418 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(r01)*(x1433)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1430)))+(x1434)+(((IkReal(-1.00000000000000))*(x1437)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1430)))+(((IkReal(-1.00000000000000))*(r11)*(x1431))));
6419 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj0)*(x1432)))+(((IkReal(-1.00000000000000))*(r01)*(x1429)*(x1430)))+(((cj5)*(r00)*(x1429)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(x1430)))+(((r10)*(sj4)*(x1431)))+(x1436)+(((sj2)*(x1428)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1432))));
6420 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6427 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6428 vinfos[0].jointtype = 1;
6429 vinfos[0].foffset = j0;
6430 vinfos[0].indices[0] = _ij0[0];
6431 vinfos[0].indices[1] = _ij0[1];
6432 vinfos[0].maxsolutions = _nj0;
6433 vinfos[1].jointtype = 1;
6434 vinfos[1].foffset = j1;
6435 vinfos[1].indices[0] = _ij1[0];
6436 vinfos[1].indices[1] = _ij1[1];
6437 vinfos[1].maxsolutions = _nj1;
6438 vinfos[2].jointtype = 1;
6439 vinfos[2].foffset = j2;
6440 vinfos[2].indices[0] = _ij2[0];
6441 vinfos[2].indices[1] = _ij2[1];
6442 vinfos[2].maxsolutions = _nj2;
6443 vinfos[3].jointtype = 1;
6444 vinfos[3].foffset = j3;
6445 vinfos[3].indices[0] = _ij3[0];
6446 vinfos[3].indices[1] = _ij3[1];
6447 vinfos[3].maxsolutions = _nj3;
6448 vinfos[4].jointtype = 1;
6449 vinfos[4].foffset = j4;
6450 vinfos[4].indices[0] = _ij4[0];
6451 vinfos[4].indices[1] = _ij4[1];
6452 vinfos[4].maxsolutions = _nj4;
6453 vinfos[5].jointtype = 1;
6454 vinfos[5].foffset = j5;
6455 vinfos[5].indices[0] = _ij5[0];
6456 vinfos[5].indices[1] = _ij5[1];
6457 vinfos[5].maxsolutions = _nj5;
6458 std::vector<int> vfree(0);
6470 IkReal x1438=((sj0)*(sj4));
6471 IkReal x1439=((IkReal(1.00000000000000))*(r12));
6472 IkReal x1440=((cj4)*(cj5));
6473 IkReal x1441=((IkReal(1.00000000000000))*(cj0));
6474 IkReal x1442=((cj4)*(sj0));
6475 IkReal x1443=((IkReal(1.00000000000000))*(sj5));
6476 IkReal x1444=((cj0)*(cj4));
6477 IkReal x1445=((r00)*(sj0));
6478 IkReal x1446=((sj4)*(sj5));
6479 IkReal x1447=((IkReal(1.00000000000000))*(cj5));
6480 IkReal x1448=((r01)*(sj5));
6481 IkReal x1449=((cj0)*(sj4));
6482 IkReal x1450=((r11)*(sj5));
6483 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(4.71238898038469))+(j3)), IkReal(6.28318530717959))));
6484 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((IkReal(0.350000000000000))*(sj2)))+(((npx)*(sj5))));
6485 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x1447)))+(((cj4)*(npz)))+(((npy)*(x1446))));
6486 evalcond[3]=((((npx)*(x1440)))+(((IkReal(-1.00000000000000))*(cj4)*(npy)*(x1443)))+(((npz)*(sj4))));
6487 evalcond[4]=((((cj5)*(r01)*(sj0)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1441)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1441)))+(((sj5)*(x1445))));
6488 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(r21)*(x1443)))+(((r22)*(sj4)))+(((r20)*(x1440))));
6489 evalcond[6]=((((IkReal(-1.00000000000000))*(r00)*(x1438)*(x1447)))+(((r02)*(x1442)))+(((x1438)*(x1448)))+(((cj5)*(r10)*(x1449)))+(((IkReal(-1.00000000000000))*(r11)*(x1441)*(x1446)))+(((IkReal(-1.00000000000000))*(x1439)*(x1444))));
6490 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1439)*(x1449)))+(((x1444)*(x1450)))+(((IkReal(-1.00000000000000))*(r10)*(x1440)*(x1441)))+(((IkReal(-1.00000000000000))*(r01)*(x1442)*(x1443)))+(((x1440)*(x1445)))+(((r02)*(x1438))));
6491 evalcond[8]=((((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1441)))+(((x1444)*(x1448)))+(((IkReal(-1.00000000000000))*(x1438)*(x1439)))+(((IkReal(-1.00000000000000))*(r00)*(x1440)*(x1441)))+(((IkReal(-1.00000000000000))*(r10)*(sj0)*(x1440)))+(((x1442)*(x1450))));
6492 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
6495 IkReal dummyeval[1];
6497 gconst92=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
6498 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
6499 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6502 IkReal dummyeval[1];
6504 gconst93=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
6505 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
6506 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6513 IkReal j1array[1], cj1array[1], sj1array[1];
6514 bool j1valid[1]={
false};
6516 IkReal x1451=((IkReal(1.00000000000000))*(sj2));
6517 IkReal x1452=((cj5)*(r21));
6518 IkReal x1453=((r20)*(sj5));
6519 IkReal x1454=((IkReal(1.00000000000000))*(cj2));
6520 IkReal x1455=((cj5)*(r11)*(sj0));
6521 IkReal x1456=((r10)*(sj0)*(sj5));
6522 IkReal x1457=((cj0)*(r00)*(sj5));
6523 IkReal x1458=((cj0)*(cj5)*(r01));
6524 if(
IKabs(((gconst93)*(((((IkReal(-1.00000000000000))*(x1452)*(x1454)))+(((IkReal(-1.00000000000000))*(x1451)*(x1457)))+(((IkReal(-1.00000000000000))*(x1451)*(x1456)))+(((IkReal(-1.00000000000000))*(x1451)*(x1458)))+(((IkReal(-1.00000000000000))*(x1453)*(x1454)))+(((IkReal(-1.00000000000000))*(x1451)*(x1455))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst93)*(((((cj2)*(x1456)))+(((IkReal(-1.00000000000000))*(x1451)*(x1452)))+(((cj2)*(x1455)))+(((cj2)*(x1458)))+(((IkReal(-1.00000000000000))*(x1451)*(x1453)))+(((cj2)*(x1457))))))) <
IKFAST_ATAN2_MAGTHRESH )
6526 j1array[0]=
IKatan2(((gconst93)*(((((IkReal(-1.00000000000000))*(x1452)*(x1454)))+(((IkReal(-1.00000000000000))*(x1451)*(x1457)))+(((IkReal(-1.00000000000000))*(x1451)*(x1456)))+(((IkReal(-1.00000000000000))*(x1451)*(x1458)))+(((IkReal(-1.00000000000000))*(x1453)*(x1454)))+(((IkReal(-1.00000000000000))*(x1451)*(x1455)))))), ((gconst93)*(((((cj2)*(x1456)))+(((IkReal(-1.00000000000000))*(x1451)*(x1452)))+(((cj2)*(x1455)))+(((cj2)*(x1458)))+(((IkReal(-1.00000000000000))*(x1451)*(x1453)))+(((cj2)*(x1457)))))));
6527 sj1array[0]=
IKsin(j1array[0]);
6528 cj1array[0]=
IKcos(j1array[0]);
6529 if( j1array[0] >
IKPI )
6533 else if( j1array[0] < -
IKPI )
6534 { j1array[0]+=
IK2PI;
6537 for(
int ij1 = 0; ij1 < 1; ++ij1)
6543 _ij1[0] = ij1; _ij1[1] = -1;
6544 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6548 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6551 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6554 IkReal x1459=
IKsin(j1);
6555 IkReal x1460=
IKcos(j1);
6556 IkReal x1461=((cj5)*(sj0));
6557 IkReal x1462=((IkReal(1.00000000000000))*(r11));
6558 IkReal x1463=((cj5)*(sj4));
6559 IkReal x1464=((cj0)*(r00));
6560 IkReal x1465=((IkReal(1.00000000000000))*(sj0));
6561 IkReal x1466=((sj4)*(sj5));
6562 IkReal x1467=((IkReal(1.00000000000000))*(cj0));
6563 IkReal x1468=((sj2)*(x1460));
6564 IkReal x1469=((cj2)*(x1459));
6565 IkReal x1470=((cj2)*(x1460));
6566 IkReal x1471=((sj2)*(x1459));
6567 IkReal x1472=((x1469)+(x1468));
6568 evalcond[0]=((((r20)*(sj5)))+(x1472)+(((cj5)*(r21))));
6569 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1463)))+(((IkReal(-1.00000000000000))*(x1470)))+(((cj4)*(r22)))+(((r21)*(x1466)))+(x1471));
6570 evalcond[2]=((((IkReal(-1.00000000000000))*(x1461)*(x1462)))+(((IkReal(-1.00000000000000))*(sj5)*(x1464)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1465)))+(((IkReal(-1.00000000000000))*(x1471)))+(x1470)+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1467))));
6571 evalcond[3]=((((r10)*(sj4)*(x1461)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1467)))+(((x1463)*(x1464)))+(((IkReal(-1.00000000000000))*(sj0)*(x1462)*(x1466)))+(((IkReal(-1.00000000000000))*(r01)*(x1466)*(x1467)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x1465)))+(x1472));
6572 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6579 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6580 vinfos[0].jointtype = 1;
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 std::vector<int> vfree(0);
6623 IkReal j1array[1], cj1array[1], sj1array[1];
6624 bool j1valid[1]={
false};
6626 IkReal x1473=((cj4)*(r22));
6627 IkReal x1474=((r20)*(sj5));
6628 IkReal x1475=((cj5)*(r21));
6629 IkReal x1476=((sj2)*(sj4));
6630 IkReal x1477=((r21)*(sj5));
6631 IkReal x1478=((IkReal(1.00000000000000))*(cj2));
6632 IkReal x1479=((cj5)*(r20));
6633 if(
IKabs(((gconst92)*(((((cj2)*(x1475)))+(((IkReal(-1.00000000000000))*(x1476)*(x1479)))+(((sj2)*(x1473)))+(((x1476)*(x1477)))+(((cj2)*(x1474))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst92)*(((((sj2)*(x1474)))+(((IkReal(-1.00000000000000))*(sj4)*(x1477)*(x1478)))+(((IkReal(-1.00000000000000))*(x1473)*(x1478)))+(((cj2)*(sj4)*(x1479)))+(((sj2)*(x1475))))))) <
IKFAST_ATAN2_MAGTHRESH )
6635 j1array[0]=
IKatan2(((gconst92)*(((((cj2)*(x1475)))+(((IkReal(-1.00000000000000))*(x1476)*(x1479)))+(((sj2)*(x1473)))+(((x1476)*(x1477)))+(((cj2)*(x1474)))))), ((gconst92)*(((((sj2)*(x1474)))+(((IkReal(-1.00000000000000))*(sj4)*(x1477)*(x1478)))+(((IkReal(-1.00000000000000))*(x1473)*(x1478)))+(((cj2)*(sj4)*(x1479)))+(((sj2)*(x1475)))))));
6636 sj1array[0]=
IKsin(j1array[0]);
6637 cj1array[0]=
IKcos(j1array[0]);
6638 if( j1array[0] >
IKPI )
6642 else if( j1array[0] < -
IKPI )
6643 { j1array[0]+=
IK2PI;
6646 for(
int ij1 = 0; ij1 < 1; ++ij1)
6652 _ij1[0] = ij1; _ij1[1] = -1;
6653 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6657 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6660 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6663 IkReal x1480=
IKsin(j1);
6664 IkReal x1481=
IKcos(j1);
6665 IkReal x1482=((cj5)*(sj0));
6666 IkReal x1483=((IkReal(1.00000000000000))*(r11));
6667 IkReal x1484=((cj5)*(sj4));
6668 IkReal x1485=((cj0)*(r00));
6669 IkReal x1486=((IkReal(1.00000000000000))*(sj0));
6670 IkReal x1487=((sj4)*(sj5));
6671 IkReal x1488=((IkReal(1.00000000000000))*(cj0));
6672 IkReal x1489=((sj2)*(x1481));
6673 IkReal x1490=((cj2)*(x1480));
6674 IkReal x1491=((cj2)*(x1481));
6675 IkReal x1492=((sj2)*(x1480));
6676 IkReal x1493=((x1489)+(x1490));
6677 evalcond[0]=((x1493)+(((r20)*(sj5)))+(((cj5)*(r21))));
6678 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1484)))+(((IkReal(-1.00000000000000))*(x1491)))+(x1492)+(((cj4)*(r22)))+(((r21)*(x1487))));
6679 evalcond[2]=((x1491)+(((IkReal(-1.00000000000000))*(x1482)*(x1483)))+(((IkReal(-1.00000000000000))*(cj5)*(r01)*(x1488)))+(((IkReal(-1.00000000000000))*(x1492)))+(((IkReal(-1.00000000000000))*(r10)*(sj5)*(x1486)))+(((IkReal(-1.00000000000000))*(sj5)*(x1485))));
6680 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x1487)*(x1488)))+(((IkReal(-1.00000000000000))*(sj0)*(x1483)*(x1487)))+(((r10)*(sj4)*(x1482)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1488)))+(x1493)+(((x1484)*(x1485)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x1486))));
6681 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6688 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6689 vinfos[0].jointtype = 1;
6690 vinfos[0].foffset = j0;
6691 vinfos[0].indices[0] = _ij0[0];
6692 vinfos[0].indices[1] = _ij0[1];
6693 vinfos[0].maxsolutions = _nj0;
6694 vinfos[1].jointtype = 1;
6695 vinfos[1].foffset = j1;
6696 vinfos[1].indices[0] = _ij1[0];
6697 vinfos[1].indices[1] = _ij1[1];
6698 vinfos[1].maxsolutions = _nj1;
6699 vinfos[2].jointtype = 1;
6700 vinfos[2].foffset = j2;
6701 vinfos[2].indices[0] = _ij2[0];
6702 vinfos[2].indices[1] = _ij2[1];
6703 vinfos[2].maxsolutions = _nj2;
6704 vinfos[3].jointtype = 1;
6705 vinfos[3].foffset = j3;
6706 vinfos[3].indices[0] = _ij3[0];
6707 vinfos[3].indices[1] = _ij3[1];
6708 vinfos[3].maxsolutions = _nj3;
6709 vinfos[4].jointtype = 1;
6710 vinfos[4].foffset = j4;
6711 vinfos[4].indices[0] = _ij4[0];
6712 vinfos[4].indices[1] = _ij4[1];
6713 vinfos[4].maxsolutions = _nj4;
6714 vinfos[5].jointtype = 1;
6715 vinfos[5].foffset = j5;
6716 vinfos[5].indices[0] = _ij5[0];
6717 vinfos[5].indices[1] = _ij5[1];
6718 vinfos[5].maxsolutions = _nj5;
6719 std::vector<int> vfree(0);
6731 IkReal x1494=((IkReal(1.00000000000000))*(r10));
6732 IkReal x1495=((sj0)*(sj5));
6733 IkReal x1496=((r02)*(sj0));
6734 IkReal x1497=((cj4)*(cj5));
6735 IkReal x1498=((IkReal(1.00000000000000))*(cj4));
6736 IkReal x1499=((cj0)*(r12));
6737 IkReal x1500=((IkReal(1.00000000000000))*(cj0));
6738 IkReal x1501=((cj5)*(r11));
6739 IkReal x1502=((cj5)*(r01));
6740 IkReal x1503=((r00)*(sj0));
6741 IkReal x1504=((npy)*(sj5));
6742 IkReal x1505=((IkReal(1.00000000000000))*(sj4));
6743 IkReal x1506=((cj0)*(sj5));
6744 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(((IkReal(3.14159265358979))+(j3)), IkReal(6.28318530717959))));
6745 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
6746 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(cj5)*(npx)*(x1505)))+(((cj4)*(npz)))+(((sj4)*(x1504))));
6747 evalcond[3]=((((npx)*(x1497)))+(((IkReal(0.350000000000000))*(sj2)))+(((npz)*(sj4)))+(((IkReal(-1.00000000000000))*(x1498)*(x1504))));
6748 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
6749 evalcond[5]=((IkReal(1.00000000000000))+(((sj0)*(x1502)))+(((IkReal(-1.00000000000000))*(x1500)*(x1501)))+(((r00)*(x1495)))+(((IkReal(-1.00000000000000))*(x1494)*(x1506))));
6750 evalcond[6]=((((IkReal(-1.00000000000000))*(x1494)*(x1495)))+(((IkReal(-1.00000000000000))*(x1500)*(x1502)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1500)))+(((IkReal(-1.00000000000000))*(sj0)*(x1501))));
6751 evalcond[7]=((((IkReal(-1.00000000000000))*(cj5)*(x1503)*(x1505)))+(((cj4)*(x1496)))+(((cj0)*(cj5)*(r10)*(sj4)))+(((r01)*(sj4)*(x1495)))+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(sj5)*(x1500)))+(((IkReal(-1.00000000000000))*(x1498)*(x1499))));
6752 evalcond[8]=((((IkReal(-1.00000000000000))*(cj0)*(x1494)*(x1497)))+(((sj4)*(x1496)))+(((IkReal(-1.00000000000000))*(r01)*(x1495)*(x1498)))+(((IkReal(-1.00000000000000))*(x1499)*(x1505)))+(((x1497)*(x1503)))+(((cj4)*(r11)*(x1506))));
6753 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
6756 IkReal dummyeval[1];
6758 gconst94=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
6759 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
6760 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6763 IkReal dummyeval[1];
6765 gconst95=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
6766 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
6767 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
6774 IkReal j1array[1], cj1array[1], sj1array[1];
6775 bool j1valid[1]={
false};
6777 IkReal x1507=((cj4)*(r22));
6778 IkReal x1508=((IkReal(1.00000000000000))*(cj2));
6779 IkReal x1509=((sj2)*(sj4));
6780 IkReal x1510=((r21)*(sj5));
6781 IkReal x1511=((IkReal(1.00000000000000))*(sj2));
6782 IkReal x1512=((cj2)*(sj4));
6783 IkReal x1513=((cj5)*(r20));
6784 IkReal x1514=((cj0)*(cj4)*(r02));
6785 IkReal x1515=((cj4)*(r12)*(sj0));
6786 IkReal x1516=((cj5)*(r10)*(sj0));
6787 IkReal x1517=((cj0)*(cj5)*(r00));
6788 IkReal x1518=((cj0)*(r01)*(sj5));
6789 IkReal x1519=((IkReal(1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5));
6790 if(
IKabs(((gconst95)*(((((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1514)))+(((x1512)*(x1517)))+(((IkReal(-1.00000000000000))*(sj4)*(x1508)*(x1518)))+(((x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1515)))+(((x1512)*(x1516)))+(((sj2)*(x1507)))+(((IkReal(-1.00000000000000))*(x1509)*(x1513))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst95)*(((((x1512)*(x1513)))+(((IkReal(-1.00000000000000))*(x1509)*(x1518)))+(((IkReal(-1.00000000000000))*(x1507)*(x1508)))+(((IkReal(-1.00000000000000))*(x1511)*(x1515)))+(((x1509)*(x1517)))+(((IkReal(-1.00000000000000))*(x1511)*(x1514)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj5)*(x1509)))+(((x1509)*(x1516)))+(((IkReal(-1.00000000000000))*(sj4)*(x1508)*(x1510))))))) <
IKFAST_ATAN2_MAGTHRESH )
6792 j1array[0]=
IKatan2(((gconst95)*(((((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1514)))+(((x1512)*(x1517)))+(((IkReal(-1.00000000000000))*(sj4)*(x1508)*(x1518)))+(((x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1515)))+(((x1512)*(x1516)))+(((sj2)*(x1507)))+(((IkReal(-1.00000000000000))*(x1509)*(x1513)))))), ((gconst95)*(((((x1512)*(x1513)))+(((IkReal(-1.00000000000000))*(x1509)*(x1518)))+(((IkReal(-1.00000000000000))*(x1507)*(x1508)))+(((IkReal(-1.00000000000000))*(x1511)*(x1515)))+(((x1509)*(x1517)))+(((IkReal(-1.00000000000000))*(x1511)*(x1514)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj5)*(x1509)))+(((x1509)*(x1516)))+(((IkReal(-1.00000000000000))*(sj4)*(x1508)*(x1510)))))));
6793 sj1array[0]=
IKsin(j1array[0]);
6794 cj1array[0]=
IKcos(j1array[0]);
6795 if( j1array[0] >
IKPI )
6799 else if( j1array[0] < -
IKPI )
6800 { j1array[0]+=
IK2PI;
6803 for(
int ij1 = 0; ij1 < 1; ++ij1)
6809 _ij1[0] = ij1; _ij1[1] = -1;
6810 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6814 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6817 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6820 IkReal x1520=
IKcos(j1);
6821 IkReal x1521=
IKsin(j1);
6822 IkReal x1522=((IkReal(1.00000000000000))*(sj4));
6823 IkReal x1523=((cj5)*(r00));
6824 IkReal x1524=((cj5)*(r20));
6825 IkReal x1525=((r12)*(sj0));
6826 IkReal x1526=((r21)*(sj5));
6827 IkReal x1527=((IkReal(1.00000000000000))*(cj4));
6828 IkReal x1528=((sj2)*(x1520));
6829 IkReal x1529=((cj2)*(x1521));
6830 IkReal x1530=((cj2)*(x1520));
6831 IkReal x1531=((r11)*(sj0)*(sj5));
6832 IkReal x1532=((cj0)*(x1527));
6833 IkReal x1533=((cj0)*(r01)*(sj5));
6834 IkReal x1534=((cj5)*(r10)*(sj0));
6835 IkReal x1535=((sj2)*(x1521));
6836 IkReal x1536=((x1528)+(x1529));
6837 evalcond[0]=((((IkReal(-1.00000000000000))*(x1522)*(x1524)))+(((IkReal(-1.00000000000000))*(x1530)))+(((cj4)*(r22)))+(((sj4)*(x1526)))+(x1535));
6838 evalcond[1]=((((IkReal(-1.00000000000000))*(x1526)*(x1527)))+(((cj4)*(x1524)))+(((r22)*(sj4)))+(x1536));
6839 evalcond[2]=((((IkReal(-1.00000000000000))*(x1522)*(x1531)))+(((IkReal(-1.00000000000000))*(r02)*(x1532)))+(((IkReal(-1.00000000000000))*(x1522)*(x1533)))+(((cj0)*(sj4)*(x1523)))+(((sj4)*(x1534)))+(x1536)+(((IkReal(-1.00000000000000))*(x1525)*(x1527))));
6840 evalcond[3]=((((IkReal(-1.00000000000000))*(x1535)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1522)))+(((cj4)*(x1531)))+(((IkReal(-1.00000000000000))*(x1522)*(x1525)))+(((cj4)*(x1533)))+(((IkReal(-1.00000000000000))*(x1527)*(x1534)))+(((IkReal(-1.00000000000000))*(x1523)*(x1532)))+(x1530));
6841 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6848 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6849 vinfos[0].jointtype = 1;
6850 vinfos[0].foffset = j0;
6851 vinfos[0].indices[0] = _ij0[0];
6852 vinfos[0].indices[1] = _ij0[1];
6853 vinfos[0].maxsolutions = _nj0;
6854 vinfos[1].jointtype = 1;
6855 vinfos[1].foffset = j1;
6856 vinfos[1].indices[0] = _ij1[0];
6857 vinfos[1].indices[1] = _ij1[1];
6858 vinfos[1].maxsolutions = _nj1;
6859 vinfos[2].jointtype = 1;
6860 vinfos[2].foffset = j2;
6861 vinfos[2].indices[0] = _ij2[0];
6862 vinfos[2].indices[1] = _ij2[1];
6863 vinfos[2].maxsolutions = _nj2;
6864 vinfos[3].jointtype = 1;
6865 vinfos[3].foffset = j3;
6866 vinfos[3].indices[0] = _ij3[0];
6867 vinfos[3].indices[1] = _ij3[1];
6868 vinfos[3].maxsolutions = _nj3;
6869 vinfos[4].jointtype = 1;
6870 vinfos[4].foffset = j4;
6871 vinfos[4].indices[0] = _ij4[0];
6872 vinfos[4].indices[1] = _ij4[1];
6873 vinfos[4].maxsolutions = _nj4;
6874 vinfos[5].jointtype = 1;
6875 vinfos[5].foffset = j5;
6876 vinfos[5].indices[0] = _ij5[0];
6877 vinfos[5].indices[1] = _ij5[1];
6878 vinfos[5].maxsolutions = _nj5;
6879 std::vector<int> vfree(0);
6892 IkReal j1array[1], cj1array[1], sj1array[1];
6893 bool j1valid[1]={
false};
6895 IkReal x1537=((cj4)*(r22));
6896 IkReal x1538=((cj2)*(sj4));
6897 IkReal x1539=((sj2)*(sj4));
6898 IkReal x1540=((r21)*(sj5));
6899 IkReal x1541=((cj5)*(r20));
6900 IkReal x1542=((cj4)*(sj2));
6901 IkReal x1543=((IkReal(1.00000000000000))*(cj2)*(x1540));
6902 if(
IKabs(((gconst94)*(((((r22)*(x1538)))+(((IkReal(-1.00000000000000))*(x1539)*(x1541)))+(((cj2)*(cj4)*(x1541)))+(((x1539)*(x1540)))+(((IkReal(-1.00000000000000))*(cj4)*(x1543)))+(((sj2)*(x1537))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst94)*(((((IkReal(-1.00000000000000))*(cj2)*(x1537)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(x1540)*(x1542)))+(((IkReal(-1.00000000000000))*(x1538)*(x1540)))+(((x1541)*(x1542)))+(((x1538)*(x1541))))))) <
IKFAST_ATAN2_MAGTHRESH )
6904 j1array[0]=
IKatan2(((gconst94)*(((((r22)*(x1538)))+(((IkReal(-1.00000000000000))*(x1539)*(x1541)))+(((cj2)*(cj4)*(x1541)))+(((x1539)*(x1540)))+(((IkReal(-1.00000000000000))*(cj4)*(x1543)))+(((sj2)*(x1537)))))), ((gconst94)*(((((IkReal(-1.00000000000000))*(cj2)*(x1537)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(x1540)*(x1542)))+(((IkReal(-1.00000000000000))*(x1538)*(x1540)))+(((x1541)*(x1542)))+(((x1538)*(x1541)))))));
6905 sj1array[0]=
IKsin(j1array[0]);
6906 cj1array[0]=
IKcos(j1array[0]);
6907 if( j1array[0] >
IKPI )
6911 else if( j1array[0] < -
IKPI )
6912 { j1array[0]+=
IK2PI;
6915 for(
int ij1 = 0; ij1 < 1; ++ij1)
6921 _ij1[0] = ij1; _ij1[1] = -1;
6922 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
6926 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
6929 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
6932 IkReal x1544=
IKcos(j1);
6933 IkReal x1545=
IKsin(j1);
6934 IkReal x1546=((IkReal(1.00000000000000))*(sj4));
6935 IkReal x1547=((cj5)*(r00));
6936 IkReal x1548=((cj5)*(r20));
6937 IkReal x1549=((r12)*(sj0));
6938 IkReal x1550=((r21)*(sj5));
6939 IkReal x1551=((IkReal(1.00000000000000))*(cj4));
6940 IkReal x1552=((sj2)*(x1544));
6941 IkReal x1553=((cj2)*(x1545));
6942 IkReal x1554=((cj2)*(x1544));
6943 IkReal x1555=((r11)*(sj0)*(sj5));
6944 IkReal x1556=((cj0)*(x1551));
6945 IkReal x1557=((cj0)*(r01)*(sj5));
6946 IkReal x1558=((cj5)*(r10)*(sj0));
6947 IkReal x1559=((sj2)*(x1545));
6948 IkReal x1560=((x1553)+(x1552));
6949 evalcond[0]=((x1559)+(((cj4)*(r22)))+(((sj4)*(x1550)))+(((IkReal(-1.00000000000000))*(x1546)*(x1548)))+(((IkReal(-1.00000000000000))*(x1554))));
6950 evalcond[1]=((((IkReal(-1.00000000000000))*(x1550)*(x1551)))+(((cj4)*(x1548)))+(((r22)*(sj4)))+(x1560));
6951 evalcond[2]=((((IkReal(-1.00000000000000))*(x1546)*(x1557)))+(((IkReal(-1.00000000000000))*(x1546)*(x1555)))+(((sj4)*(x1558)))+(x1560)+(((IkReal(-1.00000000000000))*(r02)*(x1556)))+(((cj0)*(sj4)*(x1547)))+(((IkReal(-1.00000000000000))*(x1549)*(x1551))));
6952 evalcond[3]=((((IkReal(-1.00000000000000))*(x1551)*(x1558)))+(((IkReal(-1.00000000000000))*(x1559)))+(((IkReal(-1.00000000000000))*(x1546)*(x1549)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1546)))+(x1554)+(((cj4)*(x1557)))+(((IkReal(-1.00000000000000))*(x1547)*(x1556)))+(((cj4)*(x1555))));
6953 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
6960 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6961 vinfos[0].jointtype = 1;
6962 vinfos[0].foffset = j0;
6963 vinfos[0].indices[0] = _ij0[0];
6964 vinfos[0].indices[1] = _ij0[1];
6965 vinfos[0].maxsolutions = _nj0;
6966 vinfos[1].jointtype = 1;
6967 vinfos[1].foffset = j1;
6968 vinfos[1].indices[0] = _ij1[0];
6969 vinfos[1].indices[1] = _ij1[1];
6970 vinfos[1].maxsolutions = _nj1;
6971 vinfos[2].jointtype = 1;
6972 vinfos[2].foffset = j2;
6973 vinfos[2].indices[0] = _ij2[0];
6974 vinfos[2].indices[1] = _ij2[1];
6975 vinfos[2].maxsolutions = _nj2;
6976 vinfos[3].jointtype = 1;
6977 vinfos[3].foffset = j3;
6978 vinfos[3].indices[0] = _ij3[0];
6979 vinfos[3].indices[1] = _ij3[1];
6980 vinfos[3].maxsolutions = _nj3;
6981 vinfos[4].jointtype = 1;
6982 vinfos[4].foffset = j4;
6983 vinfos[4].indices[0] = _ij4[0];
6984 vinfos[4].indices[1] = _ij4[1];
6985 vinfos[4].maxsolutions = _nj4;
6986 vinfos[5].jointtype = 1;
6987 vinfos[5].foffset = j5;
6988 vinfos[5].indices[0] = _ij5[0];
6989 vinfos[5].indices[1] = _ij5[1];
6990 vinfos[5].maxsolutions = _nj5;
6991 std::vector<int> vfree(0);
7003 IkReal x1561=((IkReal(1.00000000000000))*(r10));
7004 IkReal x1562=((sj0)*(sj5));
7005 IkReal x1563=((r02)*(sj0));
7006 IkReal x1564=((cj4)*(cj5));
7007 IkReal x1565=((IkReal(1.00000000000000))*(cj4));
7008 IkReal x1566=((cj0)*(r12));
7009 IkReal x1567=((IkReal(1.00000000000000))*(cj0));
7010 IkReal x1568=((cj5)*(r11));
7011 IkReal x1569=((cj5)*(r01));
7012 IkReal x1570=((r00)*(sj0));
7013 IkReal x1571=((npy)*(sj5));
7014 IkReal x1572=((IkReal(1.00000000000000))*(sj4));
7015 IkReal x1573=((cj0)*(sj5));
7016 evalcond[0]=((IkReal(-3.14159265358979))+(
IKfmod(j3, IkReal(6.28318530717959))));
7017 evalcond[1]=((IkReal(0.0950000000000000))+(((cj5)*(npy)))+(((npx)*(sj5))));
7018 evalcond[2]=((IkReal(-0.350000000000000))+(((IkReal(-0.350000000000000))*(cj2)))+(((cj4)*(npz)))+(((sj4)*(x1571)))+(((IkReal(-1.00000000000000))*(cj5)*(npx)*(x1572))));
7019 evalcond[3]=((((IkReal(-1.00000000000000))*(x1565)*(x1571)))+(((npx)*(x1564)))+(((IkReal(-0.350000000000000))*(sj2)))+(((npz)*(sj4))));
7020 evalcond[4]=((((r20)*(sj5)))+(((cj5)*(r21))));
7021 evalcond[5]=((IkReal(-1.00000000000000))+(((r00)*(x1562)))+(((sj0)*(x1569)))+(((IkReal(-1.00000000000000))*(x1561)*(x1573)))+(((IkReal(-1.00000000000000))*(x1567)*(x1568))));
7022 evalcond[6]=((((IkReal(-1.00000000000000))*(x1561)*(x1562)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1567)))+(((IkReal(-1.00000000000000))*(x1567)*(x1569)))+(((IkReal(-1.00000000000000))*(sj0)*(x1568))));
7023 evalcond[7]=((((IkReal(-1.00000000000000))*(r11)*(sj4)*(sj5)*(x1567)))+(((IkReal(-1.00000000000000))*(cj5)*(x1570)*(x1572)))+(((r01)*(sj4)*(x1562)))+(((cj4)*(x1563)))+(((IkReal(-1.00000000000000))*(x1565)*(x1566)))+(((cj0)*(cj5)*(r10)*(sj4))));
7024 evalcond[8]=((((x1564)*(x1570)))+(((sj4)*(x1563)))+(((cj4)*(r11)*(x1573)))+(((IkReal(-1.00000000000000))*(x1566)*(x1572)))+(((IkReal(-1.00000000000000))*(r01)*(x1562)*(x1565)))+(((IkReal(-1.00000000000000))*(cj0)*(x1561)*(x1564))));
7025 if(
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
IKabs(evalcond[6]) < 0.0000010000000000 &&
IKabs(evalcond[7]) < 0.0000010000000000 &&
IKabs(evalcond[8]) < 0.0000010000000000 )
7028 IkReal dummyeval[1];
7030 gconst96=
IKsign((((cj2)*(cj2))+((sj2)*(sj2))));
7031 dummyeval[0]=(((cj2)*(cj2))+((sj2)*(sj2)));
7032 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7035 IkReal dummyeval[1];
7037 gconst97=
IKsign(((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2))))));
7038 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj2)*(sj2))))+(((IkReal(-1.00000000000000))*((cj2)*(cj2)))));
7039 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7046 IkReal j1array[1], cj1array[1], sj1array[1];
7047 bool j1valid[1]={
false};
7049 IkReal x1574=((cj4)*(r22));
7050 IkReal x1575=((IkReal(1.00000000000000))*(cj2));
7051 IkReal x1576=((sj2)*(sj4));
7052 IkReal x1577=((r21)*(sj5));
7053 IkReal x1578=((cj2)*(sj4));
7054 IkReal x1579=((cj5)*(r20));
7055 IkReal x1580=((IkReal(1.00000000000000))*(sj2));
7056 IkReal x1581=((cj0)*(cj4)*(r02));
7057 IkReal x1582=((cj5)*(r10)*(sj0));
7058 IkReal x1583=((cj4)*(r12)*(sj0));
7059 IkReal x1584=((cj0)*(cj5)*(r00));
7060 IkReal x1585=((cj0)*(r01)*(sj5));
7061 IkReal x1586=((IkReal(1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5));
7062 if(
IKabs(((gconst97)*(((((sj2)*(x1574)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5)*(x1575)))+(((x1576)*(x1577)))+(((x1578)*(x1582)))+(((x1578)*(x1584)))+(((IkReal(-1.00000000000000))*(x1575)*(x1583)))+(((IkReal(-1.00000000000000))*(sj4)*(x1575)*(x1585)))+(((IkReal(-1.00000000000000))*(x1575)*(x1581)))+(((IkReal(-1.00000000000000))*(x1576)*(x1579))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst97)*(((((IkReal(-1.00000000000000))*(x1580)*(x1583)))+(((x1576)*(x1584)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj5)*(x1576)))+(((IkReal(-1.00000000000000))*(x1574)*(x1575)))+(((IkReal(-1.00000000000000))*(sj4)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(x1580)*(x1581)))+(((x1578)*(x1579)))+(((IkReal(-1.00000000000000))*(x1576)*(x1585)))+(((x1576)*(x1582))))))) <
IKFAST_ATAN2_MAGTHRESH )
7064 j1array[0]=
IKatan2(((gconst97)*(((((sj2)*(x1574)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj4)*(sj5)*(x1575)))+(((x1576)*(x1577)))+(((x1578)*(x1582)))+(((x1578)*(x1584)))+(((IkReal(-1.00000000000000))*(x1575)*(x1583)))+(((IkReal(-1.00000000000000))*(sj4)*(x1575)*(x1585)))+(((IkReal(-1.00000000000000))*(x1575)*(x1581)))+(((IkReal(-1.00000000000000))*(x1576)*(x1579)))))), ((gconst97)*(((((IkReal(-1.00000000000000))*(x1580)*(x1583)))+(((x1576)*(x1584)))+(((IkReal(-1.00000000000000))*(r11)*(sj0)*(sj5)*(x1576)))+(((IkReal(-1.00000000000000))*(x1574)*(x1575)))+(((IkReal(-1.00000000000000))*(sj4)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(x1580)*(x1581)))+(((x1578)*(x1579)))+(((IkReal(-1.00000000000000))*(x1576)*(x1585)))+(((x1576)*(x1582)))))));
7065 sj1array[0]=
IKsin(j1array[0]);
7066 cj1array[0]=
IKcos(j1array[0]);
7067 if( j1array[0] >
IKPI )
7071 else if( j1array[0] < -
IKPI )
7072 { j1array[0]+=
IK2PI;
7075 for(
int ij1 = 0; ij1 < 1; ++ij1)
7081 _ij1[0] = ij1; _ij1[1] = -1;
7082 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7086 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7089 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7092 IkReal x1587=
IKcos(j1);
7093 IkReal x1588=
IKsin(j1);
7094 IkReal x1589=((IkReal(1.00000000000000))*(sj4));
7095 IkReal x1590=((cj5)*(r00));
7096 IkReal x1591=((cj5)*(r20));
7097 IkReal x1592=((r12)*(sj0));
7098 IkReal x1593=((r21)*(sj5));
7099 IkReal x1594=((IkReal(1.00000000000000))*(cj4));
7100 IkReal x1595=((sj2)*(x1588));
7101 IkReal x1596=((IkReal(1.00000000000000))*(x1587));
7102 IkReal x1597=((r11)*(sj0)*(sj5));
7103 IkReal x1598=((cj0)*(x1594));
7104 IkReal x1599=((cj0)*(r01)*(sj5));
7105 IkReal x1600=((cj2)*(x1588));
7106 IkReal x1601=((cj5)*(r10)*(sj0));
7107 IkReal x1602=((cj2)*(x1596));
7108 evalcond[0]=((((IkReal(-1.00000000000000))*(x1602)))+(x1595)+(((sj4)*(x1593)))+(((IkReal(-1.00000000000000))*(x1589)*(x1591)))+(((cj4)*(r22))));
7109 evalcond[1]=((((IkReal(-1.00000000000000))*(x1600)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(sj2)*(x1596)))+(((cj4)*(x1591)))+(((IkReal(-1.00000000000000))*(x1593)*(x1594))));
7110 evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x1598)))+(((IkReal(-1.00000000000000))*(x1589)*(x1597)))+(((IkReal(-1.00000000000000))*(x1589)*(x1599)))+(((sj2)*(x1587)))+(((IkReal(-1.00000000000000))*(x1592)*(x1594)))+(((cj0)*(sj4)*(x1590)))+(((sj4)*(x1601)))+(x1600));
7111 evalcond[3]=((((IkReal(-1.00000000000000))*(x1602)))+(x1595)+(((IkReal(-1.00000000000000))*(x1594)*(x1601)))+(((IkReal(-1.00000000000000))*(x1589)*(x1592)))+(((cj4)*(x1597)))+(((cj4)*(x1599)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1589)))+(((IkReal(-1.00000000000000))*(x1590)*(x1598))));
7112 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
7119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7120 vinfos[0].jointtype = 1;
7121 vinfos[0].foffset = j0;
7122 vinfos[0].indices[0] = _ij0[0];
7123 vinfos[0].indices[1] = _ij0[1];
7124 vinfos[0].maxsolutions = _nj0;
7125 vinfos[1].jointtype = 1;
7126 vinfos[1].foffset = j1;
7127 vinfos[1].indices[0] = _ij1[0];
7128 vinfos[1].indices[1] = _ij1[1];
7129 vinfos[1].maxsolutions = _nj1;
7130 vinfos[2].jointtype = 1;
7131 vinfos[2].foffset = j2;
7132 vinfos[2].indices[0] = _ij2[0];
7133 vinfos[2].indices[1] = _ij2[1];
7134 vinfos[2].maxsolutions = _nj2;
7135 vinfos[3].jointtype = 1;
7136 vinfos[3].foffset = j3;
7137 vinfos[3].indices[0] = _ij3[0];
7138 vinfos[3].indices[1] = _ij3[1];
7139 vinfos[3].maxsolutions = _nj3;
7140 vinfos[4].jointtype = 1;
7141 vinfos[4].foffset = j4;
7142 vinfos[4].indices[0] = _ij4[0];
7143 vinfos[4].indices[1] = _ij4[1];
7144 vinfos[4].maxsolutions = _nj4;
7145 vinfos[5].jointtype = 1;
7146 vinfos[5].foffset = j5;
7147 vinfos[5].indices[0] = _ij5[0];
7148 vinfos[5].indices[1] = _ij5[1];
7149 vinfos[5].maxsolutions = _nj5;
7150 std::vector<int> vfree(0);
7163 IkReal j1array[1], cj1array[1], sj1array[1];
7164 bool j1valid[1]={
false};
7166 IkReal x1603=((IkReal(1.00000000000000))*(cj2));
7167 IkReal x1604=((cj2)*(cj4));
7168 IkReal x1605=((r21)*(sj5));
7169 IkReal x1606=((cj5)*(r20));
7170 IkReal x1607=((r22)*(sj4));
7171 IkReal x1608=((cj4)*(sj2));
7172 IkReal x1609=((sj4)*(x1606));
7173 IkReal x1610=((IkReal(1.00000000000000))*(sj2)*(x1605));
7174 if(
IKabs(((gconst96)*(((((sj2)*(x1609)))+(((x1604)*(x1606)))+(((IkReal(-1.00000000000000))*(r22)*(x1608)))+(((cj2)*(x1607)))+(((IkReal(-1.00000000000000))*(sj4)*(x1610)))+(((IkReal(-1.00000000000000))*(cj4)*(x1603)*(x1605))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst96)*(((((sj2)*(x1607)))+(((IkReal(-1.00000000000000))*(x1605)*(x1608)))+(((cj2)*(sj4)*(x1605)))+(((r22)*(x1604)))+(((x1606)*(x1608)))+(((IkReal(-1.00000000000000))*(x1603)*(x1609))))))) <
IKFAST_ATAN2_MAGTHRESH )
7176 j1array[0]=
IKatan2(((gconst96)*(((((sj2)*(x1609)))+(((x1604)*(x1606)))+(((IkReal(-1.00000000000000))*(r22)*(x1608)))+(((cj2)*(x1607)))+(((IkReal(-1.00000000000000))*(sj4)*(x1610)))+(((IkReal(-1.00000000000000))*(cj4)*(x1603)*(x1605)))))), ((gconst96)*(((((sj2)*(x1607)))+(((IkReal(-1.00000000000000))*(x1605)*(x1608)))+(((cj2)*(sj4)*(x1605)))+(((r22)*(x1604)))+(((x1606)*(x1608)))+(((IkReal(-1.00000000000000))*(x1603)*(x1609)))))));
7177 sj1array[0]=
IKsin(j1array[0]);
7178 cj1array[0]=
IKcos(j1array[0]);
7179 if( j1array[0] >
IKPI )
7183 else if( j1array[0] < -
IKPI )
7184 { j1array[0]+=
IK2PI;
7187 for(
int ij1 = 0; ij1 < 1; ++ij1)
7193 _ij1[0] = ij1; _ij1[1] = -1;
7194 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7198 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7201 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7204 IkReal x1611=
IKcos(j1);
7205 IkReal x1612=
IKsin(j1);
7206 IkReal x1613=((IkReal(1.00000000000000))*(sj4));
7207 IkReal x1614=((cj5)*(r00));
7208 IkReal x1615=((cj5)*(r20));
7209 IkReal x1616=((r12)*(sj0));
7210 IkReal x1617=((r21)*(sj5));
7211 IkReal x1618=((IkReal(1.00000000000000))*(cj4));
7212 IkReal x1619=((sj2)*(x1612));
7213 IkReal x1620=((IkReal(1.00000000000000))*(x1611));
7214 IkReal x1621=((r11)*(sj0)*(sj5));
7215 IkReal x1622=((cj0)*(x1618));
7216 IkReal x1623=((cj0)*(r01)*(sj5));
7217 IkReal x1624=((cj2)*(x1612));
7218 IkReal x1625=((cj5)*(r10)*(sj0));
7219 IkReal x1626=((cj2)*(x1620));
7220 evalcond[0]=((((IkReal(-1.00000000000000))*(x1626)))+(x1619)+(((cj4)*(r22)))+(((sj4)*(x1617)))+(((IkReal(-1.00000000000000))*(x1613)*(x1615))));
7221 evalcond[1]=((((IkReal(-1.00000000000000))*(x1624)))+(((IkReal(-1.00000000000000))*(sj2)*(x1620)))+(((r22)*(sj4)))+(((cj4)*(x1615)))+(((IkReal(-1.00000000000000))*(x1617)*(x1618))));
7222 evalcond[2]=((((IkReal(-1.00000000000000))*(x1616)*(x1618)))+(((sj2)*(x1611)))+(x1624)+(((IkReal(-1.00000000000000))*(x1613)*(x1623)))+(((cj0)*(sj4)*(x1614)))+(((sj4)*(x1625)))+(((IkReal(-1.00000000000000))*(r02)*(x1622)))+(((IkReal(-1.00000000000000))*(x1613)*(x1621))));
7223 evalcond[3]=((((IkReal(-1.00000000000000))*(x1626)))+(x1619)+(((IkReal(-1.00000000000000))*(x1613)*(x1616)))+(((IkReal(-1.00000000000000))*(x1614)*(x1622)))+(((IkReal(-1.00000000000000))*(x1618)*(x1625)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1613)))+(((cj4)*(x1623)))+(((cj4)*(x1621))));
7224 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 )
7231 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7232 vinfos[0].jointtype = 1;
7233 vinfos[0].foffset = j0;
7234 vinfos[0].indices[0] = _ij0[0];
7235 vinfos[0].indices[1] = _ij0[1];
7236 vinfos[0].maxsolutions = _nj0;
7237 vinfos[1].jointtype = 1;
7238 vinfos[1].foffset = j1;
7239 vinfos[1].indices[0] = _ij1[0];
7240 vinfos[1].indices[1] = _ij1[1];
7241 vinfos[1].maxsolutions = _nj1;
7242 vinfos[2].jointtype = 1;
7243 vinfos[2].foffset = j2;
7244 vinfos[2].indices[0] = _ij2[0];
7245 vinfos[2].indices[1] = _ij2[1];
7246 vinfos[2].maxsolutions = _nj2;
7247 vinfos[3].jointtype = 1;
7248 vinfos[3].foffset = j3;
7249 vinfos[3].indices[0] = _ij3[0];
7250 vinfos[3].indices[1] = _ij3[1];
7251 vinfos[3].maxsolutions = _nj3;
7252 vinfos[4].jointtype = 1;
7253 vinfos[4].foffset = j4;
7254 vinfos[4].indices[0] = _ij4[0];
7255 vinfos[4].indices[1] = _ij4[1];
7256 vinfos[4].maxsolutions = _nj4;
7257 vinfos[5].jointtype = 1;
7258 vinfos[5].foffset = j5;
7259 vinfos[5].indices[0] = _ij5[0];
7260 vinfos[5].indices[1] = _ij5[1];
7261 vinfos[5].maxsolutions = _nj5;
7262 std::vector<int> vfree(0);
7290 IkReal j1array[1], cj1array[1], sj1array[1];
7291 bool j1valid[1]={
false};
7293 IkReal x1627=((IkReal(1.00000000000000))*(sj4));
7294 IkReal x1628=((r22)*(sj2));
7295 IkReal x1629=((cj3)*(cj4));
7296 IkReal x1630=((cj2)*(r22));
7297 IkReal x1631=((cj3)*(cj5)*(r20));
7298 IkReal x1632=((cj2)*(r21)*(sj5));
7299 IkReal x1633=((r21)*(sj2)*(sj5));
7300 IkReal x1634=((IkReal(1.00000000000000))*(cj4)*(cj5)*(r20));
7301 if(
IKabs(((gconst89)*(((((sj2)*(sj4)*(x1631)))+(((IkReal(-1.00000000000000))*(x1627)*(x1630)))+(((cj4)*(x1632)))+(((IkReal(-1.00000000000000))*(cj2)*(x1634)))+(((IkReal(-1.00000000000000))*(x1628)*(x1629)))+(((IkReal(-1.00000000000000))*(cj3)*(x1627)*(x1633))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst89)*(((((cj3)*(sj4)*(x1632)))+(((IkReal(-1.00000000000000))*(cj2)*(x1627)*(x1631)))+(((cj4)*(x1633)))+(((x1629)*(x1630)))+(((IkReal(-1.00000000000000))*(sj2)*(x1634)))+(((IkReal(-1.00000000000000))*(x1627)*(x1628))))))) <
IKFAST_ATAN2_MAGTHRESH )
7303 j1array[0]=
IKatan2(((gconst89)*(((((sj2)*(sj4)*(x1631)))+(((IkReal(-1.00000000000000))*(x1627)*(x1630)))+(((cj4)*(x1632)))+(((IkReal(-1.00000000000000))*(cj2)*(x1634)))+(((IkReal(-1.00000000000000))*(x1628)*(x1629)))+(((IkReal(-1.00000000000000))*(cj3)*(x1627)*(x1633)))))), ((gconst89)*(((((cj3)*(sj4)*(x1632)))+(((IkReal(-1.00000000000000))*(cj2)*(x1627)*(x1631)))+(((cj4)*(x1633)))+(((x1629)*(x1630)))+(((IkReal(-1.00000000000000))*(sj2)*(x1634)))+(((IkReal(-1.00000000000000))*(x1627)*(x1628)))))));
7304 sj1array[0]=
IKsin(j1array[0]);
7305 cj1array[0]=
IKcos(j1array[0]);
7306 if( j1array[0] >
IKPI )
7310 else if( j1array[0] < -
IKPI )
7311 { j1array[0]+=
IK2PI;
7314 for(
int ij1 = 0; ij1 < 1; ++ij1)
7320 _ij1[0] = ij1; _ij1[1] = -1;
7321 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7325 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7328 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7331 IkReal x1635=
IKsin(j1);
7332 IkReal x1636=
IKcos(j1);
7333 IkReal x1637=((IkReal(1.00000000000000))*(sj2));
7334 IkReal x1638=((cj5)*(r00));
7335 IkReal x1639=((cj0)*(sj4));
7336 IkReal x1640=((cj5)*(sj4));
7337 IkReal x1641=((IkReal(1.00000000000000))*(sj5));
7338 IkReal x1642=((r10)*(sj0));
7339 IkReal x1643=((IkReal(1.00000000000000))*(sj0));
7340 IkReal x1644=((cj0)*(r01));
7341 IkReal x1645=((cj4)*(sj5));
7342 IkReal x1646=((IkReal(1.00000000000000))*(cj5));
7343 IkReal x1647=((cj2)*(x1636));
7344 IkReal x1648=((sj0)*(x1641));
7345 IkReal x1649=((cj3)*(x1635));
7346 IkReal x1650=((IkReal(1.00000000000000))*(cj0)*(cj4));
7347 IkReal x1651=((sj3)*(x1635));
7348 IkReal x1652=((sj2)*(x1636));
7349 evalcond[0]=((((IkReal(-1.00000000000000))*(cj2)*(x1651)))+(((IkReal(-1.00000000000000))*(sj3)*(x1636)*(x1637)))+(((r20)*(sj5)))+(((cj5)*(r21))));
7350 evalcond[1]=((((sj2)*(x1635)))+(((IkReal(-1.00000000000000))*(x1647)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(r20)*(x1640)))+(((r21)*(sj4)*(sj5))));
7351 evalcond[2]=((((cj2)*(x1649)))+(((cj3)*(x1652)))+(((cj4)*(cj5)*(r20)))+(((r22)*(sj4)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x1641))));
7352 evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1641)))+(((IkReal(-1.00000000000000))*(x1644)*(x1646)))+(((IkReal(-1.00000000000000))*(x1641)*(x1642)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1643)))+(((sj2)*(x1651)))+(((IkReal(-1.00000000000000))*(sj3)*(x1647))));
7353 evalcond[4]=((((x1638)*(x1639)))+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(x1648)))+(((x1640)*(x1642)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x1643)))+(((IkReal(-1.00000000000000))*(r01)*(x1639)*(x1641)))+(x1652)+(((cj2)*(x1635)))+(((IkReal(-1.00000000000000))*(r02)*(x1650))));
7354 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x1642)*(x1646)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1643)))+(((IkReal(-1.00000000000000))*(r02)*(x1639)))+(((x1644)*(x1645)))+(((cj3)*(x1647)))+(((IkReal(-1.00000000000000))*(x1637)*(x1649)))+(((r11)*(sj0)*(x1645)))+(((IkReal(-1.00000000000000))*(x1638)*(x1650))));
7355 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
7362 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7363 vinfos[0].jointtype = 1;
7364 vinfos[0].foffset = j0;
7365 vinfos[0].indices[0] = _ij0[0];
7366 vinfos[0].indices[1] = _ij0[1];
7367 vinfos[0].maxsolutions = _nj0;
7368 vinfos[1].jointtype = 1;
7369 vinfos[1].foffset = j1;
7370 vinfos[1].indices[0] = _ij1[0];
7371 vinfos[1].indices[1] = _ij1[1];
7372 vinfos[1].maxsolutions = _nj1;
7373 vinfos[2].jointtype = 1;
7374 vinfos[2].foffset = j2;
7375 vinfos[2].indices[0] = _ij2[0];
7376 vinfos[2].indices[1] = _ij2[1];
7377 vinfos[2].maxsolutions = _nj2;
7378 vinfos[3].jointtype = 1;
7379 vinfos[3].foffset = j3;
7380 vinfos[3].indices[0] = _ij3[0];
7381 vinfos[3].indices[1] = _ij3[1];
7382 vinfos[3].maxsolutions = _nj3;
7383 vinfos[4].jointtype = 1;
7384 vinfos[4].foffset = j4;
7385 vinfos[4].indices[0] = _ij4[0];
7386 vinfos[4].indices[1] = _ij4[1];
7387 vinfos[4].maxsolutions = _nj4;
7388 vinfos[5].jointtype = 1;
7389 vinfos[5].foffset = j5;
7390 vinfos[5].indices[0] = _ij5[0];
7391 vinfos[5].indices[1] = _ij5[1];
7392 vinfos[5].maxsolutions = _nj5;
7393 std::vector<int> vfree(0);
7406 IkReal j1array[1], cj1array[1], sj1array[1];
7407 bool j1valid[1]={
false};
7409 IkReal x1653=((r20)*(sj5));
7410 IkReal x1654=((cj5)*(r21));
7411 IkReal x1655=((IkReal(1.00000000000000))*(sj2));
7412 IkReal x1656=((cj2)*(sj3));
7413 IkReal x1657=((cj4)*(r22));
7414 IkReal x1658=((cj5)*(r20)*(sj4));
7415 IkReal x1659=((r21)*(sj3)*(sj4)*(sj5));
7416 if(
IKabs(((gconst88)*(((((IkReal(-1.00000000000000))*(x1655)*(x1659)))+(((cj2)*(x1654)))+(((sj2)*(sj3)*(x1658)))+(((IkReal(-1.00000000000000))*(sj3)*(x1655)*(x1657)))+(((cj2)*(x1653))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst88)*(((((r21)*(sj4)*(sj5)*(x1656)))+(((IkReal(-1.00000000000000))*(x1656)*(x1658)))+(((x1656)*(x1657)))+(((sj2)*(x1654)))+(((sj2)*(x1653))))))) <
IKFAST_ATAN2_MAGTHRESH )
7418 j1array[0]=
IKatan2(((gconst88)*(((((IkReal(-1.00000000000000))*(x1655)*(x1659)))+(((cj2)*(x1654)))+(((sj2)*(sj3)*(x1658)))+(((IkReal(-1.00000000000000))*(sj3)*(x1655)*(x1657)))+(((cj2)*(x1653)))))), ((gconst88)*(((((r21)*(sj4)*(sj5)*(x1656)))+(((IkReal(-1.00000000000000))*(x1656)*(x1658)))+(((x1656)*(x1657)))+(((sj2)*(x1654)))+(((sj2)*(x1653)))))));
7419 sj1array[0]=
IKsin(j1array[0]);
7420 cj1array[0]=
IKcos(j1array[0]);
7421 if( j1array[0] >
IKPI )
7425 else if( j1array[0] < -
IKPI )
7426 { j1array[0]+=
IK2PI;
7429 for(
int ij1 = 0; ij1 < 1; ++ij1)
7435 _ij1[0] = ij1; _ij1[1] = -1;
7436 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7440 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7443 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7446 IkReal x1660=
IKsin(j1);
7447 IkReal x1661=
IKcos(j1);
7448 IkReal x1662=((IkReal(1.00000000000000))*(sj2));
7449 IkReal x1663=((cj5)*(r00));
7450 IkReal x1664=((cj0)*(sj4));
7451 IkReal x1665=((cj5)*(sj4));
7452 IkReal x1666=((IkReal(1.00000000000000))*(sj5));
7453 IkReal x1667=((r10)*(sj0));
7454 IkReal x1668=((IkReal(1.00000000000000))*(sj0));
7455 IkReal x1669=((cj0)*(r01));
7456 IkReal x1670=((cj4)*(sj5));
7457 IkReal x1671=((IkReal(1.00000000000000))*(cj5));
7458 IkReal x1672=((cj2)*(x1661));
7459 IkReal x1673=((sj0)*(x1666));
7460 IkReal x1674=((cj3)*(x1660));
7461 IkReal x1675=((IkReal(1.00000000000000))*(cj0)*(cj4));
7462 IkReal x1676=((sj3)*(x1660));
7463 IkReal x1677=((sj2)*(x1661));
7464 evalcond[0]=((((IkReal(-1.00000000000000))*(cj2)*(x1676)))+(((IkReal(-1.00000000000000))*(sj3)*(x1661)*(x1662)))+(((r20)*(sj5)))+(((cj5)*(r21))));
7465 evalcond[1]=((((sj2)*(x1660)))+(((IkReal(-1.00000000000000))*(r20)*(x1665)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(x1672)))+(((r21)*(sj4)*(sj5))));
7466 evalcond[2]=((((cj3)*(x1677)))+(((cj2)*(x1674)))+(((cj4)*(cj5)*(r20)))+(((IkReal(-1.00000000000000))*(cj4)*(r21)*(x1666)))+(((r22)*(sj4))));
7467 evalcond[3]=((((IkReal(-1.00000000000000))*(x1669)*(x1671)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1666)))+(((sj2)*(x1676)))+(((IkReal(-1.00000000000000))*(x1666)*(x1667)))+(((IkReal(-1.00000000000000))*(sj3)*(x1672)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1668))));
7468 evalcond[4]=((((cj2)*(x1660)))+(((IkReal(-1.00000000000000))*(cj4)*(r12)*(x1668)))+(x1677)+(((IkReal(-1.00000000000000))*(r11)*(sj4)*(x1673)))+(((x1665)*(x1667)))+(((x1663)*(x1664)))+(((IkReal(-1.00000000000000))*(r01)*(x1664)*(x1666)))+(((IkReal(-1.00000000000000))*(r02)*(x1675))));
7469 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x1664)))+(((r11)*(sj0)*(x1670)))+(((cj3)*(x1672)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1668)))+(((IkReal(-1.00000000000000))*(x1662)*(x1674)))+(((IkReal(-1.00000000000000))*(cj4)*(x1667)*(x1671)))+(((x1669)*(x1670)))+(((IkReal(-1.00000000000000))*(x1663)*(x1675))));
7470 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
7477 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7478 vinfos[0].jointtype = 1;
7479 vinfos[0].foffset = j0;
7480 vinfos[0].indices[0] = _ij0[0];
7481 vinfos[0].indices[1] = _ij0[1];
7482 vinfos[0].maxsolutions = _nj0;
7483 vinfos[1].jointtype = 1;
7484 vinfos[1].foffset = j1;
7485 vinfos[1].indices[0] = _ij1[0];
7486 vinfos[1].indices[1] = _ij1[1];
7487 vinfos[1].maxsolutions = _nj1;
7488 vinfos[2].jointtype = 1;
7489 vinfos[2].foffset = j2;
7490 vinfos[2].indices[0] = _ij2[0];
7491 vinfos[2].indices[1] = _ij2[1];
7492 vinfos[2].maxsolutions = _nj2;
7493 vinfos[3].jointtype = 1;
7494 vinfos[3].foffset = j3;
7495 vinfos[3].indices[0] = _ij3[0];
7496 vinfos[3].indices[1] = _ij3[1];
7497 vinfos[3].maxsolutions = _nj3;
7498 vinfos[4].jointtype = 1;
7499 vinfos[4].foffset = j4;
7500 vinfos[4].indices[0] = _ij4[0];
7501 vinfos[4].indices[1] = _ij4[1];
7502 vinfos[4].maxsolutions = _nj4;
7503 vinfos[5].jointtype = 1;
7504 vinfos[5].foffset = j5;
7505 vinfos[5].indices[0] = _ij5[0];
7506 vinfos[5].indices[1] = _ij5[1];
7507 vinfos[5].maxsolutions = _nj5;
7508 std::vector<int> vfree(0);
7527 IkReal j1array[1], cj1array[1], sj1array[1];
7528 bool j1valid[1]={
false};
7530 IkReal x1678=((IkReal(1.00000000000000))*(sj4));
7531 IkReal x1679=((r22)*(sj2));
7532 IkReal x1680=((cj2)*(cj4));
7533 IkReal x1681=((r21)*(sj5));
7534 IkReal x1682=((cj4)*(sj2));
7535 IkReal x1683=((cj3)*(cj5)*(r20));
7536 IkReal x1684=((cj3)*(x1681));
7537 IkReal x1685=((IkReal(1.00000000000000))*(cj5)*(r20));
7538 if(
IKabs(((gconst87)*(((((IkReal(-1.00000000000000))*(cj2)*(r22)*(x1678)))+(((sj2)*(sj4)*(x1683)))+(((IkReal(-1.00000000000000))*(sj2)*(x1678)*(x1684)))+(((IkReal(-1.00000000000000))*(x1680)*(x1685)))+(((x1680)*(x1681)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x1679))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst87)*(((((x1681)*(x1682)))+(((cj2)*(sj4)*(x1684)))+(((IkReal(-1.00000000000000))*(x1682)*(x1685)))+(((IkReal(-1.00000000000000))*(cj2)*(x1678)*(x1683)))+(((cj3)*(r22)*(x1680)))+(((IkReal(-1.00000000000000))*(x1678)*(x1679))))))) <
IKFAST_ATAN2_MAGTHRESH )
7540 j1array[0]=
IKatan2(((gconst87)*(((((IkReal(-1.00000000000000))*(cj2)*(r22)*(x1678)))+(((sj2)*(sj4)*(x1683)))+(((IkReal(-1.00000000000000))*(sj2)*(x1678)*(x1684)))+(((IkReal(-1.00000000000000))*(x1680)*(x1685)))+(((x1680)*(x1681)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x1679)))))), ((gconst87)*(((((x1681)*(x1682)))+(((cj2)*(sj4)*(x1684)))+(((IkReal(-1.00000000000000))*(x1682)*(x1685)))+(((IkReal(-1.00000000000000))*(cj2)*(x1678)*(x1683)))+(((cj3)*(r22)*(x1680)))+(((IkReal(-1.00000000000000))*(x1678)*(x1679)))))));
7541 sj1array[0]=
IKsin(j1array[0]);
7542 cj1array[0]=
IKcos(j1array[0]);
7543 if( j1array[0] >
IKPI )
7547 else if( j1array[0] < -
IKPI )
7548 { j1array[0]+=
IK2PI;
7551 for(
int ij1 = 0; ij1 < 1; ++ij1)
7557 _ij1[0] = ij1; _ij1[1] = -1;
7558 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7562 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7565 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7568 IkReal x1686=
IKsin(j1);
7569 IkReal x1687=
IKcos(j1);
7570 IkReal x1688=((IkReal(1.00000000000000))*(cj2));
7571 IkReal x1689=((cj5)*(r20));
7572 IkReal x1690=((r21)*(sj5));
7573 IkReal x1691=((sj2)*(x1687));
7574 evalcond[0]=((((IkReal(-1.00000000000000))*(sj3)*(x1686)*(x1688)))+(((IkReal(-1.00000000000000))*(sj3)*(x1691)))+(((r20)*(sj5)))+(((cj5)*(r21))));
7575 evalcond[1]=((((sj4)*(x1690)))+(((cj4)*(r22)))+(((IkReal(-1.00000000000000))*(sj4)*(x1689)))+(((IkReal(-1.00000000000000))*(x1687)*(x1688)))+(((sj2)*(x1686))));
7576 evalcond[2]=((((IkReal(-1.00000000000000))*(cj4)*(x1690)))+(((cj2)*(cj3)*(x1686)))+(((r22)*(sj4)))+(((cj3)*(x1691)))+(((cj4)*(x1689))));
7577 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 )
7584 IkReal dummyeval[1];
7586 IkReal x1692=(cj5)*(cj5);
7587 IkReal x1693=(sj5)*(sj5);
7588 IkReal x1694=((IkReal(1.00000000000000))*(r10));
7589 IkReal x1695=((cj4)*(sj5));
7590 IkReal x1696=((r00)*(r11));
7591 IkReal x1697=((cj4)*(cj5));
7592 IkReal x1698=((sj4)*(x1692));
7593 IkReal x1699=((sj4)*(x1693));
7594 gconst98=
IKsign(((((IkReal(-1.00000000000000))*(r01)*(x1694)*(x1699)))+(((IkReal(-1.00000000000000))*(r02)*(x1694)*(x1695)))+(((x1696)*(x1698)))+(((IkReal(-1.00000000000000))*(r01)*(x1694)*(x1698)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1697)))+(((r01)*(r12)*(x1697)))+(((r00)*(r12)*(x1695)))+(((x1696)*(x1699)))));
7595 IkReal x1700=(cj5)*(cj5);
7596 IkReal x1701=(sj5)*(sj5);
7597 IkReal x1702=((IkReal(1.00000000000000))*(r10));
7598 IkReal x1703=((cj4)*(sj5));
7599 IkReal x1704=((r00)*(r11));
7600 IkReal x1705=((cj4)*(cj5));
7601 IkReal x1706=((sj4)*(x1700));
7602 IkReal x1707=((sj4)*(x1701));
7603 dummyeval[0]=((((x1704)*(x1706)))+(((r00)*(r12)*(x1703)))+(((IkReal(-1.00000000000000))*(r01)*(x1702)*(x1706)))+(((r01)*(r12)*(x1705)))+(((x1704)*(x1707)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1705)))+(((IkReal(-1.00000000000000))*(r02)*(x1702)*(x1703)))+(((IkReal(-1.00000000000000))*(r01)*(x1702)*(x1707))));
7604 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7607 IkReal dummyeval[1];
7609 IkReal x1708=(sj5)*(sj5);
7610 IkReal x1709=(cj5)*(cj5);
7611 IkReal x1710=((cj5)*(sj4));
7612 IkReal x1711=((IkReal(1.00000000000000))*(r02));
7613 IkReal x1712=((sj4)*(sj5));
7614 IkReal x1713=((cj4)*(r01)*(r10));
7615 IkReal x1714=((IkReal(1.00000000000000))*(cj4)*(r00)*(r11));
7616 gconst99=
IKsign(((((r01)*(r12)*(x1710)))+(((IkReal(-1.00000000000000))*(x1708)*(x1714)))+(((x1708)*(x1713)))+(((IkReal(-1.00000000000000))*(r10)*(x1711)*(x1712)))+(((IkReal(-1.00000000000000))*(x1709)*(x1714)))+(((x1709)*(x1713)))+(((IkReal(-1.00000000000000))*(r11)*(x1710)*(x1711)))+(((r00)*(r12)*(x1712)))));
7617 IkReal x1715=(sj5)*(sj5);
7618 IkReal x1716=(cj5)*(cj5);
7619 IkReal x1717=((cj5)*(sj4));
7620 IkReal x1718=((IkReal(1.00000000000000))*(r02));
7621 IkReal x1719=((sj4)*(sj5));
7622 IkReal x1720=((cj4)*(r01)*(r10));
7624 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1715)*(x1721)))+(((x1716)*(x1720)))+(((IkReal(-1.00000000000000))*(r10)*(x1718)*(x1719)))+(((r01)*(r12)*(x1717)))+(((x1715)*(x1720)))+(((IkReal(-1.00000000000000))*(x1716)*(x1721)))+(((r00)*(r12)*(x1719)))+(((IkReal(-1.00000000000000))*(r11)*(x1717)*(x1718))));
7625 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7632 IkReal j0array[1], cj0array[1], sj0array[1];
7633 bool j0valid[1]={
false};
7635 IkReal x1722=((IkReal(1.00000000000000))*(cj3));
7636 IkReal x1723=((cj4)*(cj5));
7637 IkReal x1724=((cj5)*(sj3));
7638 IkReal x1725=((sj3)*(sj5));
7639 IkReal x1726=((cj3)*(cj4)*(sj5));
7640 if(
IKabs(((gconst99)*(((((r11)*(x1724)))+(((r10)*(x1725)))+(((IkReal(-1.00000000000000))*(r10)*(x1722)*(x1723)))+(((r11)*(x1726)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1722))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst99)*(((((r00)*(x1725)))+(((r01)*(x1724)))+(((r01)*(x1726)))+(((IkReal(-1.00000000000000))*(r00)*(x1722)*(x1723)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1722))))))) <
IKFAST_ATAN2_MAGTHRESH )
7642 j0array[0]=
IKatan2(((gconst99)*(((((r11)*(x1724)))+(((r10)*(x1725)))+(((IkReal(-1.00000000000000))*(r10)*(x1722)*(x1723)))+(((r11)*(x1726)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1722)))))), ((gconst99)*(((((r00)*(x1725)))+(((r01)*(x1724)))+(((r01)*(x1726)))+(((IkReal(-1.00000000000000))*(r00)*(x1722)*(x1723)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1722)))))));
7643 sj0array[0]=
IKsin(j0array[0]);
7644 cj0array[0]=
IKcos(j0array[0]);
7645 if( j0array[0] >
IKPI )
7649 else if( j0array[0] < -
IKPI )
7650 { j0array[0]+=
IK2PI;
7653 for(
int ij0 = 0; ij0 < 1; ++ij0)
7659 _ij0[0] = ij0; _ij0[1] = -1;
7660 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
7664 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
7667 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
7670 IkReal x1727=
IKsin(j0);
7671 IkReal x1728=
IKcos(j0);
7672 IkReal x1729=((cj4)*(r12));
7673 IkReal x1730=((cj1)*(cj2));
7674 IkReal x1731=((IkReal(1.00000000000000))*(sj4));
7675 IkReal x1732=((IkReal(1.00000000000000))*(r10));
7676 IkReal x1733=((r01)*(sj5));
7677 IkReal x1734=((r02)*(sj4));
7678 IkReal x1735=((sj1)*(sj2));
7679 IkReal x1736=((cj5)*(r01));
7680 IkReal x1737=((cj5)*(r00));
7681 IkReal x1738=((r11)*(sj5));
7682 IkReal x1739=((sj5)*(x1727));
7683 IkReal x1740=((IkReal(1.00000000000000))*(x1728));
7684 IkReal x1741=((cj4)*(x1727));
7685 IkReal x1742=((cj5)*(x1727));
7686 IkReal x1743=((cj4)*(x1728));
7687 IkReal x1744=((cj5)*(sj4)*(x1728));
7688 evalcond[0]=((((x1727)*(x1736)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1740)))+(cj3)+(((r00)*(x1739)))+(((IkReal(-1.00000000000000))*(sj5)*(x1728)*(x1732))));
7689 evalcond[1]=((((IkReal(-1.00000000000000))*(x1736)*(x1740)))+(((IkReal(-1.00000000000000))*(r11)*(x1742)))+(((IkReal(-1.00000000000000))*(sj3)*(x1730)))+(((IkReal(-1.00000000000000))*(x1732)*(x1739)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1740)))+(((sj3)*(x1735))));
7690 evalcond[2]=((((IkReal(-1.00000000000000))*(x1728)*(x1731)*(x1738)))+(((sj4)*(x1727)*(x1733)))+(((IkReal(-1.00000000000000))*(x1729)*(x1740)))+(((r10)*(x1744)))+(((IkReal(-1.00000000000000))*(x1727)*(x1731)*(x1737)))+(((r02)*(x1741))));
7691 evalcond[3]=((((x1737)*(x1741)))+(sj3)+(((IkReal(-1.00000000000000))*(x1733)*(x1741)))+(((IkReal(-1.00000000000000))*(r12)*(x1728)*(x1731)))+(((x1727)*(x1734)))+(((x1738)*(x1743)))+(((IkReal(-1.00000000000000))*(cj5)*(x1732)*(x1743))));
7692 evalcond[4]=((((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1728)*(x1731)*(x1733)))+(((sj4)*(x1728)*(x1737)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1740)))+(((IkReal(-1.00000000000000))*(x1727)*(x1731)*(x1738)))+(((r10)*(sj4)*(x1742)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x1727)*(x1729))));
7693 evalcond[5]=((((IkReal(-1.00000000000000))*(r12)*(x1727)*(x1731)))+(((IkReal(-1.00000000000000))*(r02)*(x1728)*(x1731)))+(((IkReal(-1.00000000000000))*(cj4)*(x1737)*(x1740)))+(((x1738)*(x1741)))+(((IkReal(-1.00000000000000))*(cj3)*(x1735)))+(((IkReal(-1.00000000000000))*(cj5)*(x1732)*(x1741)))+(((cj3)*(x1730)))+(((x1733)*(x1743))));
7694 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
7701 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7702 vinfos[0].jointtype = 1;
7703 vinfos[0].foffset = j0;
7704 vinfos[0].indices[0] = _ij0[0];
7705 vinfos[0].indices[1] = _ij0[1];
7706 vinfos[0].maxsolutions = _nj0;
7707 vinfos[1].jointtype = 1;
7708 vinfos[1].foffset = j1;
7709 vinfos[1].indices[0] = _ij1[0];
7710 vinfos[1].indices[1] = _ij1[1];
7711 vinfos[1].maxsolutions = _nj1;
7712 vinfos[2].jointtype = 1;
7713 vinfos[2].foffset = j2;
7714 vinfos[2].indices[0] = _ij2[0];
7715 vinfos[2].indices[1] = _ij2[1];
7716 vinfos[2].maxsolutions = _nj2;
7717 vinfos[3].jointtype = 1;
7718 vinfos[3].foffset = j3;
7719 vinfos[3].indices[0] = _ij3[0];
7720 vinfos[3].indices[1] = _ij3[1];
7721 vinfos[3].maxsolutions = _nj3;
7722 vinfos[4].jointtype = 1;
7723 vinfos[4].foffset = j4;
7724 vinfos[4].indices[0] = _ij4[0];
7725 vinfos[4].indices[1] = _ij4[1];
7726 vinfos[4].maxsolutions = _nj4;
7727 vinfos[5].jointtype = 1;
7728 vinfos[5].foffset = j5;
7729 vinfos[5].indices[0] = _ij5[0];
7730 vinfos[5].indices[1] = _ij5[1];
7731 vinfos[5].maxsolutions = _nj5;
7732 std::vector<int> vfree(0);
7745 IkReal j0array[1], cj0array[1], sj0array[1];
7746 bool j0valid[1]={
false};
7748 IkReal x1745=((cj3)*(sj4));
7749 IkReal x1746=((IkReal(1.00000000000000))*(sj5));
7750 IkReal x1747=((IkReal(1.00000000000000))*(cj3)*(cj4));
7751 if(
IKabs(((gconst98)*(((((IkReal(-1.00000000000000))*(r12)*(x1747)))+(((cj5)*(r10)*(x1745)))+(((IkReal(-1.00000000000000))*(r11)*(x1745)*(x1746))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst98)*(((((IkReal(-1.00000000000000))*(r01)*(x1745)*(x1746)))+(((cj5)*(r00)*(x1745)))+(((IkReal(-1.00000000000000))*(r02)*(x1747))))))) <
IKFAST_ATAN2_MAGTHRESH )
7753 j0array[0]=
IKatan2(((gconst98)*(((((IkReal(-1.00000000000000))*(r12)*(x1747)))+(((cj5)*(r10)*(x1745)))+(((IkReal(-1.00000000000000))*(r11)*(x1745)*(x1746)))))), ((gconst98)*(((((IkReal(-1.00000000000000))*(r01)*(x1745)*(x1746)))+(((cj5)*(r00)*(x1745)))+(((IkReal(-1.00000000000000))*(r02)*(x1747)))))));
7754 sj0array[0]=
IKsin(j0array[0]);
7755 cj0array[0]=
IKcos(j0array[0]);
7756 if( j0array[0] >
IKPI )
7760 else if( j0array[0] < -
IKPI )
7761 { j0array[0]+=
IK2PI;
7764 for(
int ij0 = 0; ij0 < 1; ++ij0)
7770 _ij0[0] = ij0; _ij0[1] = -1;
7771 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
7775 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
7778 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
7781 IkReal x1748=
IKsin(j0);
7782 IkReal x1749=
IKcos(j0);
7783 IkReal x1750=((cj4)*(r12));
7784 IkReal x1751=((cj1)*(cj2));
7785 IkReal x1752=((IkReal(1.00000000000000))*(sj4));
7786 IkReal x1753=((IkReal(1.00000000000000))*(r10));
7787 IkReal x1754=((r01)*(sj5));
7788 IkReal x1755=((r02)*(sj4));
7789 IkReal x1756=((sj1)*(sj2));
7790 IkReal x1757=((cj5)*(r01));
7791 IkReal x1758=((cj5)*(r00));
7792 IkReal x1759=((r11)*(sj5));
7793 IkReal x1760=((sj5)*(x1748));
7794 IkReal x1761=((IkReal(1.00000000000000))*(x1749));
7795 IkReal x1762=((cj4)*(x1748));
7796 IkReal x1763=((cj5)*(x1748));
7797 IkReal x1764=((cj4)*(x1749));
7798 IkReal x1765=((cj5)*(sj4)*(x1749));
7799 evalcond[0]=((((x1748)*(x1757)))+(((r00)*(x1760)))+(cj3)+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1761)))+(((IkReal(-1.00000000000000))*(sj5)*(x1749)*(x1753))));
7800 evalcond[1]=((((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1761)))+(((sj3)*(x1756)))+(((IkReal(-1.00000000000000))*(r11)*(x1763)))+(((IkReal(-1.00000000000000))*(sj3)*(x1751)))+(((IkReal(-1.00000000000000))*(x1753)*(x1760)))+(((IkReal(-1.00000000000000))*(x1757)*(x1761))));
7801 evalcond[2]=((((IkReal(-1.00000000000000))*(x1748)*(x1752)*(x1758)))+(((IkReal(-1.00000000000000))*(x1749)*(x1752)*(x1759)))+(((sj4)*(x1748)*(x1754)))+(((r10)*(x1765)))+(((IkReal(-1.00000000000000))*(x1750)*(x1761)))+(((r02)*(x1762))));
7802 evalcond[3]=((((x1759)*(x1764)))+(((x1758)*(x1762)))+(((x1748)*(x1755)))+(sj3)+(((IkReal(-1.00000000000000))*(r12)*(x1749)*(x1752)))+(((IkReal(-1.00000000000000))*(x1754)*(x1762)))+(((IkReal(-1.00000000000000))*(cj5)*(x1753)*(x1764))));
7803 evalcond[4]=((((IkReal(-1.00000000000000))*(x1748)*(x1750)))+(((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1748)*(x1752)*(x1759)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1761)))+(((sj4)*(x1749)*(x1758)))+(((r10)*(sj4)*(x1763)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x1749)*(x1752)*(x1754))));
7804 evalcond[5]=((((x1754)*(x1764)))+(((IkReal(-1.00000000000000))*(r02)*(x1749)*(x1752)))+(((x1759)*(x1762)))+(((IkReal(-1.00000000000000))*(r12)*(x1748)*(x1752)))+(((IkReal(-1.00000000000000))*(cj5)*(x1753)*(x1762)))+(((IkReal(-1.00000000000000))*(cj4)*(x1758)*(x1761)))+(((cj3)*(x1751)))+(((IkReal(-1.00000000000000))*(cj3)*(x1756))));
7805 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
7812 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7813 vinfos[0].jointtype = 1;
7814 vinfos[0].foffset = j0;
7815 vinfos[0].indices[0] = _ij0[0];
7816 vinfos[0].indices[1] = _ij0[1];
7817 vinfos[0].maxsolutions = _nj0;
7818 vinfos[1].jointtype = 1;
7819 vinfos[1].foffset = j1;
7820 vinfos[1].indices[0] = _ij1[0];
7821 vinfos[1].indices[1] = _ij1[1];
7822 vinfos[1].maxsolutions = _nj1;
7823 vinfos[2].jointtype = 1;
7824 vinfos[2].foffset = j2;
7825 vinfos[2].indices[0] = _ij2[0];
7826 vinfos[2].indices[1] = _ij2[1];
7827 vinfos[2].maxsolutions = _nj2;
7828 vinfos[3].jointtype = 1;
7829 vinfos[3].foffset = j3;
7830 vinfos[3].indices[0] = _ij3[0];
7831 vinfos[3].indices[1] = _ij3[1];
7832 vinfos[3].maxsolutions = _nj3;
7833 vinfos[4].jointtype = 1;
7834 vinfos[4].foffset = j4;
7835 vinfos[4].indices[0] = _ij4[0];
7836 vinfos[4].indices[1] = _ij4[1];
7837 vinfos[4].maxsolutions = _nj4;
7838 vinfos[5].jointtype = 1;
7839 vinfos[5].foffset = j5;
7840 vinfos[5].indices[0] = _ij5[0];
7841 vinfos[5].indices[1] = _ij5[1];
7842 vinfos[5].maxsolutions = _nj5;
7843 std::vector<int> vfree(0);
7862 IkReal j1array[1], cj1array[1], sj1array[1];
7863 bool j1valid[1]={
false};
7865 IkReal x1766=((cj2)*(sj5));
7866 IkReal x1767=((cj5)*(r21));
7867 IkReal x1768=((cj2)*(sj3));
7868 IkReal x1769=((cj4)*(r22));
7869 IkReal x1770=((sj2)*(sj5));
7870 IkReal x1771=((sj2)*(sj3));
7871 IkReal x1772=((r21)*(sj3)*(sj4));
7872 IkReal x1773=((cj5)*(r20)*(sj4));
7873 if(
IKabs(((gconst86)*(((((x1771)*(x1773)))+(((IkReal(-1.00000000000000))*(x1769)*(x1771)))+(((cj2)*(x1767)))+(((r20)*(x1766)))+(((IkReal(-1.00000000000000))*(x1770)*(x1772))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst86)*(((((r20)*(x1770)))+(((IkReal(-1.00000000000000))*(x1768)*(x1773)))+(((x1768)*(x1769)))+(((sj2)*(x1767)))+(((x1766)*(x1772))))))) <
IKFAST_ATAN2_MAGTHRESH )
7875 j1array[0]=
IKatan2(((gconst86)*(((((x1771)*(x1773)))+(((IkReal(-1.00000000000000))*(x1769)*(x1771)))+(((cj2)*(x1767)))+(((r20)*(x1766)))+(((IkReal(-1.00000000000000))*(x1770)*(x1772)))))), ((gconst86)*(((((r20)*(x1770)))+(((IkReal(-1.00000000000000))*(x1768)*(x1773)))+(((x1768)*(x1769)))+(((sj2)*(x1767)))+(((x1766)*(x1772)))))));
7876 sj1array[0]=
IKsin(j1array[0]);
7877 cj1array[0]=
IKcos(j1array[0]);
7878 if( j1array[0] >
IKPI )
7882 else if( j1array[0] < -
IKPI )
7883 { j1array[0]+=
IK2PI;
7886 for(
int ij1 = 0; ij1 < 1; ++ij1)
7892 _ij1[0] = ij1; _ij1[1] = -1;
7893 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
7897 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
7900 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
7903 IkReal x1774=
IKsin(j1);
7904 IkReal x1775=
IKcos(j1);
7905 IkReal x1776=((IkReal(1.00000000000000))*(cj2));
7906 IkReal x1777=((cj5)*(r20));
7907 IkReal x1778=((r21)*(sj5));
7908 IkReal x1779=((sj2)*(x1775));
7909 evalcond[0]=((((IkReal(-1.00000000000000))*(sj3)*(x1779)))+(((r20)*(sj5)))+(((IkReal(-1.00000000000000))*(sj3)*(x1774)*(x1776)))+(((cj5)*(r21))));
7910 evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(x1777)))+(((IkReal(-1.00000000000000))*(x1775)*(x1776)))+(((sj2)*(x1774)))+(((cj4)*(r22)))+(((sj4)*(x1778))));
7911 evalcond[2]=((((r22)*(sj4)))+(((cj2)*(cj3)*(x1774)))+(((cj3)*(x1779)))+(((cj4)*(x1777)))+(((IkReal(-1.00000000000000))*(cj4)*(x1778))));
7912 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 )
7919 IkReal dummyeval[1];
7921 IkReal x1780=(cj5)*(cj5);
7922 IkReal x1781=(sj5)*(sj5);
7923 IkReal x1782=((IkReal(1.00000000000000))*(r10));
7924 IkReal x1783=((cj4)*(sj5));
7925 IkReal x1784=((r00)*(r11));
7926 IkReal x1785=((cj4)*(cj5));
7927 IkReal x1786=((sj4)*(x1780));
7928 IkReal x1787=((sj4)*(x1781));
7929 gconst98=
IKsign(((((x1784)*(x1786)))+(((r00)*(r12)*(x1783)))+(((IkReal(-1.00000000000000))*(r02)*(x1782)*(x1783)))+(((IkReal(-1.00000000000000))*(r01)*(x1782)*(x1786)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1785)))+(((IkReal(-1.00000000000000))*(r01)*(x1782)*(x1787)))+(((r01)*(r12)*(x1785)))+(((x1784)*(x1787)))));
7930 IkReal x1788=(cj5)*(cj5);
7931 IkReal x1789=(sj5)*(sj5);
7932 IkReal x1790=((IkReal(1.00000000000000))*(r10));
7933 IkReal x1791=((cj4)*(sj5));
7934 IkReal x1792=((r00)*(r11));
7935 IkReal x1793=((cj4)*(cj5));
7936 IkReal x1794=((sj4)*(x1788));
7937 IkReal x1795=((sj4)*(x1789));
7938 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(x1790)*(x1791)))+(((x1792)*(x1794)))+(((IkReal(-1.00000000000000))*(r01)*(x1790)*(x1794)))+(((IkReal(-1.00000000000000))*(r01)*(x1790)*(x1795)))+(((r01)*(r12)*(x1793)))+(((x1792)*(x1795)))+(((IkReal(-1.00000000000000))*(r02)*(r11)*(x1793)))+(((r00)*(r12)*(x1791))));
7939 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7942 IkReal dummyeval[1];
7944 IkReal x1796=(sj5)*(sj5);
7945 IkReal x1797=(cj5)*(cj5);
7946 IkReal x1798=((cj5)*(sj4));
7947 IkReal x1799=((IkReal(1.00000000000000))*(r02));
7948 IkReal x1800=((sj4)*(sj5));
7949 IkReal x1801=((cj4)*(r01)*(r10));
7950 IkReal x1802=((IkReal(1.00000000000000))*(cj4)*(r00)*(r11));
7951 gconst99=
IKsign(((((r00)*(r12)*(x1800)))+(((IkReal(-1.00000000000000))*(r11)*(x1798)*(x1799)))+(((x1796)*(x1801)))+(((IkReal(-1.00000000000000))*(x1797)*(x1802)))+(((x1797)*(x1801)))+(((IkReal(-1.00000000000000))*(r10)*(x1799)*(x1800)))+(((IkReal(-1.00000000000000))*(x1796)*(x1802)))+(((r01)*(r12)*(x1798)))));
7952 IkReal x1803=(sj5)*(sj5);
7953 IkReal x1804=(cj5)*(cj5);
7954 IkReal x1805=((cj5)*(sj4));
7955 IkReal x1806=((IkReal(1.00000000000000))*(r02));
7956 IkReal x1807=((sj4)*(sj5));
7957 IkReal x1808=((cj4)*(r01)*(r10));
7959 dummyeval[0]=((((x1803)*(x1808)))+(((r00)*(r12)*(x1807)))+(((IkReal(-1.00000000000000))*(r10)*(x1806)*(x1807)))+(((IkReal(-1.00000000000000))*(x1803)*(x1809)))+(((x1804)*(x1808)))+(((r01)*(r12)*(x1805)))+(((IkReal(-1.00000000000000))*(r11)*(x1805)*(x1806)))+(((IkReal(-1.00000000000000))*(x1804)*(x1809))));
7960 if(
IKabs(dummyeval[0]) < 0.0000010000000000 )
7967 IkReal j0array[1], cj0array[1], sj0array[1];
7968 bool j0valid[1]={
false};
7970 IkReal x1810=((IkReal(1.00000000000000))*(cj3));
7971 IkReal x1811=((cj4)*(cj5));
7972 IkReal x1812=((cj5)*(sj3));
7973 IkReal x1813=((sj3)*(sj5));
7974 IkReal x1814=((cj3)*(cj4)*(sj5));
7975 if(
IKabs(((gconst99)*(((((r11)*(x1812)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1810)))+(((r11)*(x1814)))+(((r10)*(x1813)))+(((IkReal(-1.00000000000000))*(r10)*(x1810)*(x1811))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst99)*(((((r01)*(x1814)))+(((IkReal(-1.00000000000000))*(r00)*(x1810)*(x1811)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1810)))+(((r00)*(x1813)))+(((r01)*(x1812))))))) <
IKFAST_ATAN2_MAGTHRESH )
7977 j0array[0]=
IKatan2(((gconst99)*(((((r11)*(x1812)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x1810)))+(((r11)*(x1814)))+(((r10)*(x1813)))+(((IkReal(-1.00000000000000))*(r10)*(x1810)*(x1811)))))), ((gconst99)*(((((r01)*(x1814)))+(((IkReal(-1.00000000000000))*(r00)*(x1810)*(x1811)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x1810)))+(((r00)*(x1813)))+(((r01)*(x1812)))))));
7978 sj0array[0]=
IKsin(j0array[0]);
7979 cj0array[0]=
IKcos(j0array[0]);
7980 if( j0array[0] >
IKPI )
7984 else if( j0array[0] < -
IKPI )
7985 { j0array[0]+=
IK2PI;
7988 for(
int ij0 = 0; ij0 < 1; ++ij0)
7994 _ij0[0] = ij0; _ij0[1] = -1;
7995 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
7999 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
8002 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
8005 IkReal x1815=
IKsin(j0);
8006 IkReal x1816=
IKcos(j0);
8007 IkReal x1817=((cj4)*(r12));
8008 IkReal x1818=((cj1)*(cj2));
8009 IkReal x1819=((IkReal(1.00000000000000))*(sj4));
8010 IkReal x1820=((IkReal(1.00000000000000))*(r10));
8011 IkReal x1821=((r01)*(sj5));
8012 IkReal x1822=((r02)*(sj4));
8013 IkReal x1823=((sj1)*(sj2));
8014 IkReal x1824=((cj5)*(r01));
8015 IkReal x1825=((cj5)*(r00));
8016 IkReal x1826=((r11)*(sj5));
8017 IkReal x1827=((sj5)*(x1815));
8018 IkReal x1828=((IkReal(1.00000000000000))*(x1816));
8019 IkReal x1829=((cj4)*(x1815));
8020 IkReal x1830=((cj5)*(x1815));
8021 IkReal x1831=((cj4)*(x1816));
8022 IkReal x1832=((cj5)*(sj4)*(x1816));
8023 evalcond[0]=((((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1828)))+(cj3)+(((x1815)*(x1824)))+(((IkReal(-1.00000000000000))*(sj5)*(x1816)*(x1820)))+(((r00)*(x1827))));
8024 evalcond[1]=((((IkReal(-1.00000000000000))*(x1824)*(x1828)))+(((IkReal(-1.00000000000000))*(r11)*(x1830)))+(((sj3)*(x1823)))+(((IkReal(-1.00000000000000))*(sj3)*(x1818)))+(((IkReal(-1.00000000000000))*(x1820)*(x1827)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1828))));
8025 evalcond[2]=((((sj4)*(x1815)*(x1821)))+(((IkReal(-1.00000000000000))*(x1817)*(x1828)))+(((r10)*(x1832)))+(((IkReal(-1.00000000000000))*(x1816)*(x1819)*(x1826)))+(((r02)*(x1829)))+(((IkReal(-1.00000000000000))*(x1815)*(x1819)*(x1825))));
8026 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(x1816)*(x1819)))+(sj3)+(((x1815)*(x1822)))+(((IkReal(-1.00000000000000))*(cj5)*(x1820)*(x1831)))+(((IkReal(-1.00000000000000))*(x1821)*(x1829)))+(((x1826)*(x1831)))+(((x1825)*(x1829))));
8027 evalcond[4]=((((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1828)))+(((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(x1815)*(x1819)*(x1826)))+(((r10)*(sj4)*(x1830)))+(((IkReal(-1.00000000000000))*(x1816)*(x1819)*(x1821)))+(((sj4)*(x1816)*(x1825)))+(((IkReal(-1.00000000000000))*(x1815)*(x1817)))+(((cj2)*(sj1))));
8028 evalcond[5]=((((cj3)*(x1818)))+(((IkReal(-1.00000000000000))*(cj4)*(x1825)*(x1828)))+(((x1826)*(x1829)))+(((IkReal(-1.00000000000000))*(r12)*(x1815)*(x1819)))+(((x1821)*(x1831)))+(((IkReal(-1.00000000000000))*(cj3)*(x1823)))+(((IkReal(-1.00000000000000))*(r02)*(x1816)*(x1819)))+(((IkReal(-1.00000000000000))*(cj5)*(x1820)*(x1829))));
8029 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
8036 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8037 vinfos[0].jointtype = 1;
8038 vinfos[0].foffset = j0;
8039 vinfos[0].indices[0] = _ij0[0];
8040 vinfos[0].indices[1] = _ij0[1];
8041 vinfos[0].maxsolutions = _nj0;
8042 vinfos[1].jointtype = 1;
8043 vinfos[1].foffset = j1;
8044 vinfos[1].indices[0] = _ij1[0];
8045 vinfos[1].indices[1] = _ij1[1];
8046 vinfos[1].maxsolutions = _nj1;
8047 vinfos[2].jointtype = 1;
8048 vinfos[2].foffset = j2;
8049 vinfos[2].indices[0] = _ij2[0];
8050 vinfos[2].indices[1] = _ij2[1];
8051 vinfos[2].maxsolutions = _nj2;
8052 vinfos[3].jointtype = 1;
8053 vinfos[3].foffset = j3;
8054 vinfos[3].indices[0] = _ij3[0];
8055 vinfos[3].indices[1] = _ij3[1];
8056 vinfos[3].maxsolutions = _nj3;
8057 vinfos[4].jointtype = 1;
8058 vinfos[4].foffset = j4;
8059 vinfos[4].indices[0] = _ij4[0];
8060 vinfos[4].indices[1] = _ij4[1];
8061 vinfos[4].maxsolutions = _nj4;
8062 vinfos[5].jointtype = 1;
8063 vinfos[5].foffset = j5;
8064 vinfos[5].indices[0] = _ij5[0];
8065 vinfos[5].indices[1] = _ij5[1];
8066 vinfos[5].maxsolutions = _nj5;
8067 std::vector<int> vfree(0);
8080 IkReal j0array[1], cj0array[1], sj0array[1];
8081 bool j0valid[1]={
false};
8083 IkReal x1833=((cj3)*(sj4));
8084 IkReal x1834=((IkReal(1.00000000000000))*(sj5));
8085 IkReal x1835=((IkReal(1.00000000000000))*(cj3)*(cj4));
8086 if(
IKabs(((gconst98)*(((((IkReal(-1.00000000000000))*(r12)*(x1835)))+(((IkReal(-1.00000000000000))*(r11)*(x1833)*(x1834)))+(((cj5)*(r10)*(x1833))))))) <
IKFAST_ATAN2_MAGTHRESH &&
IKabs(((gconst98)*(((((IkReal(-1.00000000000000))*(r02)*(x1835)))+(((IkReal(-1.00000000000000))*(r01)*(x1833)*(x1834)))+(((cj5)*(r00)*(x1833))))))) <
IKFAST_ATAN2_MAGTHRESH )
8088 j0array[0]=
IKatan2(((gconst98)*(((((IkReal(-1.00000000000000))*(r12)*(x1835)))+(((IkReal(-1.00000000000000))*(r11)*(x1833)*(x1834)))+(((cj5)*(r10)*(x1833)))))), ((gconst98)*(((((IkReal(-1.00000000000000))*(r02)*(x1835)))+(((IkReal(-1.00000000000000))*(r01)*(x1833)*(x1834)))+(((cj5)*(r00)*(x1833)))))));
8089 sj0array[0]=
IKsin(j0array[0]);
8090 cj0array[0]=
IKcos(j0array[0]);
8091 if( j0array[0] >
IKPI )
8095 else if( j0array[0] < -
IKPI )
8096 { j0array[0]+=
IK2PI;
8099 for(
int ij0 = 0; ij0 < 1; ++ij0)
8105 _ij0[0] = ij0; _ij0[1] = -1;
8106 for(
int iij0 = ij0+1; iij0 < 1; ++iij0)
8110 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
8113 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
8116 IkReal x1836=
IKsin(j0);
8117 IkReal x1837=
IKcos(j0);
8118 IkReal x1838=((cj4)*(r12));
8119 IkReal x1839=((cj1)*(cj2));
8120 IkReal x1840=((IkReal(1.00000000000000))*(sj4));
8121 IkReal x1841=((IkReal(1.00000000000000))*(r10));
8122 IkReal x1842=((r01)*(sj5));
8123 IkReal x1843=((r02)*(sj4));
8124 IkReal x1844=((sj1)*(sj2));
8125 IkReal x1845=((cj5)*(r01));
8126 IkReal x1846=((cj5)*(r00));
8127 IkReal x1847=((r11)*(sj5));
8128 IkReal x1848=((sj5)*(x1836));
8129 IkReal x1849=((IkReal(1.00000000000000))*(x1837));
8130 IkReal x1850=((cj4)*(x1836));
8131 IkReal x1851=((cj5)*(x1836));
8132 IkReal x1852=((cj4)*(x1837));
8133 IkReal x1853=((cj5)*(sj4)*(x1837));
8134 evalcond[0]=((((r00)*(x1848)))+(((IkReal(-1.00000000000000))*(cj5)*(r11)*(x1849)))+(((IkReal(-1.00000000000000))*(sj5)*(x1837)*(x1841)))+(cj3)+(((x1836)*(x1845))));
8135 evalcond[1]=((((IkReal(-1.00000000000000))*(x1845)*(x1849)))+(((IkReal(-1.00000000000000))*(r00)*(sj5)*(x1849)))+(((sj3)*(x1844)))+(((IkReal(-1.00000000000000))*(sj3)*(x1839)))+(((IkReal(-1.00000000000000))*(r11)*(x1851)))+(((IkReal(-1.00000000000000))*(x1841)*(x1848))));
8136 evalcond[2]=((((IkReal(-1.00000000000000))*(x1837)*(x1840)*(x1847)))+(((IkReal(-1.00000000000000))*(x1838)*(x1849)))+(((r02)*(x1850)))+(((sj4)*(x1836)*(x1842)))+(((IkReal(-1.00000000000000))*(x1836)*(x1840)*(x1846)))+(((r10)*(x1853))));
8137 evalcond[3]=((((x1846)*(x1850)))+(((IkReal(-1.00000000000000))*(x1842)*(x1850)))+(((IkReal(-1.00000000000000))*(r12)*(x1837)*(x1840)))+(sj3)+(((x1847)*(x1852)))+(((x1836)*(x1843)))+(((IkReal(-1.00000000000000))*(cj5)*(x1841)*(x1852))));
8138 evalcond[4]=((((r10)*(sj4)*(x1851)))+(((cj1)*(sj2)))+(((IkReal(-1.00000000000000))*(cj4)*(r02)*(x1849)))+(((sj4)*(x1837)*(x1846)))+(((IkReal(-1.00000000000000))*(x1837)*(x1840)*(x1842)))+(((cj2)*(sj1)))+(((IkReal(-1.00000000000000))*(x1836)*(x1838)))+(((IkReal(-1.00000000000000))*(x1836)*(x1840)*(x1847))));
8139 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x1846)*(x1849)))+(((IkReal(-1.00000000000000))*(r02)*(x1837)*(x1840)))+(((IkReal(-1.00000000000000))*(r12)*(x1836)*(x1840)))+(((IkReal(-1.00000000000000))*(cj3)*(x1844)))+(((cj3)*(x1839)))+(((x1847)*(x1850)))+(((IkReal(-1.00000000000000))*(cj5)*(x1841)*(x1850)))+(((x1842)*(x1852))));
8140 if(
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 )
8147 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8148 vinfos[0].jointtype = 1;
8149 vinfos[0].foffset = j0;
8150 vinfos[0].indices[0] = _ij0[0];
8151 vinfos[0].indices[1] = _ij0[1];
8152 vinfos[0].maxsolutions = _nj0;
8153 vinfos[1].jointtype = 1;
8154 vinfos[1].foffset = j1;
8155 vinfos[1].indices[0] = _ij1[0];
8156 vinfos[1].indices[1] = _ij1[1];
8157 vinfos[1].maxsolutions = _nj1;
8158 vinfos[2].jointtype = 1;
8159 vinfos[2].foffset = j2;
8160 vinfos[2].indices[0] = _ij2[0];
8161 vinfos[2].indices[1] = _ij2[1];
8162 vinfos[2].maxsolutions = _nj2;
8163 vinfos[3].jointtype = 1;
8164 vinfos[3].foffset = j3;
8165 vinfos[3].indices[0] = _ij3[0];
8166 vinfos[3].indices[1] = _ij3[1];
8167 vinfos[3].maxsolutions = _nj3;
8168 vinfos[4].jointtype = 1;
8169 vinfos[4].foffset = j4;
8170 vinfos[4].indices[0] = _ij4[0];
8171 vinfos[4].indices[1] = _ij4[1];
8172 vinfos[4].maxsolutions = _nj4;
8173 vinfos[5].jointtype = 1;
8174 vinfos[5].foffset = j5;
8175 vinfos[5].indices[0] = _ij5[0];
8176 vinfos[5].indices[1] = _ij5[1];
8177 vinfos[5].maxsolutions = _nj5;
8178 std::vector<int> vfree(0);
8211 static inline void polyroots8(IkReal rawcoeffs[8+1], IkReal rawroots[8],
int& numroots)
8215 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
8216 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
8217 complex<IkReal> coeffs[8];
8218 const int maxsteps = 110;
8219 for(
int i = 0; i < 8; ++i) {
8220 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
8222 complex<IkReal> roots[8];
8224 roots[0] = complex<IkReal>(1,0);
8225 roots[1] = complex<IkReal>(0.4,0.9);
8228 for(
int i = 2; i < 8; ++i) {
8229 roots[i] = roots[i-1]*roots[1];
8233 bool changed =
false;
8234 for(
int i = 0; i < 8; ++i) {
8235 if ( err[i] >= tol ) {
8238 complex<IkReal>
x = roots[i] + coeffs[0];
8239 for(
int j = 1; j < 8; ++j) {
8240 x = roots[i] * x + coeffs[j];
8242 for(
int j = 0; j < 8; ++j) {
8244 if( roots[i] != roots[j] ) {
8245 x /= (roots[i] - roots[j]);
8259 bool visited[8] = {
false};
8260 for(
int i = 0; i < 8; ++i) {
8264 complex<IkReal> newroot=roots[i];
8266 for(
int j = i+1; j < 8; ++j) {
8267 if(
abs(roots[i]-roots[j]) < 8*tolsqrt ) {
8268 newroot += roots[j];
8277 if(
IKabs(imag(newroot)) < tolsqrt ) {
8278 rawroots[numroots++] = real(newroot);
8290 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
8293 IKFAST_API
const char*
GetKinematicsHash() {
return "<robot:genericrobot - fanuc_m430ia2p (f1cfe1db779d431226bee8068f3d5170)>"; }
8297 #ifdef IKFAST_NAMESPACE 8301 #ifndef IKFAST_NO_MAIN 8304 #ifdef IKFAST_NAMESPACE 8305 using namespace IKFAST_NAMESPACE;
8310 printf(
"\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" 8311 "Returns the ik solutions given the transformation of the end effector specified by\n" 8312 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" 8319 IkReal eerot[9],eetrans[3];
8320 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
8321 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
8322 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
8323 for(std::size_t i = 0; i < vfree.size(); ++i)
8324 vfree[i] = atof(argv[13+i]);
8325 bool bSuccess =
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
8328 fprintf(stderr,
"Failed to get ik solution\n");
8336 printf(
"sol%d (free=%d): ", (
int)i, (
int)sol.
GetFree().size());
8337 std::vector<IkReal> vsolfree(sol.
GetFree().size());
8338 sol.
GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
8339 for( std::size_t j = 0; j < solvalues.size(); ++j)
8340 printf(
"%.15f, ", solvalues[j]);
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)
IKFAST_API const char * GetKinematicsHash()
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
#define IKFAST_STRINGIZE(s)
float IKfmod(float x, float y)
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)
IKFAST_API int GetIkType()
int main(int argc, char **argv)
#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
The discrete solutions are returned in this structure.
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
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 dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
IKFAST_API int GetIkRealSize()
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
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
IKFAST_API const char * GetIkFastVersion()
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
#define IKFAST_SINCOS_THRESH
IKFAST_API int GetNumJoints()
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
#define IKFAST_COMPILE_ASSERT(x)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKFAST_API int GetNumFreeParameters()
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
static void polyroots8(IkReal rawcoeffs[8+1], IkReal rawroots[8], int &numroots)
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
#define IKFAST_ATAN2_MAGTHRESH
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
float IKatan2(float fy, float fx)
Default implementation of IkSolutionListBase.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
manages all the solutions
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
IKFAST_API int * GetFreeParameters()
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored