choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
22 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
23 using namespace ikfast;
24 
25 // check if the included ikfast version matches what this file was compiled with
26 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
27 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a);
28 
29 #include <cmath>
30 #include <vector>
31 #include <limits>
32 #include <algorithm>
33 #include <complex>
34 
35 #ifndef IKFAST_ASSERT
36 #include <stdexcept>
37 #include <sstream>
38 #include <iostream>
39 
40 #ifdef _MSC_VER
41 #ifndef __PRETTY_FUNCTION__
42 #define __PRETTY_FUNCTION__ __FUNCDNAME__
43 #endif
44 #endif
45 
46 #ifndef __PRETTY_FUNCTION__
47 #define __PRETTY_FUNCTION__ __func__
48 #endif
49 
50 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
51 
52 #endif
53 
54 #if defined(_MSC_VER)
55 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
56 #else
57 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
58 #endif
59 
60 #define IK2PI ((IkReal)6.28318530717959)
61 #define IKPI ((IkReal)3.14159265358979)
62 #define IKPI_2 ((IkReal)1.57079632679490)
63 
64 #ifdef _MSC_VER
65 #ifndef isnan
66 #define isnan _isnan
67 #endif
68 #ifndef isinf
69 #define isinf _isinf
70 #endif
71 //#ifndef isfinite
72 //#define isfinite _isfinite
73 //#endif
74 #endif // _MSC_VER
75 
76 // lapack routines
77 extern "C" {
78  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
79  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
80  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
81  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
82  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
83  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
84 }
85 
86 using namespace std; // necessary to get std math routines
87 
88 #ifdef IKFAST_NAMESPACE
89 namespace IKFAST_NAMESPACE {
90 #endif
91 
92 inline float IKabs(float f) { return fabsf(f); }
93 inline double IKabs(double f) { return fabs(f); }
94 
95 inline float IKsqr(float f) { return f*f; }
96 inline double IKsqr(double f) { return f*f; }
97 
98 inline float IKlog(float f) { return logf(f); }
99 inline double IKlog(double f) { return log(f); }
100 
101 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
102 #ifndef IKFAST_SINCOS_THRESH
103 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
104 #endif
105 
106 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
107 #ifndef IKFAST_ATAN2_MAGTHRESH
108 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
109 #endif
110 
111 // minimum distance of separate solutions
112 #ifndef IKFAST_SOLUTION_THRESH
113 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
114 #endif
115 
116 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
117 #ifndef IKFAST_EVALCOND_THRESH
118 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
119 #endif
120 
121 
122 inline float IKasin(float f)
123 {
124 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
125 if( f <= -1 ) return float(-IKPI_2);
126 else if( f >= 1 ) return float(IKPI_2);
127 return asinf(f);
128 }
129 inline double IKasin(double f)
130 {
131 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
132 if( f <= -1 ) return -IKPI_2;
133 else if( f >= 1 ) return IKPI_2;
134 return asin(f);
135 }
136 
137 // return positive value in [0,y)
138 inline float IKfmod(float x, float y)
139 {
140  while(x < 0) {
141  x += y;
142  }
143  return fmodf(x,y);
144 }
145 
146 // return positive value in [0,y)
147 inline double IKfmod(double x, double y)
148 {
149  while(x < 0) {
150  x += y;
151  }
152  return fmod(x,y);
153 }
154 
155 inline float IKacos(float f)
156 {
157 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
158 if( f <= -1 ) return float(IKPI);
159 else if( f >= 1 ) return float(0);
160 return acosf(f);
161 }
162 inline double IKacos(double f)
163 {
164 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
165 if( f <= -1 ) return IKPI;
166 else if( f >= 1 ) return 0;
167 return acos(f);
168 }
169 inline float IKsin(float f) { return sinf(f); }
170 inline double IKsin(double f) { return sin(f); }
171 inline float IKcos(float f) { return cosf(f); }
172 inline double IKcos(double f) { return cos(f); }
173 inline float IKtan(float f) { return tanf(f); }
174 inline double IKtan(double f) { return tan(f); }
175 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
176 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
177 inline float IKatan2Simple(float fy, float fx) {
178  return atan2f(fy,fx);
179 }
180 inline float IKatan2(float fy, float fx) {
181  if( isnan(fy) ) {
182  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
183  return float(IKPI_2);
184  }
185  else if( isnan(fx) ) {
186  return 0;
187  }
188  return atan2f(fy,fx);
189 }
190 inline double IKatan2Simple(double fy, double fx) {
191  return atan2(fy,fx);
192 }
193 inline double IKatan2(double fy, double fx) {
194  if( isnan(fy) ) {
195  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
196  return IKPI_2;
197  }
198  else if( isnan(fx) ) {
199  return 0;
200  }
201  return atan2(fy,fx);
202 }
203 
204 template <typename T>
205 struct CheckValue
206 {
207  T value;
208  bool valid;
209 };
210 
211 template <typename T>
212 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
213 {
214  CheckValue<T> ret;
215  ret.valid = false;
216  ret.value = 0;
217  if( !isnan(fy) && !isnan(fx) ) {
218  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
219  ret.value = IKatan2Simple(fy,fx);
220  ret.valid = true;
221  }
222  }
223  return ret;
224 }
225 
226 inline float IKsign(float f) {
227  if( f > 0 ) {
228  return float(1);
229  }
230  else if( f < 0 ) {
231  return float(-1);
232  }
233  return 0;
234 }
235 
236 inline double IKsign(double f) {
237  if( f > 0 ) {
238  return 1.0;
239  }
240  else if( f < 0 ) {
241  return -1.0;
242  }
243  return 0;
244 }
245 
246 template <typename T>
247 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
248 {
249  CheckValue<T> ret;
250  ret.valid = true;
251  if( n == 0 ) {
252  ret.value = 1.0;
253  return ret;
254  }
255  else if( n == 1 )
256  {
257  ret.value = f;
258  return ret;
259  }
260  else if( n < 0 )
261  {
262  if( f == 0 )
263  {
264  ret.valid = false;
265  ret.value = (T)1.0e30;
266  return ret;
267  }
268  if( n == -1 ) {
269  ret.value = T(1.0)/f;
270  return ret;
271  }
272  }
273 
274  int num = n > 0 ? n : -n;
275  if( num == 2 ) {
276  ret.value = f*f;
277  }
278  else if( num == 3 ) {
279  ret.value = f*f*f;
280  }
281  else {
282  ret.value = 1.0;
283  while(num>0) {
284  if( num & 1 ) {
285  ret.value *= f;
286  }
287  num >>= 1;
288  f *= f;
289  }
290  }
291 
292  if( n < 0 ) {
293  ret.value = T(1.0)/ret.value;
294  }
295  return ret;
296 }
297 
300 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
301 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45;
302 x0=IKsin(j[1]);
303 x1=IKcos(j[2]);
304 x2=IKcos(j[3]);
305 x3=IKsin(j[2]);
306 x4=IKsin(j[3]);
307 x5=IKcos(j[4]);
308 x6=IKcos(j[1]);
309 x7=IKsin(j[4]);
310 x8=IKsin(j[6]);
311 x9=IKcos(j[6]);
312 x10=IKsin(j[5]);
313 x11=IKcos(j[5]);
314 x12=((1.0)*x5);
315 x13=((0.62)*x1);
316 x14=((0.6)*x1);
317 x15=((0.115)*x1);
318 x16=((1.0)*x1);
319 x17=(x2*x6);
320 x18=(x1*x2);
321 x19=(x5*x6);
322 x20=(x2*x3);
323 x21=(x10*x5);
324 x22=(x4*x6);
325 x23=(x0*x7);
326 x24=(x3*x4);
327 x25=(x6*x7);
328 x26=(x0*x12);
329 x27=((1.0)*x24);
330 x28=(x0*x24);
331 x29=(x0*x1*x4);
332 x30=(x0*x16*x2);
333 x31=(x16*x17);
334 x32=((((-1.0)*x27))+x18);
335 x33=((((-1.0)*x16*x2))+x27);
336 x34=((((-1.0)*x20))+(((-1.0)*x16*x4)));
337 x35=(((x0*x20))+x29);
338 x36=(((x1*x22))+((x17*x3)));
339 x37=(x36*x7);
340 x38=(x35*x7);
341 x39=(x36*x5);
342 x40=(x35*x5);
343 x41=((((-1.0)*x25))+x40);
344 x42=(x39+x23);
345 x43=((((-1.0)*x10*x34))+(((-1.0)*x11*x12*x32)));
346 x44=(((x10*(((((-1.0)*x0*x27))+x30))))+((x11*x41)));
347 x45=(((x11*x42))+((x10*((x31+(((-1.0)*x22*x3)))))));
348 eerot[0]=(((x8*(((((-1.0)*x38))+(((-1.0)*x12*x6))))))+((x44*x9)));
349 eerot[1]=(((x9*((x38+x19))))+((x44*x8)));
350 eerot[2]=(((x10*x41))+((x11*(((((-1.0)*x30))+x28)))));
351 IkReal x46=((1.0)*x0*x2);
352 eetrans[0]=((-3.153)+((x10*(((((0.115)*x40))+(((-0.115)*x25))))))+(((-0.12)*x0*x20))+(((-1.0)*x0*x14))+(((-0.12)*x29))+((x11*(((((0.115)*x28))+(((-1.0)*x15*x46))))))+(((-1.0)*x13*x46))+(((0.62)*x28))+(((-0.18)*x0))+j[0]);
353 eerot[3]=(((x8*(((((-1.0)*x37))+x26))))+((x45*x9)));
354 eerot[4]=(((x45*x8))+((x9*(((((-1.0)*x26))+x37)))));
355 eerot[5]=(((x10*x42))+((x11*(((((-1.0)*x31))+((x22*x3)))))));
356 IkReal x47=((1.0)*x17);
357 IkReal x48=(x22*x3);
358 eetrans[1]=((-0.212)+((x11*(((((0.115)*x48))+(((-1.0)*x15*x47))))))+((x10*(((((0.115)*x23))+(((0.115)*x39))))))+(((-0.12)*x17*x3))+(((-1.0)*x14*x6))+(((-1.0)*x13*x47))+(((-0.18)*x6))+(((-0.12)*x1*x22))+(((0.62)*x48)));
359 eerot[6]=(((x43*x9))+((x32*x7*x8)));
360 eerot[7]=(((x43*x8))+((x33*x7*x9)));
361 eerot[8]=(((x21*x33))+((x11*x34)));
362 IkReal x49=((1.0)*x15);
363 eetrans[2]=((1.34)+(((0.12)*x18))+(((-0.6)*x3))+((x11*(((((-1.0)*x4*x49))+(((-0.115)*x20))))))+(((-0.12)*x24))+((x21*(((((0.115)*x24))+(((-1.0)*x2*x49))))))+(((-0.62)*x20))+(((-1.0)*x13*x4)));
364 }
365 
366 IKFAST_API int GetNumFreeParameters() { return 1; }
367 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {2}; return freeparams; }
368 IKFAST_API int GetNumJoints() { return 7; }
369 
370 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
371 
372 IKFAST_API int GetIkType() { return 0x67000001; }
373 
374 class IKSolver {
375 public:
376 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,j6,cj6,sj6,htj6,j6mul,j2,cj2,sj2,htj2,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
377 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij2[2], _nj2;
378 
379 IkReal j100, cj100, sj100;
380 unsigned char _ij100[2], _nj100;
381 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
382 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; _ij2[0] = -1; _ij2[1] = -1; _nj2 = 0;
383 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
384  solutions.Clear();
385 j2=pfree[0]; cj2=cos(pfree[0]); sj2=sin(pfree[0]), htj2=tan(pfree[0]*0.5);
386 r00 = eerot[0*3+0];
387 r01 = eerot[0*3+1];
388 r02 = eerot[0*3+2];
389 r10 = eerot[1*3+0];
390 r11 = eerot[1*3+1];
391 r12 = eerot[1*3+2];
392 r20 = eerot[2*3+0];
393 r21 = eerot[2*3+1];
394 r22 = eerot[2*3+2];
395 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
396 
397 new_r00=((-1.0)*r21);
398 new_r01=((-1.0)*r20);
399 new_r02=r22;
400 new_px=((1.34)+(((-1.0)*pz))+(((0.115)*r22)));
401 new_r10=r11;
402 new_r11=r10;
403 new_r12=((-1.0)*r12);
404 new_py=((0.212)+py+(((-0.115)*r12)));
405 new_r20=r01;
406 new_r21=r00;
407 new_r22=((-1.0)*r02);
408 new_pz=((3.153)+px+(((-0.115)*r02)));
409 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;
410 IkReal x50=((1.0)*px);
411 IkReal x51=((1.0)*pz);
412 IkReal x52=((1.0)*py);
413 pp=((px*px)+(py*py)+(pz*pz));
414 npx=(((px*r00))+((py*r10))+((pz*r20)));
415 npy=(((px*r01))+((py*r11))+((pz*r21)));
416 npz=(((px*r02))+((py*r12))+((pz*r22)));
417 rxp0_0=((((-1.0)*r20*x52))+((pz*r10)));
418 rxp0_1=(((px*r20))+(((-1.0)*r00*x51)));
419 rxp0_2=((((-1.0)*r10*x50))+((py*r00)));
420 rxp1_0=((((-1.0)*r21*x52))+((pz*r11)));
421 rxp1_1=(((px*r21))+(((-1.0)*r01*x51)));
422 rxp1_2=((((-1.0)*r11*x50))+((py*r01)));
423 rxp2_0=(((pz*r12))+(((-1.0)*r22*x52)));
424 rxp2_1=(((px*r22))+(((-1.0)*r02*x51)));
425 rxp2_2=((((-1.0)*r12*x50))+((py*r02)));
426 {
427 IkReal j3eval[1];
428 j3eval[0]=((sj2*sj2)+(cj2*cj2));
429 if( IKabs(j3eval[0]) < 0.0000010000000000 )
430 {
431 continue; // no branches [j0, j1, j3]
432 
433 } else
434 {
435 {
436 IkReal j3array[2], cj3array[2], sj3array[2];
437 bool j3valid[2]={false};
438 _nj3 = 2;
439 IkReal x53=((((0.62)*sj2))+(((-0.12)*cj2)));
440 IkReal x54=((((0.62)*cj2))+(((0.12)*sj2)));
441 CheckValue<IkReal> x57 = IKatan2WithCheck(IkReal(x53),IkReal(x54),IKFAST_ATAN2_MAGTHRESH);
442 if(!x57.valid){
443 continue;
444 }
445 IkReal x55=((1.0)*(x57.value));
446 if((((x54*x54)+(x53*x53))) < -0.00001)
447 continue;
448 CheckValue<IkReal> x58=IKPowWithIntegerCheck(IKabs(IKsqrt(((x54*x54)+(x53*x53)))),-1);
449 if(!x58.valid){
450 continue;
451 }
452 if( (((x58.value)*(((((0.6)*sj2))+(((-1.0)*px)))))) < -1-IKFAST_SINCOS_THRESH || (((x58.value)*(((((0.6)*sj2))+(((-1.0)*px)))))) > 1+IKFAST_SINCOS_THRESH )
453  continue;
454 IkReal x56=IKasin(((x58.value)*(((((0.6)*sj2))+(((-1.0)*px))))));
455 j3array[0]=((((-1.0)*x55))+(((-1.0)*x56)));
456 sj3array[0]=IKsin(j3array[0]);
457 cj3array[0]=IKcos(j3array[0]);
458 j3array[1]=((3.14159265358979)+x56+(((-1.0)*x55)));
459 sj3array[1]=IKsin(j3array[1]);
460 cj3array[1]=IKcos(j3array[1]);
461 if( j3array[0] > IKPI )
462 {
463  j3array[0]-=IK2PI;
464 }
465 else if( j3array[0] < -IKPI )
466 { j3array[0]+=IK2PI;
467 }
468 j3valid[0] = true;
469 if( j3array[1] > IKPI )
470 {
471  j3array[1]-=IK2PI;
472 }
473 else if( j3array[1] < -IKPI )
474 { j3array[1]+=IK2PI;
475 }
476 j3valid[1] = true;
477 for(int ij3 = 0; ij3 < 2; ++ij3)
478 {
479 if( !j3valid[ij3] )
480 {
481  continue;
482 }
483 _ij3[0] = ij3; _ij3[1] = -1;
484 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
485 {
486 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
487 {
488  j3valid[iij3]=false; _ij3[1] = iij3; break;
489 }
490 }
491 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
492 
493 {
494 IkReal j0array[2], cj0array[2], sj0array[2];
495 bool j0valid[2]={false};
496 _nj0 = 2;
497 if((((0.7912)+(((0.0432)*cj2*sj3))+(((0.0432)*cj3*sj2))+(((-0.2232)*sj2*sj3))+(((0.144)*sj3))+(((-1.0)*pp))+(pz*pz)+(((0.216)*cj2))+(((0.2232)*cj2*cj3))+(((0.744)*cj3)))) < -0.00001)
498 continue;
499 IkReal x59=IKsqrt(((0.7912)+(((0.0432)*cj2*sj3))+(((0.0432)*cj3*sj2))+(((-0.2232)*sj2*sj3))+(((0.144)*sj3))+(((-1.0)*pp))+(pz*pz)+(((0.216)*cj2))+(((0.2232)*cj2*cj3))+(((0.744)*cj3))));
500 j0array[0]=(pz+x59);
501 sj0array[0]=IKsin(j0array[0]);
502 cj0array[0]=IKcos(j0array[0]);
503 j0array[1]=(pz+(((-1.0)*x59)));
504 sj0array[1]=IKsin(j0array[1]);
505 cj0array[1]=IKcos(j0array[1]);
506 j0valid[0] = true;
507 j0valid[1] = true;
508 for(int ij0 = 0; ij0 < 2; ++ij0)
509 {
510 if( !j0valid[ij0] )
511 {
512  continue;
513 }
514 _ij0[0] = ij0; _ij0[1] = -1;
515 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
516 {
517 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
518 {
519  j0valid[iij0]=false; _ij0[1] = iij0; break;
520 }
521 }
522 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
523 
524 {
525 IkReal j1eval[3];
526 IkReal x60=(sj2*sj3);
527 IkReal x61=(cj2*cj3);
528 IkReal x62=(cj3*sj2);
529 IkReal x63=(cj2*sj3);
530 j1eval[0]=((1.5)+(((5.16666666666667)*x61))+(((-5.16666666666667)*x60))+x62+x63+(((5.0)*cj2)));
531 j1eval[1]=IKsign(((0.18)+(((-0.62)*x60))+(((0.12)*x63))+(((0.12)*x62))+(((0.62)*x61))+(((0.6)*cj2))));
532 j1eval[2]=((IKabs(py))+(IKabs(((((-1.0)*pz))+j0))));
533 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
534 {
535 {
536 IkReal j1eval[2];
537 IkReal x64=((1.0)*sj2);
538 IkReal x65=(((sj2*(px*px)))+(((-1.0)*x64*(j0*j0)))+(((-1.0)*pp*x64))+(((2.0)*j0*pz*sj2)));
539 j1eval[0]=x65;
540 j1eval[1]=IKsign(x65);
541 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
542 {
543 {
544 IkReal j1eval[2];
545 IkReal x66=((((-2.0)*cj2*j0*pz))+(((-1.0)*cj2*(px*px)))+((cj2*(j0*j0)))+((cj2*pp)));
546 j1eval[0]=x66;
547 j1eval[1]=IKsign(x66);
548 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
549 {
550 {
551 IkReal evalcond[1];
552 bool bgotonextstatement = true;
553 do
554 {
555 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j2)))), 6.28318530717959)));
556 if( IKabs(evalcond[0]) < 0.0000050000000000 )
557 {
558 bgotonextstatement=false;
559 {
560 IkReal j1eval[3];
561 sj2=1.0;
562 cj2=0;
563 j2=1.5707963267949;
564 j1eval[0]=((1.5)+cj3+(((-5.16666666666667)*sj3)));
565 j1eval[1]=IKsign(((0.18)+(((-0.62)*sj3))+(((0.12)*cj3))));
566 j1eval[2]=((IKabs(py))+(IKabs(((((-1.0)*pz))+j0))));
567 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
568 {
569 {
570 IkReal j1eval[3];
571 sj2=1.0;
572 cj2=0;
573 j2=1.5707963267949;
574 IkReal x67=((0.12)*cj3);
575 IkReal x68=((0.62)*sj3);
576 IkReal x69=((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)));
577 j1eval[0]=x69;
578 j1eval[1]=IKsign(x69);
579 j1eval[2]=((IKabs(((((0.18)*pz))+((j0*x68))+(((-1.0)*j0*x67))+(((-0.18)*j0))+(((-1.0)*pz*x68))+((pz*x67)))))+(((0.02)*(IKabs(((((6.0)*cj3*py))+(((9.0)*py))+(((-31.0)*py*sj3))))))));
580 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
581 {
582 {
583 IkReal j1eval[2];
584 sj2=1.0;
585 cj2=0;
586 j2=1.5707963267949;
587 IkReal x70=((5.16666666666667)*sj3);
588 j1eval[0]=((1.5)+cj3+(((-1.0)*x70)));
589 j1eval[1]=(((pz*x70))+(((-1.0)*cj3*pz))+((cj3*j0))+(((1.5)*j0))+(((-1.5)*pz))+(((-1.0)*j0*x70)));
590 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
591 {
592 {
593 IkReal evalcond[1];
594 bool bgotonextstatement = true;
595 do
596 {
597 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((3.23944890393253)+j3)))), 6.28318530717959)));
598 if( IKabs(evalcond[0]) < 0.0000050000000000 )
599 {
600 bgotonextstatement=false;
601 {
602 IkReal j1array[1], cj1array[1], sj1array[1];
603 bool j1valid[1]={false};
604 _nj1 = 1;
605 if( IKabs(((((1000000000.0)*pz))+(((-1000000000.0)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((1000000000.0)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((1000000000.0)*pz))+(((-1000000000.0)*j0))))+IKsqr(((1000000000.0)*py))-1) <= IKFAST_SINCOS_THRESH )
606  continue;
607 j1array[0]=IKatan2(((((1000000000.0)*pz))+(((-1000000000.0)*j0))), ((1000000000.0)*py));
608 sj1array[0]=IKsin(j1array[0]);
609 cj1array[0]=IKcos(j1array[0]);
610 if( j1array[0] > IKPI )
611 {
612  j1array[0]-=IK2PI;
613 }
614 else if( j1array[0] < -IKPI )
615 { j1array[0]+=IK2PI;
616 }
617 j1valid[0] = true;
618 for(int ij1 = 0; ij1 < 1; ++ij1)
619 {
620 if( !j1valid[ij1] )
621 {
622  continue;
623 }
624 _ij1[0] = ij1; _ij1[1] = -1;
625 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
626 {
627 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
628 {
629  j1valid[iij1]=false; _ij1[1] = iij1; break;
630 }
631 }
632 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
633 {
634 IkReal evalcond[5];
635 IkReal x71=IKcos(j1);
636 IkReal x72=IKsin(j1);
637 IkReal x73=((1.0)*pz);
638 IkReal x74=((0.36)*x72);
639 IkReal x75=(py*x71);
640 evalcond[0]=(py+(((-1.0e-9)*x71)));
641 evalcond[1]=(pz+(((-1.0)*j0))+(((-1.0e-9)*x72)));
642 evalcond[2]=(((py*x72))+((j0*x71))+(((-1.0)*x71*x73)));
643 evalcond[3]=((1.0e-9)+((j0*x72))+(((-1.0)*x75))+(((-1.0)*x72*x73)));
644 evalcond[4]=((3.6e-10)+(((-0.36)*x75))+((j0*x74))+(((-1.0)*pz*x74)));
645 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
646 {
647 continue;
648 }
649 }
650 
651 rotationfunction0(solutions);
652 }
653 }
654 
655 }
656 } while(0);
657 if( bgotonextstatement )
658 {
659 bool bgotonextstatement = true;
660 do
661 {
662 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-0.480225169536768)+j3)))), 6.28318530717959)));
663 if( IKabs(evalcond[0]) < 0.0000050000000000 )
664 {
665 bgotonextstatement=false;
666 {
667 IkReal j1array[1], cj1array[1], sj1array[1];
668 bool j1valid[1]={false};
669 _nj1 = 1;
670 if( IKabs(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((5317165966.79048)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0))))+IKsqr(((5317165966.79048)*py))-1) <= IKFAST_SINCOS_THRESH )
671  continue;
672 j1array[0]=IKatan2(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0))), ((5317165966.79048)*py));
673 sj1array[0]=IKsin(j1array[0]);
674 cj1array[0]=IKcos(j1array[0]);
675 if( j1array[0] > IKPI )
676 {
677  j1array[0]-=IK2PI;
678 }
679 else if( j1array[0] < -IKPI )
680 { j1array[0]+=IK2PI;
681 }
682 j1valid[0] = true;
683 for(int ij1 = 0; ij1 < 1; ++ij1)
684 {
685 if( !j1valid[ij1] )
686 {
687  continue;
688 }
689 _ij1[0] = ij1; _ij1[1] = -1;
690 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
691 {
692 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
693 {
694  j1valid[iij1]=false; _ij1[1] = iij1; break;
695 }
696 }
697 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
698 {
699 IkReal evalcond[5];
700 IkReal x821=IKcos(j1);
701 IkReal x822=IKsin(j1);
702 IkReal x823=(py*x821);
703 IkReal x824=(pz*x822);
704 IkReal x825=(j0*x822);
705 evalcond[0]=((((-1.88070112207465e-10)*x821))+py);
706 evalcond[1]=((((-1.88070112207465e-10)*x822))+pz+(((-1.0)*j0)));
707 evalcond[2]=(((py*x822))+((j0*x821))+(((-1.0)*pz*x821)));
708 evalcond[3]=((1.88070112207465e-10)+(((-1.0)*x824))+(((-1.0)*x823))+x825);
709 evalcond[4]=((6.77052403946874e-11)+(((0.36)*x825))+(((-0.36)*x823))+(((-0.36)*x824)));
710 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
711 {
712 continue;
713 }
714 }
715 
716 rotationfunction0(solutions);
717 }
718 }
719 
720 }
721 } while(0);
722 if( bgotonextstatement )
723 {
724 bool bgotonextstatement = true;
725 do
726 {
727 IkReal x826=IKcos(pz);
728 IkReal x827=IKsin(pz);
729 if((((-1.0)*(py*py))) < -0.00001)
730 continue;
731 IkReal x828=IKsqrt(((-1.0)*(py*py)));
732 IkReal x829=IKcos(x828);
733 IkReal x830=IKsin(x828);
734 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
735 continue;
736 IkReal gconst0=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
737 IkReal gconst1=(((x826*x830))+((x827*x829)));
738 IkReal gconst2=((((-1.0)*x827*x830))+((x826*x829)));
739 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
740 continue;
741 evalcond[0]=IKabs(((((-1.0)*pz))+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
742 if( IKabs(evalcond[0]) < 0.0000050000000000 )
743 {
744 bgotonextstatement=false;
745 {
746 IkReal j1eval[1];
747 IkReal x831=IKcos(pz);
748 IkReal x832=IKsin(pz);
749 IkReal x833=x828;
750 IkReal x834=IKcos(x833);
751 IkReal x835=IKsin(x833);
752 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
753 continue;
754 IkReal x836=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
755 sj2=1.0;
756 cj2=0;
757 j2=1.5707963267949;
758 sj0=gconst1;
759 cj0=gconst2;
760 j0=x836;
761 IkReal gconst0=x836;
762 IkReal gconst1=(((x831*x835))+((x832*x834)));
763 IkReal gconst2=(((x831*x834))+(((-1.0)*x832*x835)));
764 j1eval[0]=IKabs(((-1.5)+(((5.16666666666667)*sj3))+(((8.33333333333333)*py))+(((-1.0)*cj3))));
765 if( IKabs(j1eval[0]) < 0.0000000100000000 )
766 {
767 continue; // no branches [j1]
768 
769 } else
770 {
771 IkReal op[2+1], zeror[2];
772 int numroots;
773 IkReal x837=((8.33333333333333)*py);
774 IkReal x838=((5.16666666666667)*sj3);
775 op[0]=((-1.5)+x837+x838+(((-1.0)*cj3)));
776 op[1]=0;
777 op[2]=((1.5)+cj3+(((-1.0)*x838))+x837);
778 polyroots2(op,zeror,numroots);
779 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
780 int numsolutions = 0;
781 for(int ij1 = 0; ij1 < numroots; ++ij1)
782 {
783 IkReal htj1 = zeror[ij1];
784 tempj1array[0]=((2.0)*(atan(htj1)));
785 for(int kj1 = 0; kj1 < 1; ++kj1)
786 {
787 j1array[numsolutions] = tempj1array[kj1];
788 if( j1array[numsolutions] > IKPI )
789 {
790  j1array[numsolutions]-=IK2PI;
791 }
792 else if( j1array[numsolutions] < -IKPI )
793 {
794  j1array[numsolutions]+=IK2PI;
795 }
796 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
797 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
798 numsolutions++;
799 }
800 }
801 bool j1valid[2]={true,true};
802 _nj1 = 2;
803 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
804  {
805 if( !j1valid[ij1] )
806 {
807  continue;
808 }
809  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
810 htj1 = IKtan(j1/2);
811 
812 _ij1[0] = ij1; _ij1[1] = -1;
813 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
814 {
815 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
816 {
817  j1valid[iij1]=false; _ij1[1] = iij1; break;
818 }
819 }
820 rotationfunction0(solutions);
821  }
822 
823 }
824 
825 }
826 
827 }
828 } while(0);
829 if( bgotonextstatement )
830 {
831 bool bgotonextstatement = true;
832 do
833 {
834 IkReal x839=IKcos(pz);
835 IkReal x840=IKsin(pz);
836 if((((-1.0)*(py*py))) < -0.00001)
837 continue;
838 IkReal x841=IKsqrt(((-1.0)*(py*py)));
839 IkReal x842=IKcos(x841);
840 IkReal x843=IKsin(x841);
841 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
842 continue;
843 IkReal gconst3=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
844 IkReal gconst4=(((x840*x842))+(((-1.0)*x839*x843)));
845 IkReal gconst5=(((x840*x843))+((x839*x842)));
846 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
847 continue;
848 evalcond[0]=IKabs(((((-1.0)*pz))+(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
849 if( IKabs(evalcond[0]) < 0.0000050000000000 )
850 {
851 bgotonextstatement=false;
852 {
853 IkReal j1eval[1];
854 IkReal x844=IKcos(pz);
855 IkReal x845=IKsin(pz);
856 IkReal x846=x841;
857 IkReal x847=IKcos(x846);
858 IkReal x848=IKsin(x846);
859 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
860 continue;
861 IkReal x849=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
862 sj2=1.0;
863 cj2=0;
864 j2=1.5707963267949;
865 sj0=gconst4;
866 cj0=gconst5;
867 j0=x849;
868 IkReal gconst3=x849;
869 IkReal gconst4=(((x845*x847))+(((-1.0)*x844*x848)));
870 IkReal gconst5=(((x845*x848))+((x844*x847)));
871 j1eval[0]=IKabs(((-1.5)+(((5.16666666666667)*sj3))+(((8.33333333333333)*py))+(((-1.0)*cj3))));
872 if( IKabs(j1eval[0]) < 0.0000000100000000 )
873 {
874 continue; // no branches [j1]
875 
876 } else
877 {
878 IkReal op[2+1], zeror[2];
879 int numroots;
880 IkReal x850=((8.33333333333333)*py);
881 IkReal x851=((5.16666666666667)*sj3);
882 op[0]=((-1.5)+x850+x851+(((-1.0)*cj3)));
883 op[1]=0;
884 op[2]=((1.5)+(((-1.0)*x851))+cj3+x850);
885 polyroots2(op,zeror,numroots);
886 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
887 int numsolutions = 0;
888 for(int ij1 = 0; ij1 < numroots; ++ij1)
889 {
890 IkReal htj1 = zeror[ij1];
891 tempj1array[0]=((2.0)*(atan(htj1)));
892 for(int kj1 = 0; kj1 < 1; ++kj1)
893 {
894 j1array[numsolutions] = tempj1array[kj1];
895 if( j1array[numsolutions] > IKPI )
896 {
897  j1array[numsolutions]-=IK2PI;
898 }
899 else if( j1array[numsolutions] < -IKPI )
900 {
901  j1array[numsolutions]+=IK2PI;
902 }
903 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
904 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
905 numsolutions++;
906 }
907 }
908 bool j1valid[2]={true,true};
909 _nj1 = 2;
910 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
911  {
912 if( !j1valid[ij1] )
913 {
914  continue;
915 }
916  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
917 htj1 = IKtan(j1/2);
918 
919 _ij1[0] = ij1; _ij1[1] = -1;
920 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
921 {
922 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
923 {
924  j1valid[iij1]=false; _ij1[1] = iij1; break;
925 }
926 }
927 rotationfunction0(solutions);
928  }
929 
930 }
931 
932 }
933 
934 }
935 } while(0);
936 if( bgotonextstatement )
937 {
938 bool bgotonextstatement = true;
939 do
940 {
941 if( 1 )
942 {
943 bgotonextstatement=false;
944 continue; // branch miss [j1]
945 
946 }
947 } while(0);
948 if( bgotonextstatement )
949 {
950 }
951 }
952 }
953 }
954 }
955 }
956 
957 } else
958 {
959 {
960 IkReal j1array[1], cj1array[1], sj1array[1];
961 bool j1valid[1]={false};
962 _nj1 = 1;
963 IkReal x852=((0.62)*sj3);
964 IkReal x853=((0.12)*cj3);
965 CheckValue<IkReal> x854=IKPowWithIntegerCheck(((0.18)+(((-1.0)*x852))+x853),-1);
966 if(!x854.valid){
967 continue;
968 }
969 CheckValue<IkReal> x855=IKPowWithIntegerCheck(((((-0.18)*pz))+(((0.18)*j0))+((j0*x853))+(((-1.0)*pz*x853))+(((-1.0)*j0*x852))+((pz*x852))),-1);
970 if(!x855.valid){
971 continue;
972 }
973 if( IKabs(((x854.value)*(((((-1.0)*pz))+j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x855.value)*(((((-1.0)*j0*py))+((py*pz)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x854.value)*(((((-1.0)*pz))+j0))))+IKsqr(((x855.value)*(((((-1.0)*j0*py))+((py*pz))))))-1) <= IKFAST_SINCOS_THRESH )
974  continue;
975 j1array[0]=IKatan2(((x854.value)*(((((-1.0)*pz))+j0))), ((x855.value)*(((((-1.0)*j0*py))+((py*pz))))));
976 sj1array[0]=IKsin(j1array[0]);
977 cj1array[0]=IKcos(j1array[0]);
978 if( j1array[0] > IKPI )
979 {
980  j1array[0]-=IK2PI;
981 }
982 else if( j1array[0] < -IKPI )
983 { j1array[0]+=IK2PI;
984 }
985 j1valid[0] = true;
986 for(int ij1 = 0; ij1 < 1; ++ij1)
987 {
988 if( !j1valid[ij1] )
989 {
990  continue;
991 }
992 _ij1[0] = ij1; _ij1[1] = -1;
993 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
994 {
995 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
996 {
997  j1valid[iij1]=false; _ij1[1] = iij1; break;
998 }
999 }
1000 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1001 {
1002 IkReal evalcond[5];
1003 IkReal x856=IKcos(j1);
1004 IkReal x857=IKsin(j1);
1005 IkReal x858=((0.62)*sj3);
1006 IkReal x859=((0.12)*cj3);
1007 IkReal x860=(py*x856);
1008 IkReal x861=(pz*x857);
1009 IkReal x862=(j0*x857);
1010 evalcond[0]=((((-1.0)*pz*x856))+((j0*x856))+((py*x857)));
1011 evalcond[1]=((((0.18)*x856))+((x856*x859))+py+(((-1.0)*x856*x858)));
1012 evalcond[2]=((((0.18)*x857))+pz+(((-1.0)*j0))+((x857*x859))+(((-1.0)*x857*x858)));
1013 evalcond[3]=((-0.18)+(((-1.0)*x859))+(((-1.0)*x861))+(((-1.0)*x860))+x858+x862);
1014 evalcond[4]=((-0.0648)+(((0.2232)*sj3))+(((-0.0432)*cj3))+(((0.36)*x862))+(((-0.36)*x860))+(((-0.36)*x861)));
1015 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1016 {
1017 continue;
1018 }
1019 }
1020 
1021 rotationfunction0(solutions);
1022 }
1023 }
1024 
1025 }
1026 
1027 }
1028 
1029 } else
1030 {
1031 {
1032 IkReal j1array[1], cj1array[1], sj1array[1];
1033 bool j1valid[1]={false};
1034 _nj1 = 1;
1035 IkReal x863=((0.62)*sj3);
1036 IkReal x864=((0.12)*cj3);
1037 CheckValue<IkReal> x865 = IKatan2WithCheck(IkReal(((((0.18)*pz))+(((-1.0)*pz*x863))+(((-0.18)*j0))+(((-1.0)*j0*x864))+((pz*x864))+((j0*x863)))),IkReal(((((0.18)*py))+(((-1.0)*py*x863))+((py*x864)))),IKFAST_ATAN2_MAGTHRESH);
1038 if(!x865.valid){
1039 continue;
1040 }
1041 CheckValue<IkReal> x866=IKPowWithIntegerCheck(IKsign(((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)))),-1);
1042 if(!x866.valid){
1043 continue;
1044 }
1045 j1array[0]=((-1.5707963267949)+(x865.value)+(((1.5707963267949)*(x866.value))));
1046 sj1array[0]=IKsin(j1array[0]);
1047 cj1array[0]=IKcos(j1array[0]);
1048 if( j1array[0] > IKPI )
1049 {
1050  j1array[0]-=IK2PI;
1051 }
1052 else if( j1array[0] < -IKPI )
1053 { j1array[0]+=IK2PI;
1054 }
1055 j1valid[0] = true;
1056 for(int ij1 = 0; ij1 < 1; ++ij1)
1057 {
1058 if( !j1valid[ij1] )
1059 {
1060  continue;
1061 }
1062 _ij1[0] = ij1; _ij1[1] = -1;
1063 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1064 {
1065 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1066 {
1067  j1valid[iij1]=false; _ij1[1] = iij1; break;
1068 }
1069 }
1070 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1071 {
1072 IkReal evalcond[5];
1073 IkReal x867=IKcos(j1);
1074 IkReal x868=IKsin(j1);
1075 IkReal x869=((0.62)*sj3);
1076 IkReal x870=((0.12)*cj3);
1077 IkReal x871=(py*x867);
1078 IkReal x872=(pz*x868);
1079 IkReal x873=(j0*x868);
1080 evalcond[0]=((((-1.0)*pz*x867))+((py*x868))+((j0*x867)));
1081 evalcond[1]=(((x867*x870))+(((0.18)*x867))+py+(((-1.0)*x867*x869)));
1082 evalcond[2]=((((0.18)*x868))+((x868*x870))+(((-1.0)*x868*x869))+pz+(((-1.0)*j0)));
1083 evalcond[3]=((-0.18)+(((-1.0)*x871))+(((-1.0)*x872))+x869+x873+(((-1.0)*x870)));
1084 evalcond[4]=((-0.0648)+(((-0.36)*x872))+(((-0.36)*x871))+(((0.2232)*sj3))+(((0.36)*x873))+(((-0.0432)*cj3)));
1085 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1086 {
1087 continue;
1088 }
1089 }
1090 
1091 rotationfunction0(solutions);
1092 }
1093 }
1094 
1095 }
1096 
1097 }
1098 
1099 } else
1100 {
1101 {
1102 IkReal j1array[1], cj1array[1], sj1array[1];
1103 bool j1valid[1]={false};
1104 _nj1 = 1;
1105 CheckValue<IkReal> x874 = IKatan2WithCheck(IkReal(((((-1.0)*pz))+j0)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
1106 if(!x874.valid){
1107 continue;
1108 }
1109 CheckValue<IkReal> x875=IKPowWithIntegerCheck(IKsign(((0.18)+(((-0.62)*sj3))+(((0.12)*cj3)))),-1);
1110 if(!x875.valid){
1111 continue;
1112 }
1113 j1array[0]=((-1.5707963267949)+(x874.value)+(((1.5707963267949)*(x875.value))));
1114 sj1array[0]=IKsin(j1array[0]);
1115 cj1array[0]=IKcos(j1array[0]);
1116 if( j1array[0] > IKPI )
1117 {
1118  j1array[0]-=IK2PI;
1119 }
1120 else if( j1array[0] < -IKPI )
1121 { j1array[0]+=IK2PI;
1122 }
1123 j1valid[0] = true;
1124 for(int ij1 = 0; ij1 < 1; ++ij1)
1125 {
1126 if( !j1valid[ij1] )
1127 {
1128  continue;
1129 }
1130 _ij1[0] = ij1; _ij1[1] = -1;
1131 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1132 {
1133 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1134 {
1135  j1valid[iij1]=false; _ij1[1] = iij1; break;
1136 }
1137 }
1138 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1139 {
1140 IkReal evalcond[5];
1141 IkReal x876=IKcos(j1);
1142 IkReal x877=IKsin(j1);
1143 IkReal x878=((0.62)*sj3);
1144 IkReal x879=((0.12)*cj3);
1145 IkReal x880=(py*x876);
1146 IkReal x881=(pz*x877);
1147 IkReal x882=(j0*x877);
1148 evalcond[0]=((((-1.0)*pz*x876))+((py*x877))+((j0*x876)));
1149 evalcond[1]=(((x876*x879))+(((-1.0)*x876*x878))+(((0.18)*x876))+py);
1150 evalcond[2]=((((-1.0)*x877*x878))+(((0.18)*x877))+pz+(((-1.0)*j0))+((x877*x879)));
1151 evalcond[3]=((-0.18)+(((-1.0)*x881))+(((-1.0)*x880))+x878+x882+(((-1.0)*x879)));
1152 evalcond[4]=((-0.0648)+(((0.36)*x882))+(((0.2232)*sj3))+(((-0.36)*x880))+(((-0.36)*x881))+(((-0.0432)*cj3)));
1153 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1154 {
1155 continue;
1156 }
1157 }
1158 
1159 rotationfunction0(solutions);
1160 }
1161 }
1162 
1163 }
1164 
1165 }
1166 
1167 }
1168 } while(0);
1169 if( bgotonextstatement )
1170 {
1171 bool bgotonextstatement = true;
1172 do
1173 {
1174 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j2)))), 6.28318530717959)));
1175 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1176 {
1177 bgotonextstatement=false;
1178 {
1179 IkReal j1eval[3];
1180 sj2=-1.0;
1181 cj2=0;
1182 j2=-1.5707963267949;
1183 j1eval[0]=((1.5)+(((5.16666666666667)*sj3))+(((-1.0)*cj3)));
1184 j1eval[1]=((IKabs(py))+(IKabs(((((-1.0)*pz))+j0))));
1185 j1eval[2]=IKsign(((0.18)+(((0.62)*sj3))+(((-0.12)*cj3))));
1186 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
1187 {
1188 {
1189 IkReal j1eval[3];
1190 sj2=-1.0;
1191 cj2=0;
1192 j2=-1.5707963267949;
1193 IkReal x883=((0.12)*cj3);
1194 IkReal x884=((0.62)*sj3);
1195 IkReal x885=((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)));
1196 j1eval[0]=x885;
1197 j1eval[1]=((IKabs(((((-0.18)*pz))+(((0.18)*j0))+((pz*x883))+(((-1.0)*pz*x884))+(((-1.0)*j0*x883))+((j0*x884)))))+(((0.02)*(IKabs(((((6.0)*cj3*py))+(((-9.0)*py))+(((-31.0)*py*sj3))))))));
1198 j1eval[2]=IKsign(x885);
1199 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
1200 {
1201 {
1202 IkReal j1eval[2];
1203 sj2=-1.0;
1204 cj2=0;
1205 j2=-1.5707963267949;
1206 IkReal x886=((1.0)*cj3);
1207 IkReal x887=((5.16666666666667)*sj3);
1208 j1eval[0]=((1.5)+(((-1.0)*x886))+x887);
1209 j1eval[1]=((((-1.0)*pz*x887))+(((1.5)*j0))+(((-1.0)*j0*x886))+(((-1.5)*pz))+((cj3*pz))+((j0*x887)));
1210 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
1211 {
1212 {
1213 IkReal evalcond[1];
1214 bool bgotonextstatement = true;
1215 do
1216 {
1217 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((0.0978562503427396)+j3)))), 6.28318530717959)));
1218 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1219 {
1220 bgotonextstatement=false;
1221 {
1222 IkReal j1array[1], cj1array[1], sj1array[1];
1223 bool j1valid[1]={false};
1224 _nj1 = 1;
1225 if( IKabs(((((1000000000.0)*pz))+(((-1000000000.0)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((1000000000.0)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((1000000000.0)*pz))+(((-1000000000.0)*j0))))+IKsqr(((1000000000.0)*py))-1) <= IKFAST_SINCOS_THRESH )
1226  continue;
1227 j1array[0]=IKatan2(((((1000000000.0)*pz))+(((-1000000000.0)*j0))), ((1000000000.0)*py));
1228 sj1array[0]=IKsin(j1array[0]);
1229 cj1array[0]=IKcos(j1array[0]);
1230 if( j1array[0] > IKPI )
1231 {
1232  j1array[0]-=IK2PI;
1233 }
1234 else if( j1array[0] < -IKPI )
1235 { j1array[0]+=IK2PI;
1236 }
1237 j1valid[0] = true;
1238 for(int ij1 = 0; ij1 < 1; ++ij1)
1239 {
1240 if( !j1valid[ij1] )
1241 {
1242  continue;
1243 }
1244 _ij1[0] = ij1; _ij1[1] = -1;
1245 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1246 {
1247 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1248 {
1249  j1valid[iij1]=false; _ij1[1] = iij1; break;
1250 }
1251 }
1252 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1253 {
1254 IkReal evalcond[5];
1255 IkReal x888=IKcos(j1);
1256 IkReal x889=IKsin(j1);
1257 IkReal x890=((1.0)*j0);
1258 IkReal x891=(py*x888);
1259 IkReal x892=(pz*x889);
1260 evalcond[0]=((((-1.0e-9)*x888))+py);
1261 evalcond[1]=((((-1.0e-9)*x889))+(((-1.0)*x890))+pz);
1262 evalcond[2]=(((py*x889))+(((-1.0)*pz*x888))+((j0*x888)));
1263 evalcond[3]=((-1.0e-9)+(((-1.0)*x889*x890))+x892+x891);
1264 evalcond[4]=((3.6e-10)+(((-0.36)*x891))+(((-0.36)*x892))+(((0.36)*j0*x889)));
1265 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1266 {
1267 continue;
1268 }
1269 }
1270 
1271 rotationfunction0(solutions);
1272 }
1273 }
1274 
1275 }
1276 } while(0);
1277 if( bgotonextstatement )
1278 {
1279 bool bgotonextstatement = true;
1280 do
1281 {
1282 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.62181782312656)+j3)))), 6.28318530717959)));
1283 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1284 {
1285 bgotonextstatement=false;
1286 {
1287 IkReal j1array[1], cj1array[1], sj1array[1];
1288 bool j1valid[1]={false};
1289 _nj1 = 1;
1290 if( IKabs(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((5317165966.79048)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0))))+IKsqr(((5317165966.79048)*py))-1) <= IKFAST_SINCOS_THRESH )
1291  continue;
1292 j1array[0]=IKatan2(((((5317165966.79048)*pz))+(((-5317165966.79048)*j0))), ((5317165966.79048)*py));
1293 sj1array[0]=IKsin(j1array[0]);
1294 cj1array[0]=IKcos(j1array[0]);
1295 if( j1array[0] > IKPI )
1296 {
1297  j1array[0]-=IK2PI;
1298 }
1299 else if( j1array[0] < -IKPI )
1300 { j1array[0]+=IK2PI;
1301 }
1302 j1valid[0] = true;
1303 for(int ij1 = 0; ij1 < 1; ++ij1)
1304 {
1305 if( !j1valid[ij1] )
1306 {
1307  continue;
1308 }
1309 _ij1[0] = ij1; _ij1[1] = -1;
1310 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1311 {
1312 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1313 {
1314  j1valid[iij1]=false; _ij1[1] = iij1; break;
1315 }
1316 }
1317 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1318 {
1319 IkReal evalcond[5];
1320 IkReal x893=IKcos(j1);
1321 IkReal x894=IKsin(j1);
1322 IkReal x895=((1.0)*j0);
1323 IkReal x896=(py*x893);
1324 IkReal x897=((0.36)*x894);
1325 evalcond[0]=((((-1.88070112207465e-10)*x893))+py);
1326 evalcond[1]=((((-1.88070112207465e-10)*x894))+(((-1.0)*x895))+pz);
1327 evalcond[2]=(((py*x894))+(((-1.0)*pz*x893))+((j0*x893)));
1328 evalcond[3]=((-1.88070112207465e-10)+((pz*x894))+(((-1.0)*x894*x895))+x896);
1329 evalcond[4]=((6.77052403946874e-11)+(((-1.0)*pz*x897))+(((-0.36)*x896))+((j0*x897)));
1330 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1331 {
1332 continue;
1333 }
1334 }
1335 
1336 rotationfunction0(solutions);
1337 }
1338 }
1339 
1340 }
1341 } while(0);
1342 if( bgotonextstatement )
1343 {
1344 bool bgotonextstatement = true;
1345 do
1346 {
1347 IkReal x898=IKcos(pz);
1348 IkReal x899=IKsin(pz);
1349 if((((-1.0)*(py*py))) < -0.00001)
1350 continue;
1351 IkReal x900=IKsqrt(((-1.0)*(py*py)));
1352 IkReal x901=IKcos(x900);
1353 IkReal x902=IKsin(x900);
1354 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1355 continue;
1356 IkReal gconst6=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1357 IkReal gconst7=(((x899*x901))+((x898*x902)));
1358 IkReal gconst8=(((x898*x901))+(((-1.0)*x899*x902)));
1359 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1360 continue;
1361 evalcond[0]=IKabs(((((-1.0)*pz))+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
1362 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1363 {
1364 bgotonextstatement=false;
1365 {
1366 IkReal j1eval[1];
1367 IkReal x903=IKcos(pz);
1368 IkReal x904=IKsin(pz);
1369 IkReal x905=x900;
1370 IkReal x906=IKcos(x905);
1371 IkReal x907=IKsin(x905);
1372 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1373 continue;
1374 IkReal x908=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1375 sj2=-1.0;
1376 cj2=0;
1377 j2=-1.5707963267949;
1378 sj0=gconst7;
1379 cj0=gconst8;
1380 j0=x908;
1381 IkReal gconst6=x908;
1382 IkReal gconst7=(((x903*x907))+((x904*x906)));
1383 IkReal gconst8=(((x903*x906))+(((-1.0)*x904*x907)));
1384 j1eval[0]=IKabs(((-1.5)+cj3+(((-5.16666666666667)*sj3))+(((8.33333333333333)*py))));
1385 if( IKabs(j1eval[0]) < 0.0000000100000000 )
1386 {
1387 continue; // no branches [j1]
1388 
1389 } else
1390 {
1391 IkReal op[2+1], zeror[2];
1392 int numroots;
1393 IkReal x909=((5.16666666666667)*sj3);
1394 IkReal x910=((8.33333333333333)*py);
1395 op[0]=((-1.5)+cj3+(((-1.0)*x909))+x910);
1396 op[1]=0;
1397 op[2]=((1.5)+x910+x909+(((-1.0)*cj3)));
1398 polyroots2(op,zeror,numroots);
1399 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1400 int numsolutions = 0;
1401 for(int ij1 = 0; ij1 < numroots; ++ij1)
1402 {
1403 IkReal htj1 = zeror[ij1];
1404 tempj1array[0]=((2.0)*(atan(htj1)));
1405 for(int kj1 = 0; kj1 < 1; ++kj1)
1406 {
1407 j1array[numsolutions] = tempj1array[kj1];
1408 if( j1array[numsolutions] > IKPI )
1409 {
1410  j1array[numsolutions]-=IK2PI;
1411 }
1412 else if( j1array[numsolutions] < -IKPI )
1413 {
1414  j1array[numsolutions]+=IK2PI;
1415 }
1416 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
1417 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
1418 numsolutions++;
1419 }
1420 }
1421 bool j1valid[2]={true,true};
1422 _nj1 = 2;
1423 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
1424  {
1425 if( !j1valid[ij1] )
1426 {
1427  continue;
1428 }
1429  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1430 htj1 = IKtan(j1/2);
1431 
1432 _ij1[0] = ij1; _ij1[1] = -1;
1433 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1434 {
1435 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1436 {
1437  j1valid[iij1]=false; _ij1[1] = iij1; break;
1438 }
1439 }
1440 rotationfunction0(solutions);
1441  }
1442 
1443 }
1444 
1445 }
1446 
1447 }
1448 } while(0);
1449 if( bgotonextstatement )
1450 {
1451 bool bgotonextstatement = true;
1452 do
1453 {
1454 IkReal x911=IKcos(pz);
1455 IkReal x912=IKsin(pz);
1456 if((((-1.0)*(py*py))) < -0.00001)
1457 continue;
1458 IkReal x913=IKsqrt(((-1.0)*(py*py)));
1459 IkReal x914=IKcos(x913);
1460 IkReal x915=IKsin(x913);
1461 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1462 continue;
1463 IkReal gconst9=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1464 IkReal gconst10=((((-1.0)*x911*x915))+((x912*x914)));
1465 IkReal gconst11=(((x911*x914))+((x912*x915)));
1466 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1467 continue;
1468 evalcond[0]=IKabs(((((-1.0)*pz))+(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
1469 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1470 {
1471 bgotonextstatement=false;
1472 {
1473 IkReal j1eval[1];
1474 IkReal x916=IKcos(pz);
1475 IkReal x917=IKsin(pz);
1476 IkReal x918=x913;
1477 IkReal x919=IKcos(x918);
1478 IkReal x920=IKsin(x918);
1479 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1480 continue;
1481 IkReal x921=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1482 sj2=-1.0;
1483 cj2=0;
1484 j2=-1.5707963267949;
1485 sj0=gconst10;
1486 cj0=gconst11;
1487 j0=x921;
1488 IkReal gconst9=x921;
1489 IkReal gconst10=(((x917*x919))+(((-1.0)*x916*x920)));
1490 IkReal gconst11=(((x917*x920))+((x916*x919)));
1491 j1eval[0]=IKabs(((-1.5)+cj3+(((-5.16666666666667)*sj3))+(((8.33333333333333)*py))));
1492 if( IKabs(j1eval[0]) < 0.0000000100000000 )
1493 {
1494 continue; // no branches [j1]
1495 
1496 } else
1497 {
1498 IkReal op[2+1], zeror[2];
1499 int numroots;
1500 IkReal x922=((5.16666666666667)*sj3);
1501 IkReal x923=((8.33333333333333)*py);
1502 op[0]=((-1.5)+(((-1.0)*x922))+cj3+x923);
1503 op[1]=0;
1504 op[2]=((1.5)+x922+x923+(((-1.0)*cj3)));
1505 polyroots2(op,zeror,numroots);
1506 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1507 int numsolutions = 0;
1508 for(int ij1 = 0; ij1 < numroots; ++ij1)
1509 {
1510 IkReal htj1 = zeror[ij1];
1511 tempj1array[0]=((2.0)*(atan(htj1)));
1512 for(int kj1 = 0; kj1 < 1; ++kj1)
1513 {
1514 j1array[numsolutions] = tempj1array[kj1];
1515 if( j1array[numsolutions] > IKPI )
1516 {
1517  j1array[numsolutions]-=IK2PI;
1518 }
1519 else if( j1array[numsolutions] < -IKPI )
1520 {
1521  j1array[numsolutions]+=IK2PI;
1522 }
1523 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
1524 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
1525 numsolutions++;
1526 }
1527 }
1528 bool j1valid[2]={true,true};
1529 _nj1 = 2;
1530 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
1531  {
1532 if( !j1valid[ij1] )
1533 {
1534  continue;
1535 }
1536  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1537 htj1 = IKtan(j1/2);
1538 
1539 _ij1[0] = ij1; _ij1[1] = -1;
1540 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1541 {
1542 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1543 {
1544  j1valid[iij1]=false; _ij1[1] = iij1; break;
1545 }
1546 }
1547 rotationfunction0(solutions);
1548  }
1549 
1550 }
1551 
1552 }
1553 
1554 }
1555 } while(0);
1556 if( bgotonextstatement )
1557 {
1558 bool bgotonextstatement = true;
1559 do
1560 {
1561 if( 1 )
1562 {
1563 bgotonextstatement=false;
1564 continue; // branch miss [j1]
1565 
1566 }
1567 } while(0);
1568 if( bgotonextstatement )
1569 {
1570 }
1571 }
1572 }
1573 }
1574 }
1575 }
1576 
1577 } else
1578 {
1579 {
1580 IkReal j1array[1], cj1array[1], sj1array[1];
1581 bool j1valid[1]={false};
1582 _nj1 = 1;
1583 IkReal x924=((0.62)*sj3);
1584 IkReal x925=((0.12)*cj3);
1585 CheckValue<IkReal> x926=IKPowWithIntegerCheck(((0.18)+(((-1.0)*x925))+x924),-1);
1586 if(!x926.valid){
1587 continue;
1588 }
1589 CheckValue<IkReal> x927=IKPowWithIntegerCheck((((j0*x924))+(((-0.18)*pz))+(((0.18)*j0))+(((-1.0)*j0*x925))+((pz*x925))+(((-1.0)*pz*x924))),-1);
1590 if(!x927.valid){
1591 continue;
1592 }
1593 if( IKabs(((x926.value)*(((((-1.0)*pz))+j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x927.value)*(((((-1.0)*j0*py))+((py*pz)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x926.value)*(((((-1.0)*pz))+j0))))+IKsqr(((x927.value)*(((((-1.0)*j0*py))+((py*pz))))))-1) <= IKFAST_SINCOS_THRESH )
1594  continue;
1595 j1array[0]=IKatan2(((x926.value)*(((((-1.0)*pz))+j0))), ((x927.value)*(((((-1.0)*j0*py))+((py*pz))))));
1596 sj1array[0]=IKsin(j1array[0]);
1597 cj1array[0]=IKcos(j1array[0]);
1598 if( j1array[0] > IKPI )
1599 {
1600  j1array[0]-=IK2PI;
1601 }
1602 else if( j1array[0] < -IKPI )
1603 { j1array[0]+=IK2PI;
1604 }
1605 j1valid[0] = true;
1606 for(int ij1 = 0; ij1 < 1; ++ij1)
1607 {
1608 if( !j1valid[ij1] )
1609 {
1610  continue;
1611 }
1612 _ij1[0] = ij1; _ij1[1] = -1;
1613 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1614 {
1615 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1616 {
1617  j1valid[iij1]=false; _ij1[1] = iij1; break;
1618 }
1619 }
1620 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1621 {
1622 IkReal evalcond[5];
1623 IkReal x928=IKcos(j1);
1624 IkReal x929=IKsin(j1);
1625 IkReal x930=((0.62)*sj3);
1626 IkReal x931=((0.12)*cj3);
1627 IkReal x932=((1.0)*j0);
1628 IkReal x933=(py*x928);
1629 IkReal x934=(pz*x929);
1630 evalcond[0]=(((j0*x928))+((py*x929))+(((-1.0)*pz*x928)));
1631 evalcond[1]=((((-1.0)*x928*x931))+((x928*x930))+(((0.18)*x928))+py);
1632 evalcond[2]=((((0.18)*x929))+pz+(((-1.0)*x929*x931))+((x929*x930))+(((-1.0)*x932)));
1633 evalcond[3]=((0.18)+(((-1.0)*x929*x932))+x934+x930+x933+(((-1.0)*x931)));
1634 evalcond[4]=((-0.0648)+(((-0.2232)*sj3))+(((0.36)*j0*x929))+(((-0.36)*x934))+(((-0.36)*x933))+(((0.0432)*cj3)));
1635 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1636 {
1637 continue;
1638 }
1639 }
1640 
1641 rotationfunction0(solutions);
1642 }
1643 }
1644 
1645 }
1646 
1647 }
1648 
1649 } else
1650 {
1651 {
1652 IkReal j1array[1], cj1array[1], sj1array[1];
1653 bool j1valid[1]={false};
1654 _nj1 = 1;
1655 IkReal x935=((0.62)*sj3);
1656 IkReal x936=((0.12)*cj3);
1657 CheckValue<IkReal> x937=IKPowWithIntegerCheck(IKsign(((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)))),-1);
1658 if(!x937.valid){
1659 continue;
1660 }
1661 CheckValue<IkReal> x938 = IKatan2WithCheck(IkReal((((j0*x935))+(((-0.18)*pz))+(((0.18)*j0))+(((-1.0)*j0*x936))+((pz*x936))+(((-1.0)*pz*x935)))),IkReal(((((-0.18)*py))+((py*x936))+(((-1.0)*py*x935)))),IKFAST_ATAN2_MAGTHRESH);
1662 if(!x938.valid){
1663 continue;
1664 }
1665 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x937.value)))+(x938.value));
1666 sj1array[0]=IKsin(j1array[0]);
1667 cj1array[0]=IKcos(j1array[0]);
1668 if( j1array[0] > IKPI )
1669 {
1670  j1array[0]-=IK2PI;
1671 }
1672 else if( j1array[0] < -IKPI )
1673 { j1array[0]+=IK2PI;
1674 }
1675 j1valid[0] = true;
1676 for(int ij1 = 0; ij1 < 1; ++ij1)
1677 {
1678 if( !j1valid[ij1] )
1679 {
1680  continue;
1681 }
1682 _ij1[0] = ij1; _ij1[1] = -1;
1683 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1684 {
1685 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1686 {
1687  j1valid[iij1]=false; _ij1[1] = iij1; break;
1688 }
1689 }
1690 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1691 {
1692 IkReal evalcond[5];
1693 IkReal x939=IKcos(j1);
1694 IkReal x940=IKsin(j1);
1695 IkReal x941=((0.62)*sj3);
1696 IkReal x942=((0.12)*cj3);
1697 IkReal x943=((1.0)*j0);
1698 IkReal x944=(py*x939);
1699 IkReal x945=(pz*x940);
1700 evalcond[0]=(((j0*x939))+(((-1.0)*pz*x939))+((py*x940)));
1701 evalcond[1]=((((-1.0)*x939*x942))+((x939*x941))+(((0.18)*x939))+py);
1702 evalcond[2]=((((-1.0)*x943))+(((0.18)*x940))+pz+((x940*x941))+(((-1.0)*x940*x942)));
1703 evalcond[3]=((0.18)+(((-1.0)*x942))+x941+x944+x945+(((-1.0)*x940*x943)));
1704 evalcond[4]=((-0.0648)+(((-0.2232)*sj3))+(((-0.36)*x944))+(((-0.36)*x945))+(((0.36)*j0*x940))+(((0.0432)*cj3)));
1705 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1706 {
1707 continue;
1708 }
1709 }
1710 
1711 rotationfunction0(solutions);
1712 }
1713 }
1714 
1715 }
1716 
1717 }
1718 
1719 } else
1720 {
1721 {
1722 IkReal j1array[1], cj1array[1], sj1array[1];
1723 bool j1valid[1]={false};
1724 _nj1 = 1;
1725 CheckValue<IkReal> x946 = IKatan2WithCheck(IkReal(((((-1.0)*pz))+j0)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
1726 if(!x946.valid){
1727 continue;
1728 }
1729 CheckValue<IkReal> x947=IKPowWithIntegerCheck(IKsign(((0.18)+(((0.62)*sj3))+(((-0.12)*cj3)))),-1);
1730 if(!x947.valid){
1731 continue;
1732 }
1733 j1array[0]=((-1.5707963267949)+(x946.value)+(((1.5707963267949)*(x947.value))));
1734 sj1array[0]=IKsin(j1array[0]);
1735 cj1array[0]=IKcos(j1array[0]);
1736 if( j1array[0] > IKPI )
1737 {
1738  j1array[0]-=IK2PI;
1739 }
1740 else if( j1array[0] < -IKPI )
1741 { j1array[0]+=IK2PI;
1742 }
1743 j1valid[0] = true;
1744 for(int ij1 = 0; ij1 < 1; ++ij1)
1745 {
1746 if( !j1valid[ij1] )
1747 {
1748  continue;
1749 }
1750 _ij1[0] = ij1; _ij1[1] = -1;
1751 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
1752 {
1753 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1754 {
1755  j1valid[iij1]=false; _ij1[1] = iij1; break;
1756 }
1757 }
1758 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1759 {
1760 IkReal evalcond[5];
1761 IkReal x948=IKcos(j1);
1762 IkReal x949=IKsin(j1);
1763 IkReal x950=((0.62)*sj3);
1764 IkReal x951=((0.12)*cj3);
1765 IkReal x952=((1.0)*j0);
1766 IkReal x953=(py*x948);
1767 IkReal x954=(pz*x949);
1768 evalcond[0]=((((-1.0)*pz*x948))+((py*x949))+((j0*x948)));
1769 evalcond[1]=((((0.18)*x948))+((x948*x950))+py+(((-1.0)*x948*x951)));
1770 evalcond[2]=((((0.18)*x949))+((x949*x950))+pz+(((-1.0)*x949*x951))+(((-1.0)*x952)));
1771 evalcond[3]=((0.18)+(((-1.0)*x949*x952))+(((-1.0)*x951))+x954+x953+x950);
1772 evalcond[4]=((-0.0648)+(((-0.2232)*sj3))+(((0.36)*j0*x949))+(((-0.36)*x953))+(((-0.36)*x954))+(((0.0432)*cj3)));
1773 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
1774 {
1775 continue;
1776 }
1777 }
1778 
1779 rotationfunction0(solutions);
1780 }
1781 }
1782 
1783 }
1784 
1785 }
1786 
1787 }
1788 } while(0);
1789 if( bgotonextstatement )
1790 {
1791 bool bgotonextstatement = true;
1792 do
1793 {
1794 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j2))), 6.28318530717959)));
1795 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1796 {
1797 bgotonextstatement=false;
1798 {
1799 IkReal j1eval[3];
1800 sj2=0;
1801 cj2=1.0;
1802 j2=0;
1803 j1eval[0]=((6.5)+sj3+(((5.16666666666667)*cj3)));
1804 j1eval[1]=IKsign(((0.78)+(((0.62)*cj3))+(((0.12)*sj3))));
1805 j1eval[2]=((IKabs(py))+(IKabs(((((-1.0)*pz))+j0))));
1806 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
1807 {
1808 {
1809 IkReal j1eval[3];
1810 sj2=0;
1811 cj2=1.0;
1812 j2=0;
1813 IkReal x955=((0.62)*cj3);
1814 IkReal x956=((0.12)*sj3);
1815 IkReal x957=((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)));
1816 j1eval[0]=x957;
1817 j1eval[1]=((((0.02)*(IKabs(((((-39.0)*py))+(((-31.0)*cj3*py))+(((-6.0)*py*sj3)))))))+(IKabs(((((-0.78)*pz))+(((-1.0)*pz*x955))+(((-1.0)*pz*x956))+(((0.78)*j0))+((j0*x955))+((j0*x956))))));
1818 j1eval[2]=IKsign(x957);
1819 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
1820 {
1821 {
1822 IkReal j1eval[2];
1823 sj2=0;
1824 cj2=1.0;
1825 j2=0;
1826 IkReal x958=((5.16666666666667)*cj3);
1827 j1eval[0]=((6.5)+sj3+x958);
1828 j1eval[1]=((((-1.0)*pz*x958))+(((6.5)*j0))+(((-6.5)*pz))+(((-1.0)*pz*sj3))+((j0*sj3))+((j0*x958)));
1829 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
1830 {
1831 {
1832 IkReal evalcond[1];
1833 bool bgotonextstatement = true;
1834 do
1835 {
1836 IkReal x959=IKcos(pz);
1837 IkReal x960=IKsin(pz);
1838 if((((-1.0)*(py*py))) < -0.00001)
1839 continue;
1840 IkReal x961=IKsqrt(((-1.0)*(py*py)));
1841 IkReal x962=IKcos(x961);
1842 IkReal x963=IKsin(x961);
1843 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1844 continue;
1845 IkReal gconst12=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1846 IkReal gconst13=(((x960*x962))+((x959*x963)));
1847 IkReal gconst14=((((-1.0)*x960*x963))+((x959*x962)));
1848 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1849 continue;
1850 evalcond[0]=IKabs(((((-1.0)*pz))+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
1851 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1852 {
1853 bgotonextstatement=false;
1854 {
1855 IkReal j1eval[1];
1856 IkReal x964=IKcos(pz);
1857 IkReal x965=IKsin(pz);
1858 IkReal x966=x961;
1859 IkReal x967=IKcos(x966);
1860 IkReal x968=IKsin(x966);
1861 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1862 continue;
1863 IkReal x969=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1864 sj2=0;
1865 cj2=1.0;
1866 j2=0;
1867 sj0=gconst13;
1868 cj0=gconst14;
1869 j0=x969;
1870 IkReal gconst12=x969;
1871 IkReal gconst13=(((x965*x967))+((x964*x968)));
1872 IkReal gconst14=((((-1.0)*x965*x968))+((x964*x967)));
1873 j1eval[0]=IKabs(((-6.5)+(((-1.0)*sj3))+(((8.33333333333333)*py))+(((-5.16666666666667)*cj3))));
1874 if( IKabs(j1eval[0]) < 0.0000000100000000 )
1875 {
1876 continue; // no branches [j1]
1877 
1878 } else
1879 {
1880 IkReal op[2+1], zeror[2];
1881 int numroots;
1882 IkReal x970=((8.33333333333333)*py);
1883 IkReal x971=((5.16666666666667)*cj3);
1884 op[0]=((-6.5)+(((-1.0)*sj3))+x970+(((-1.0)*x971)));
1885 op[1]=0;
1886 op[2]=((6.5)+sj3+x971+x970);
1887 polyroots2(op,zeror,numroots);
1888 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1889 int numsolutions = 0;
1890 for(int ij1 = 0; ij1 < numroots; ++ij1)
1891 {
1892 IkReal htj1 = zeror[ij1];
1893 tempj1array[0]=((2.0)*(atan(htj1)));
1894 for(int kj1 = 0; kj1 < 1; ++kj1)
1895 {
1896 j1array[numsolutions] = tempj1array[kj1];
1897 if( j1array[numsolutions] > IKPI )
1898 {
1899  j1array[numsolutions]-=IK2PI;
1900 }
1901 else if( j1array[numsolutions] < -IKPI )
1902 {
1903  j1array[numsolutions]+=IK2PI;
1904 }
1905 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
1906 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
1907 numsolutions++;
1908 }
1909 }
1910 bool j1valid[2]={true,true};
1911 _nj1 = 2;
1912 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
1913  {
1914 if( !j1valid[ij1] )
1915 {
1916  continue;
1917 }
1918  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1919 htj1 = IKtan(j1/2);
1920 
1921 _ij1[0] = ij1; _ij1[1] = -1;
1922 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1923 {
1924 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
1925 {
1926  j1valid[iij1]=false; _ij1[1] = iij1; break;
1927 }
1928 }
1929 rotationfunction0(solutions);
1930  }
1931 
1932 }
1933 
1934 }
1935 
1936 }
1937 } while(0);
1938 if( bgotonextstatement )
1939 {
1940 bool bgotonextstatement = true;
1941 do
1942 {
1943 IkReal x972=IKcos(pz);
1944 IkReal x973=IKsin(pz);
1945 if((((-1.0)*(py*py))) < -0.00001)
1946 continue;
1947 IkReal x974=IKsqrt(((-1.0)*(py*py)));
1948 IkReal x975=IKcos(x974);
1949 IkReal x976=IKsin(x974);
1950 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1951 continue;
1952 IkReal gconst15=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1953 IkReal gconst16=(((x973*x975))+(((-1.0)*x972*x976)));
1954 IkReal gconst17=(((x973*x976))+((x972*x975)));
1955 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1956 continue;
1957 evalcond[0]=IKabs(((((-1.0)*pz))+(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
1958 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1959 {
1960 bgotonextstatement=false;
1961 {
1962 IkReal j1eval[1];
1963 IkReal x977=IKcos(pz);
1964 IkReal x978=IKsin(pz);
1965 IkReal x979=x974;
1966 IkReal x980=IKcos(x979);
1967 IkReal x981=IKsin(x979);
1968 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1969 continue;
1970 IkReal x982=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1971 sj2=0;
1972 cj2=1.0;
1973 j2=0;
1974 sj0=gconst16;
1975 cj0=gconst17;
1976 j0=x982;
1977 IkReal gconst15=x982;
1978 IkReal gconst16=((((-1.0)*x977*x981))+((x978*x980)));
1979 IkReal gconst17=(((x977*x980))+((x978*x981)));
1980 j1eval[0]=IKabs(((-6.5)+(((-1.0)*sj3))+(((8.33333333333333)*py))+(((-5.16666666666667)*cj3))));
1981 if( IKabs(j1eval[0]) < 0.0000000100000000 )
1982 {
1983 continue; // no branches [j1]
1984 
1985 } else
1986 {
1987 IkReal op[2+1], zeror[2];
1988 int numroots;
1989 IkReal x983=((8.33333333333333)*py);
1990 IkReal x984=((5.16666666666667)*cj3);
1991 op[0]=((-6.5)+(((-1.0)*x984))+(((-1.0)*sj3))+x983);
1992 op[1]=0;
1993 op[2]=((6.5)+sj3+x984+x983);
1994 polyroots2(op,zeror,numroots);
1995 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1996 int numsolutions = 0;
1997 for(int ij1 = 0; ij1 < numroots; ++ij1)
1998 {
1999 IkReal htj1 = zeror[ij1];
2000 tempj1array[0]=((2.0)*(atan(htj1)));
2001 for(int kj1 = 0; kj1 < 1; ++kj1)
2002 {
2003 j1array[numsolutions] = tempj1array[kj1];
2004 if( j1array[numsolutions] > IKPI )
2005 {
2006  j1array[numsolutions]-=IK2PI;
2007 }
2008 else if( j1array[numsolutions] < -IKPI )
2009 {
2010  j1array[numsolutions]+=IK2PI;
2011 }
2012 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
2013 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
2014 numsolutions++;
2015 }
2016 }
2017 bool j1valid[2]={true,true};
2018 _nj1 = 2;
2019 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
2020  {
2021 if( !j1valid[ij1] )
2022 {
2023  continue;
2024 }
2025  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2026 htj1 = IKtan(j1/2);
2027 
2028 _ij1[0] = ij1; _ij1[1] = -1;
2029 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2030 {
2031 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2032 {
2033  j1valid[iij1]=false; _ij1[1] = iij1; break;
2034 }
2035 }
2036 rotationfunction0(solutions);
2037  }
2038 
2039 }
2040 
2041 }
2042 
2043 }
2044 } while(0);
2045 if( bgotonextstatement )
2046 {
2047 bool bgotonextstatement = true;
2048 do
2049 {
2050 if( 1 )
2051 {
2052 bgotonextstatement=false;
2053 continue; // branch miss [j1]
2054 
2055 }
2056 } while(0);
2057 if( bgotonextstatement )
2058 {
2059 }
2060 }
2061 }
2062 }
2063 
2064 } else
2065 {
2066 {
2067 IkReal j1array[1], cj1array[1], sj1array[1];
2068 bool j1valid[1]={false};
2069 _nj1 = 1;
2070 IkReal x985=((0.62)*cj3);
2071 IkReal x986=((0.12)*sj3);
2072 CheckValue<IkReal> x987=IKPowWithIntegerCheck(((0.78)+x985+x986),-1);
2073 if(!x987.valid){
2074 continue;
2075 }
2076 CheckValue<IkReal> x988=IKPowWithIntegerCheck(((((-0.78)*pz))+(((-1.0)*pz*x985))+(((-1.0)*pz*x986))+((j0*x985))+((j0*x986))+(((0.78)*j0))),-1);
2077 if(!x988.valid){
2078 continue;
2079 }
2080 if( IKabs(((x987.value)*(((((-1.0)*pz))+j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x988.value)*(((((-1.0)*j0*py))+((py*pz)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x987.value)*(((((-1.0)*pz))+j0))))+IKsqr(((x988.value)*(((((-1.0)*j0*py))+((py*pz))))))-1) <= IKFAST_SINCOS_THRESH )
2081  continue;
2082 j1array[0]=IKatan2(((x987.value)*(((((-1.0)*pz))+j0))), ((x988.value)*(((((-1.0)*j0*py))+((py*pz))))));
2083 sj1array[0]=IKsin(j1array[0]);
2084 cj1array[0]=IKcos(j1array[0]);
2085 if( j1array[0] > IKPI )
2086 {
2087  j1array[0]-=IK2PI;
2088 }
2089 else if( j1array[0] < -IKPI )
2090 { j1array[0]+=IK2PI;
2091 }
2092 j1valid[0] = true;
2093 for(int ij1 = 0; ij1 < 1; ++ij1)
2094 {
2095 if( !j1valid[ij1] )
2096 {
2097  continue;
2098 }
2099 _ij1[0] = ij1; _ij1[1] = -1;
2100 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2101 {
2102 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2103 {
2104  j1valid[iij1]=false; _ij1[1] = iij1; break;
2105 }
2106 }
2107 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2108 {
2109 IkReal evalcond[5];
2110 IkReal x989=IKcos(j1);
2111 IkReal x990=IKsin(j1);
2112 IkReal x991=((1.0)*j0);
2113 IkReal x992=((0.62)*cj3);
2114 IkReal x993=((0.12)*sj3);
2115 IkReal x994=(py*x989);
2116 IkReal x995=(pz*x990);
2117 evalcond[0]=(((py*x990))+(((-1.0)*pz*x989))+((j0*x989)));
2118 evalcond[1]=(((x989*x993))+((x989*x992))+py+(((0.78)*x989)));
2119 evalcond[2]=(((x990*x992))+((x990*x993))+(((-1.0)*x991))+pz+(((0.78)*x990)));
2120 evalcond[3]=((0.78)+(((-1.0)*x990*x991))+x993+x992+x995+x994);
2121 evalcond[4]=((-1.2168)+(((-1.56)*x995))+(((-1.56)*x994))+(((-0.1872)*sj3))+(((-0.9672)*cj3))+(((1.56)*j0*x990)));
2122 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2123 {
2124 continue;
2125 }
2126 }
2127 
2128 rotationfunction0(solutions);
2129 }
2130 }
2131 
2132 }
2133 
2134 }
2135 
2136 } else
2137 {
2138 {
2139 IkReal j1array[1], cj1array[1], sj1array[1];
2140 bool j1valid[1]={false};
2141 _nj1 = 1;
2142 IkReal x996=((0.62)*cj3);
2143 IkReal x997=((0.12)*sj3);
2144 CheckValue<IkReal> x998=IKPowWithIntegerCheck(IKsign(((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)))),-1);
2145 if(!x998.valid){
2146 continue;
2147 }
2148 CheckValue<IkReal> x999 = IKatan2WithCheck(IkReal(((((-0.78)*pz))+(((-1.0)*pz*x996))+(((-1.0)*pz*x997))+((j0*x997))+((j0*x996))+(((0.78)*j0)))),IkReal(((((-0.78)*py))+(((-1.0)*py*x996))+(((-1.0)*py*x997)))),IKFAST_ATAN2_MAGTHRESH);
2149 if(!x999.valid){
2150 continue;
2151 }
2152 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x998.value)))+(x999.value));
2153 sj1array[0]=IKsin(j1array[0]);
2154 cj1array[0]=IKcos(j1array[0]);
2155 if( j1array[0] > IKPI )
2156 {
2157  j1array[0]-=IK2PI;
2158 }
2159 else if( j1array[0] < -IKPI )
2160 { j1array[0]+=IK2PI;
2161 }
2162 j1valid[0] = true;
2163 for(int ij1 = 0; ij1 < 1; ++ij1)
2164 {
2165 if( !j1valid[ij1] )
2166 {
2167  continue;
2168 }
2169 _ij1[0] = ij1; _ij1[1] = -1;
2170 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2171 {
2172 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2173 {
2174  j1valid[iij1]=false; _ij1[1] = iij1; break;
2175 }
2176 }
2177 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2178 {
2179 IkReal evalcond[5];
2180 IkReal x1000=IKcos(j1);
2181 IkReal x1001=IKsin(j1);
2182 IkReal x1002=((1.0)*j0);
2183 IkReal x1003=((0.62)*cj3);
2184 IkReal x1004=((0.12)*sj3);
2185 IkReal x1005=(py*x1000);
2186 IkReal x1006=(pz*x1001);
2187 evalcond[0]=(((j0*x1000))+(((-1.0)*pz*x1000))+((py*x1001)));
2188 evalcond[1]=(((x1000*x1003))+((x1000*x1004))+py+(((0.78)*x1000)));
2189 evalcond[2]=((((-1.0)*x1002))+pz+(((0.78)*x1001))+((x1001*x1004))+((x1001*x1003)));
2190 evalcond[3]=((0.78)+x1006+x1005+x1004+x1003+(((-1.0)*x1001*x1002)));
2191 evalcond[4]=((-1.2168)+(((1.56)*j0*x1001))+(((-0.1872)*sj3))+(((-1.56)*x1006))+(((-1.56)*x1005))+(((-0.9672)*cj3)));
2192 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2193 {
2194 continue;
2195 }
2196 }
2197 
2198 rotationfunction0(solutions);
2199 }
2200 }
2201 
2202 }
2203 
2204 }
2205 
2206 } else
2207 {
2208 {
2209 IkReal j1array[1], cj1array[1], sj1array[1];
2210 bool j1valid[1]={false};
2211 _nj1 = 1;
2212 CheckValue<IkReal> x1007 = IKatan2WithCheck(IkReal(((((-1.0)*pz))+j0)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
2213 if(!x1007.valid){
2214 continue;
2215 }
2216 CheckValue<IkReal> x1008=IKPowWithIntegerCheck(IKsign(((0.78)+(((0.62)*cj3))+(((0.12)*sj3)))),-1);
2217 if(!x1008.valid){
2218 continue;
2219 }
2220 j1array[0]=((-1.5707963267949)+(x1007.value)+(((1.5707963267949)*(x1008.value))));
2221 sj1array[0]=IKsin(j1array[0]);
2222 cj1array[0]=IKcos(j1array[0]);
2223 if( j1array[0] > IKPI )
2224 {
2225  j1array[0]-=IK2PI;
2226 }
2227 else if( j1array[0] < -IKPI )
2228 { j1array[0]+=IK2PI;
2229 }
2230 j1valid[0] = true;
2231 for(int ij1 = 0; ij1 < 1; ++ij1)
2232 {
2233 if( !j1valid[ij1] )
2234 {
2235  continue;
2236 }
2237 _ij1[0] = ij1; _ij1[1] = -1;
2238 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2239 {
2240 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2241 {
2242  j1valid[iij1]=false; _ij1[1] = iij1; break;
2243 }
2244 }
2245 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2246 {
2247 IkReal evalcond[5];
2248 IkReal x1009=IKcos(j1);
2249 IkReal x1010=IKsin(j1);
2250 IkReal x1011=((1.0)*j0);
2251 IkReal x1012=((0.62)*cj3);
2252 IkReal x1013=((0.12)*sj3);
2253 IkReal x1014=(py*x1009);
2254 IkReal x1015=(pz*x1010);
2255 evalcond[0]=(((j0*x1009))+(((-1.0)*pz*x1009))+((py*x1010)));
2256 evalcond[1]=(((x1009*x1012))+((x1009*x1013))+py+(((0.78)*x1009)));
2257 evalcond[2]=((((-1.0)*x1011))+pz+(((0.78)*x1010))+((x1010*x1013))+((x1010*x1012)));
2258 evalcond[3]=((0.78)+(((-1.0)*x1010*x1011))+x1014+x1015+x1012+x1013);
2259 evalcond[4]=((-1.2168)+(((-1.56)*x1015))+(((-1.56)*x1014))+(((-0.1872)*sj3))+(((1.56)*j0*x1010))+(((-0.9672)*cj3)));
2260 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2261 {
2262 continue;
2263 }
2264 }
2265 
2266 rotationfunction0(solutions);
2267 }
2268 }
2269 
2270 }
2271 
2272 }
2273 
2274 }
2275 } while(0);
2276 if( bgotonextstatement )
2277 {
2278 bool bgotonextstatement = true;
2279 do
2280 {
2281 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j2)))), 6.28318530717959)));
2282 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2283 {
2284 bgotonextstatement=false;
2285 {
2286 IkReal j1eval[3];
2287 sj2=0;
2288 cj2=-1.0;
2289 j2=3.14159265358979;
2290 j1eval[0]=((-3.5)+(((-1.0)*sj3))+(((-5.16666666666667)*cj3)));
2291 j1eval[1]=((IKabs(py))+(IKabs(((((-1.0)*pz))+j0))));
2292 j1eval[2]=IKsign(((-0.42)+(((-0.12)*sj3))+(((-0.62)*cj3))));
2293 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
2294 {
2295 {
2296 IkReal j1eval[3];
2297 sj2=0;
2298 cj2=-1.0;
2299 j2=3.14159265358979;
2300 IkReal x1016=((0.62)*cj3);
2301 IkReal x1017=((0.12)*sj3);
2302 IkReal x1018=((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)));
2303 j1eval[0]=x1018;
2304 j1eval[1]=IKsign(x1018);
2305 j1eval[2]=((IKabs(((((-1.0)*pz*x1016))+(((-1.0)*pz*x1017))+(((0.42)*j0))+(((-0.42)*pz))+((j0*x1016))+((j0*x1017)))))+(((0.02)*(IKabs(((((-31.0)*cj3*py))+(((-6.0)*py*sj3))+(((-21.0)*py))))))));
2306 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
2307 {
2308 {
2309 IkReal j1eval[2];
2310 sj2=0;
2311 cj2=-1.0;
2312 j2=3.14159265358979;
2313 IkReal x1019=((1.0)*sj3);
2314 IkReal x1020=((5.16666666666667)*cj3);
2315 j1eval[0]=((-3.5)+(((-1.0)*x1019))+(((-1.0)*x1020)));
2316 j1eval[1]=((((-1.0)*j0*x1020))+(((-1.0)*j0*x1019))+(((3.5)*pz))+(((-3.5)*j0))+((pz*sj3))+((pz*x1020)));
2317 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
2318 {
2319 {
2320 IkReal evalcond[1];
2321 bool bgotonextstatement = true;
2322 do
2323 {
2324 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-2.48957727181973)+j3)))), 6.28318530717959)));
2325 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2326 {
2327 bgotonextstatement=false;
2328 {
2329 IkReal j1array[1], cj1array[1], sj1array[1];
2330 bool j1valid[1]={false};
2331 _nj1 = 1;
2332 if( IKabs(((((1250000000.0)*pz))+(((-1250000000.0)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((1250000000.0)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((1250000000.0)*pz))+(((-1250000000.0)*j0))))+IKsqr(((1250000000.0)*py))-1) <= IKFAST_SINCOS_THRESH )
2333  continue;
2334 j1array[0]=IKatan2(((((1250000000.0)*pz))+(((-1250000000.0)*j0))), ((1250000000.0)*py));
2335 sj1array[0]=IKsin(j1array[0]);
2336 cj1array[0]=IKcos(j1array[0]);
2337 if( j1array[0] > IKPI )
2338 {
2339  j1array[0]-=IK2PI;
2340 }
2341 else if( j1array[0] < -IKPI )
2342 { j1array[0]+=IK2PI;
2343 }
2344 j1valid[0] = true;
2345 for(int ij1 = 0; ij1 < 1; ++ij1)
2346 {
2347 if( !j1valid[ij1] )
2348 {
2349  continue;
2350 }
2351 _ij1[0] = ij1; _ij1[1] = -1;
2352 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2353 {
2354 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2355 {
2356  j1valid[iij1]=false; _ij1[1] = iij1; break;
2357 }
2358 }
2359 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2360 {
2361 IkReal evalcond[5];
2362 IkReal x1021=IKcos(j1);
2363 IkReal x1022=IKsin(j1);
2364 IkReal x1023=((1.0)*pz);
2365 IkReal x1024=((0.84)*x1022);
2366 IkReal x1025=(py*x1021);
2367 evalcond[0]=(py+(((-8.0e-10)*x1021)));
2368 evalcond[1]=(pz+(((-8.0e-10)*x1022))+(((-1.0)*j0)));
2369 evalcond[2]=(((py*x1022))+((j0*x1021))+(((-1.0)*x1021*x1023)));
2370 evalcond[3]=((8.0e-10)+(((-1.0)*x1025))+((j0*x1022))+(((-1.0)*x1022*x1023)));
2371 evalcond[4]=((-6.72e-10)+(((-1.0)*j0*x1024))+(((0.84)*x1025))+((pz*x1024)));
2372 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2373 {
2374 continue;
2375 }
2376 }
2377 
2378 rotationfunction0(solutions);
2379 }
2380 }
2381 
2382 }
2383 } while(0);
2384 if( bgotonextstatement )
2385 {
2386 bool bgotonextstatement = true;
2387 do
2388 {
2389 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-4.17597695455389)+j3)))), 6.28318530717959)));
2390 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2391 {
2392 bgotonextstatement=false;
2393 {
2394 IkReal j1array[1], cj1array[1], sj1array[1];
2395 bool j1valid[1]={false};
2396 _nj1 = 1;
2397 if( IKabs(((((547354476.351043)*pz))+(((-547354476.351043)*j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((547354476.351043)*py)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((547354476.351043)*pz))+(((-547354476.351043)*j0))))+IKsqr(((547354476.351043)*py))-1) <= IKFAST_SINCOS_THRESH )
2398  continue;
2399 j1array[0]=IKatan2(((((547354476.351043)*pz))+(((-547354476.351043)*j0))), ((547354476.351043)*py));
2400 sj1array[0]=IKsin(j1array[0]);
2401 cj1array[0]=IKcos(j1array[0]);
2402 if( j1array[0] > IKPI )
2403 {
2404  j1array[0]-=IK2PI;
2405 }
2406 else if( j1array[0] < -IKPI )
2407 { j1array[0]+=IK2PI;
2408 }
2409 j1valid[0] = true;
2410 for(int ij1 = 0; ij1 < 1; ++ij1)
2411 {
2412 if( !j1valid[ij1] )
2413 {
2414  continue;
2415 }
2416 _ij1[0] = ij1; _ij1[1] = -1;
2417 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2418 {
2419 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2420 {
2421  j1valid[iij1]=false; _ij1[1] = iij1; break;
2422 }
2423 }
2424 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2425 {
2426 IkReal evalcond[5];
2427 IkReal x1026=IKcos(j1);
2428 IkReal x1027=IKsin(j1);
2429 IkReal x1028=(pz*x1027);
2430 IkReal x1029=(j0*x1027);
2431 IkReal x1030=((1.0)*x1026);
2432 evalcond[0]=((((-1.8269696206132e-9)*x1026))+py);
2433 evalcond[1]=((((-1.8269696206132e-9)*x1027))+pz+(((-1.0)*j0)));
2434 evalcond[2]=(((py*x1027))+(((-1.0)*pz*x1030))+((j0*x1026)));
2435 evalcond[3]=((1.8269696206132e-9)+x1029+(((-1.0)*py*x1030))+(((-1.0)*x1028)));
2436 evalcond[4]=((-1.53465448131508e-9)+(((0.84)*py*x1026))+(((0.84)*x1028))+(((-0.84)*x1029)));
2437 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2438 {
2439 continue;
2440 }
2441 }
2442 
2443 rotationfunction0(solutions);
2444 }
2445 }
2446 
2447 }
2448 } while(0);
2449 if( bgotonextstatement )
2450 {
2451 bool bgotonextstatement = true;
2452 do
2453 {
2454 IkReal x1031=IKcos(pz);
2455 IkReal x1032=IKsin(pz);
2456 if((((-1.0)*(py*py))) < -0.00001)
2457 continue;
2458 IkReal x1033=IKsqrt(((-1.0)*(py*py)));
2459 IkReal x1034=IKcos(x1033);
2460 IkReal x1035=IKsin(x1033);
2461 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2462 continue;
2463 IkReal gconst18=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2464 IkReal gconst19=(((x1031*x1035))+((x1032*x1034)));
2465 IkReal gconst20=(((x1031*x1034))+(((-1.0)*x1032*x1035)));
2466 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2467 continue;
2468 evalcond[0]=IKabs(((((-1.0)*pz))+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
2469 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2470 {
2471 bgotonextstatement=false;
2472 {
2473 IkReal j1eval[1];
2474 IkReal x1036=IKcos(pz);
2475 IkReal x1037=IKsin(pz);
2476 IkReal x1038=x1033;
2477 IkReal x1039=IKcos(x1038);
2478 IkReal x1040=IKsin(x1038);
2479 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2480 continue;
2481 IkReal x1041=((IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2482 sj2=0;
2483 cj2=-1.0;
2484 j2=3.14159265358979;
2485 sj0=gconst19;
2486 cj0=gconst20;
2487 j0=x1041;
2488 IkReal gconst18=x1041;
2489 IkReal gconst19=(((x1036*x1040))+((x1037*x1039)));
2490 IkReal gconst20=(((x1036*x1039))+(((-1.0)*x1037*x1040)));
2491 j1eval[0]=IKabs(((3.5)+sj3+(((8.33333333333333)*py))+(((5.16666666666667)*cj3))));
2492 if( IKabs(j1eval[0]) < 0.0000000100000000 )
2493 {
2494 continue; // no branches [j1]
2495 
2496 } else
2497 {
2498 IkReal op[2+1], zeror[2];
2499 int numroots;
2500 IkReal x1042=((8.33333333333333)*py);
2501 IkReal x1043=((5.16666666666667)*cj3);
2502 op[0]=((3.5)+sj3+x1043+x1042);
2503 op[1]=0;
2504 op[2]=((-3.5)+(((-1.0)*sj3))+x1042+(((-1.0)*x1043)));
2505 polyroots2(op,zeror,numroots);
2506 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
2507 int numsolutions = 0;
2508 for(int ij1 = 0; ij1 < numroots; ++ij1)
2509 {
2510 IkReal htj1 = zeror[ij1];
2511 tempj1array[0]=((2.0)*(atan(htj1)));
2512 for(int kj1 = 0; kj1 < 1; ++kj1)
2513 {
2514 j1array[numsolutions] = tempj1array[kj1];
2515 if( j1array[numsolutions] > IKPI )
2516 {
2517  j1array[numsolutions]-=IK2PI;
2518 }
2519 else if( j1array[numsolutions] < -IKPI )
2520 {
2521  j1array[numsolutions]+=IK2PI;
2522 }
2523 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
2524 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
2525 numsolutions++;
2526 }
2527 }
2528 bool j1valid[2]={true,true};
2529 _nj1 = 2;
2530 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
2531  {
2532 if( !j1valid[ij1] )
2533 {
2534  continue;
2535 }
2536  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2537 htj1 = IKtan(j1/2);
2538 
2539 _ij1[0] = ij1; _ij1[1] = -1;
2540 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2541 {
2542 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2543 {
2544  j1valid[iij1]=false; _ij1[1] = iij1; break;
2545 }
2546 }
2547 rotationfunction0(solutions);
2548  }
2549 
2550 }
2551 
2552 }
2553 
2554 }
2555 } while(0);
2556 if( bgotonextstatement )
2557 {
2558 bool bgotonextstatement = true;
2559 do
2560 {
2561 IkReal x1044=IKcos(pz);
2562 IkReal x1045=IKsin(pz);
2563 if((((-1.0)*(py*py))) < -0.00001)
2564 continue;
2565 IkReal x1046=IKsqrt(((-1.0)*(py*py)));
2566 IkReal x1047=IKcos(x1046);
2567 IkReal x1048=IKsin(x1046);
2568 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2569 continue;
2570 IkReal gconst21=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2571 IkReal gconst22=((((-1.0)*x1044*x1048))+((x1045*x1047)));
2572 IkReal gconst23=(((x1045*x1048))+((x1044*x1047)));
2573 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2574 continue;
2575 evalcond[0]=IKabs(((((-1.0)*pz))+(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
2576 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2577 {
2578 bgotonextstatement=false;
2579 {
2580 IkReal j1eval[1];
2581 IkReal x1049=IKcos(pz);
2582 IkReal x1050=IKsin(pz);
2583 IkReal x1051=x1046;
2584 IkReal x1052=IKcos(x1051);
2585 IkReal x1053=IKsin(x1051);
2586 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2587 continue;
2588 IkReal x1054=(pz+(((-1.0)*(IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2589 sj2=0;
2590 cj2=-1.0;
2591 j2=3.14159265358979;
2592 sj0=gconst22;
2593 cj0=gconst23;
2594 j0=x1054;
2595 IkReal gconst21=x1054;
2596 IkReal gconst22=((((-1.0)*x1049*x1053))+((x1050*x1052)));
2597 IkReal gconst23=(((x1050*x1053))+((x1049*x1052)));
2598 j1eval[0]=IKabs(((3.5)+sj3+(((8.33333333333333)*py))+(((5.16666666666667)*cj3))));
2599 if( IKabs(j1eval[0]) < 0.0000000100000000 )
2600 {
2601 continue; // no branches [j1]
2602 
2603 } else
2604 {
2605 IkReal op[2+1], zeror[2];
2606 int numroots;
2607 IkReal x1055=((8.33333333333333)*py);
2608 IkReal x1056=((5.16666666666667)*cj3);
2609 op[0]=((3.5)+sj3+x1055+x1056);
2610 op[1]=0;
2611 op[2]=((-3.5)+(((-1.0)*x1056))+(((-1.0)*sj3))+x1055);
2612 polyroots2(op,zeror,numroots);
2613 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
2614 int numsolutions = 0;
2615 for(int ij1 = 0; ij1 < numroots; ++ij1)
2616 {
2617 IkReal htj1 = zeror[ij1];
2618 tempj1array[0]=((2.0)*(atan(htj1)));
2619 for(int kj1 = 0; kj1 < 1; ++kj1)
2620 {
2621 j1array[numsolutions] = tempj1array[kj1];
2622 if( j1array[numsolutions] > IKPI )
2623 {
2624  j1array[numsolutions]-=IK2PI;
2625 }
2626 else if( j1array[numsolutions] < -IKPI )
2627 {
2628  j1array[numsolutions]+=IK2PI;
2629 }
2630 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
2631 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
2632 numsolutions++;
2633 }
2634 }
2635 bool j1valid[2]={true,true};
2636 _nj1 = 2;
2637 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
2638  {
2639 if( !j1valid[ij1] )
2640 {
2641  continue;
2642 }
2643  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2644 htj1 = IKtan(j1/2);
2645 
2646 _ij1[0] = ij1; _ij1[1] = -1;
2647 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2648 {
2649 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2650 {
2651  j1valid[iij1]=false; _ij1[1] = iij1; break;
2652 }
2653 }
2654 rotationfunction0(solutions);
2655  }
2656 
2657 }
2658 
2659 }
2660 
2661 }
2662 } while(0);
2663 if( bgotonextstatement )
2664 {
2665 bool bgotonextstatement = true;
2666 do
2667 {
2668 if( 1 )
2669 {
2670 bgotonextstatement=false;
2671 continue; // branch miss [j1]
2672 
2673 }
2674 } while(0);
2675 if( bgotonextstatement )
2676 {
2677 }
2678 }
2679 }
2680 }
2681 }
2682 }
2683 
2684 } else
2685 {
2686 {
2687 IkReal j1array[1], cj1array[1], sj1array[1];
2688 bool j1valid[1]={false};
2689 _nj1 = 1;
2690 IkReal x1057=((0.62)*cj3);
2691 IkReal x1058=((0.12)*sj3);
2692 CheckValue<IkReal> x1059=IKPowWithIntegerCheck(((-0.42)+(((-1.0)*x1058))+(((-1.0)*x1057))),-1);
2693 if(!x1059.valid){
2694 continue;
2695 }
2696 CheckValue<IkReal> x1060=IKPowWithIntegerCheck((((pz*x1058))+((pz*x1057))+(((-0.42)*j0))+(((0.42)*pz))+(((-1.0)*j0*x1058))+(((-1.0)*j0*x1057))),-1);
2697 if(!x1060.valid){
2698 continue;
2699 }
2700 if( IKabs(((x1059.value)*(((((-1.0)*pz))+j0)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x1060.value)*(((((-1.0)*j0*py))+((py*pz)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x1059.value)*(((((-1.0)*pz))+j0))))+IKsqr(((x1060.value)*(((((-1.0)*j0*py))+((py*pz))))))-1) <= IKFAST_SINCOS_THRESH )
2701  continue;
2702 j1array[0]=IKatan2(((x1059.value)*(((((-1.0)*pz))+j0))), ((x1060.value)*(((((-1.0)*j0*py))+((py*pz))))));
2703 sj1array[0]=IKsin(j1array[0]);
2704 cj1array[0]=IKcos(j1array[0]);
2705 if( j1array[0] > IKPI )
2706 {
2707  j1array[0]-=IK2PI;
2708 }
2709 else if( j1array[0] < -IKPI )
2710 { j1array[0]+=IK2PI;
2711 }
2712 j1valid[0] = true;
2713 for(int ij1 = 0; ij1 < 1; ++ij1)
2714 {
2715 if( !j1valid[ij1] )
2716 {
2717  continue;
2718 }
2719 _ij1[0] = ij1; _ij1[1] = -1;
2720 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2721 {
2722 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2723 {
2724  j1valid[iij1]=false; _ij1[1] = iij1; break;
2725 }
2726 }
2727 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2728 {
2729 IkReal evalcond[5];
2730 IkReal x1061=IKcos(j1);
2731 IkReal x1062=IKsin(j1);
2732 IkReal x1063=((0.62)*cj3);
2733 IkReal x1064=((0.12)*sj3);
2734 IkReal x1065=(pz*x1062);
2735 IkReal x1066=(j0*x1062);
2736 IkReal x1067=((1.0)*x1061);
2737 evalcond[0]=(((py*x1062))+(((-1.0)*pz*x1067))+((j0*x1061)));
2738 evalcond[1]=((((-0.42)*x1061))+(((-1.0)*x1061*x1063))+(((-1.0)*x1061*x1064))+py);
2739 evalcond[2]=((((-0.42)*x1062))+(((-1.0)*x1062*x1064))+(((-1.0)*x1062*x1063))+pz+(((-1.0)*j0)));
2740 evalcond[3]=((0.42)+x1063+x1064+x1066+(((-1.0)*py*x1067))+(((-1.0)*x1065)));
2741 evalcond[4]=((-0.3528)+(((-0.1008)*sj3))+(((0.84)*x1065))+(((-0.5208)*cj3))+(((0.84)*py*x1061))+(((-0.84)*x1066)));
2742 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2743 {
2744 continue;
2745 }
2746 }
2747 
2748 rotationfunction0(solutions);
2749 }
2750 }
2751 
2752 }
2753 
2754 }
2755 
2756 } else
2757 {
2758 {
2759 IkReal j1array[1], cj1array[1], sj1array[1];
2760 bool j1valid[1]={false};
2761 _nj1 = 1;
2762 IkReal x1068=((0.62)*cj3);
2763 IkReal x1069=((0.12)*sj3);
2764 CheckValue<IkReal> x1070=IKPowWithIntegerCheck(IKsign(((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)))),-1);
2765 if(!x1070.valid){
2766 continue;
2767 }
2768 CheckValue<IkReal> x1071 = IKatan2WithCheck(IkReal(((((0.42)*j0))+(((-1.0)*pz*x1068))+(((-1.0)*pz*x1069))+((j0*x1069))+((j0*x1068))+(((-0.42)*pz)))),IkReal(((((-1.0)*py*x1069))+(((-1.0)*py*x1068))+(((-0.42)*py)))),IKFAST_ATAN2_MAGTHRESH);
2769 if(!x1071.valid){
2770 continue;
2771 }
2772 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1070.value)))+(x1071.value));
2773 sj1array[0]=IKsin(j1array[0]);
2774 cj1array[0]=IKcos(j1array[0]);
2775 if( j1array[0] > IKPI )
2776 {
2777  j1array[0]-=IK2PI;
2778 }
2779 else if( j1array[0] < -IKPI )
2780 { j1array[0]+=IK2PI;
2781 }
2782 j1valid[0] = true;
2783 for(int ij1 = 0; ij1 < 1; ++ij1)
2784 {
2785 if( !j1valid[ij1] )
2786 {
2787  continue;
2788 }
2789 _ij1[0] = ij1; _ij1[1] = -1;
2790 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2791 {
2792 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2793 {
2794  j1valid[iij1]=false; _ij1[1] = iij1; break;
2795 }
2796 }
2797 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2798 {
2799 IkReal evalcond[5];
2800 IkReal x1072=IKcos(j1);
2801 IkReal x1073=IKsin(j1);
2802 IkReal x1074=((0.62)*cj3);
2803 IkReal x1075=((0.12)*sj3);
2804 IkReal x1076=(pz*x1073);
2805 IkReal x1077=(j0*x1073);
2806 IkReal x1078=((1.0)*x1072);
2807 evalcond[0]=(((py*x1073))+(((-1.0)*pz*x1078))+((j0*x1072)));
2808 evalcond[1]=((((-0.42)*x1072))+py+(((-1.0)*x1072*x1075))+(((-1.0)*x1072*x1074)));
2809 evalcond[2]=((((-1.0)*x1073*x1075))+(((-1.0)*x1073*x1074))+(((-0.42)*x1073))+pz+(((-1.0)*j0)));
2810 evalcond[3]=((0.42)+x1077+x1074+x1075+(((-1.0)*py*x1078))+(((-1.0)*x1076)));
2811 evalcond[4]=((-0.3528)+(((-0.1008)*sj3))+(((0.84)*x1076))+(((-0.5208)*cj3))+(((0.84)*py*x1072))+(((-0.84)*x1077)));
2812 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2813 {
2814 continue;
2815 }
2816 }
2817 
2818 rotationfunction0(solutions);
2819 }
2820 }
2821 
2822 }
2823 
2824 }
2825 
2826 } else
2827 {
2828 {
2829 IkReal j1array[1], cj1array[1], sj1array[1];
2830 bool j1valid[1]={false};
2831 _nj1 = 1;
2832 CheckValue<IkReal> x1079 = IKatan2WithCheck(IkReal(((((-1.0)*pz))+j0)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
2833 if(!x1079.valid){
2834 continue;
2835 }
2836 CheckValue<IkReal> x1080=IKPowWithIntegerCheck(IKsign(((-0.42)+(((-0.12)*sj3))+(((-0.62)*cj3)))),-1);
2837 if(!x1080.valid){
2838 continue;
2839 }
2840 j1array[0]=((-1.5707963267949)+(x1079.value)+(((1.5707963267949)*(x1080.value))));
2841 sj1array[0]=IKsin(j1array[0]);
2842 cj1array[0]=IKcos(j1array[0]);
2843 if( j1array[0] > IKPI )
2844 {
2845  j1array[0]-=IK2PI;
2846 }
2847 else if( j1array[0] < -IKPI )
2848 { j1array[0]+=IK2PI;
2849 }
2850 j1valid[0] = true;
2851 for(int ij1 = 0; ij1 < 1; ++ij1)
2852 {
2853 if( !j1valid[ij1] )
2854 {
2855  continue;
2856 }
2857 _ij1[0] = ij1; _ij1[1] = -1;
2858 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2859 {
2860 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2861 {
2862  j1valid[iij1]=false; _ij1[1] = iij1; break;
2863 }
2864 }
2865 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2866 {
2867 IkReal evalcond[5];
2868 IkReal x1081=IKcos(j1);
2869 IkReal x1082=IKsin(j1);
2870 IkReal x1083=((0.62)*cj3);
2871 IkReal x1084=((0.12)*sj3);
2872 IkReal x1085=(pz*x1082);
2873 IkReal x1086=(j0*x1082);
2874 IkReal x1087=((1.0)*x1081);
2875 evalcond[0]=((((-1.0)*pz*x1087))+((py*x1082))+((j0*x1081)));
2876 evalcond[1]=((((-1.0)*x1081*x1084))+(((-1.0)*x1081*x1083))+py+(((-0.42)*x1081)));
2877 evalcond[2]=(pz+(((-1.0)*x1082*x1083))+(((-1.0)*x1082*x1084))+(((-1.0)*j0))+(((-0.42)*x1082)));
2878 evalcond[3]=((0.42)+(((-1.0)*py*x1087))+x1086+x1084+x1083+(((-1.0)*x1085)));
2879 evalcond[4]=((-0.3528)+(((-0.1008)*sj3))+(((-0.5208)*cj3))+(((0.84)*py*x1081))+(((-0.84)*x1086))+(((0.84)*x1085)));
2880 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
2881 {
2882 continue;
2883 }
2884 }
2885 
2886 rotationfunction0(solutions);
2887 }
2888 }
2889 
2890 }
2891 
2892 }
2893 
2894 }
2895 } while(0);
2896 if( bgotonextstatement )
2897 {
2898 bool bgotonextstatement = true;
2899 do
2900 {
2901 if( 1 )
2902 {
2903 bgotonextstatement=false;
2904 continue; // branch miss [j1]
2905 
2906 }
2907 } while(0);
2908 if( bgotonextstatement )
2909 {
2910 }
2911 }
2912 }
2913 }
2914 }
2915 }
2916 
2917 } else
2918 {
2919 {
2920 IkReal j1array[1], cj1array[1], sj1array[1];
2921 bool j1valid[1]={false};
2922 _nj1 = 1;
2923 IkReal x1088=((0.62)*cj3);
2924 IkReal x1089=((0.18)*cj2);
2925 IkReal x1090=((0.12)*sj3);
2926 IkReal x1091=(px*sj2);
2927 CheckValue<IkReal> x1092=IKPowWithIntegerCheck(IKsign(((((-2.0)*cj2*j0*pz))+(((-1.0)*cj2*(px*px)))+((cj2*(j0*j0)))+((cj2*pp)))),-1);
2928 if(!x1092.valid){
2929 continue;
2930 }
2931 CheckValue<IkReal> x1093 = IKatan2WithCheck(IkReal(((((-1.0)*pz*x1088))+(((-1.0)*pz*x1089))+(((-1.0)*j0*x1091))+(((-1.0)*pz*x1090))+(((-0.6)*pz))+((j0*x1090))+((j0*x1088))+((j0*x1089))+(((0.6)*j0))+((pz*x1091)))),IkReal(((((-1.0)*py*x1089))+(((-1.0)*py*x1088))+(((-1.0)*py*x1090))+(((-0.6)*py))+((py*x1091)))),IKFAST_ATAN2_MAGTHRESH);
2932 if(!x1093.valid){
2933 continue;
2934 }
2935 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1092.value)))+(x1093.value));
2936 sj1array[0]=IKsin(j1array[0]);
2937 cj1array[0]=IKcos(j1array[0]);
2938 if( j1array[0] > IKPI )
2939 {
2940  j1array[0]-=IK2PI;
2941 }
2942 else if( j1array[0] < -IKPI )
2943 { j1array[0]+=IK2PI;
2944 }
2945 j1valid[0] = true;
2946 for(int ij1 = 0; ij1 < 1; ++ij1)
2947 {
2948 if( !j1valid[ij1] )
2949 {
2950  continue;
2951 }
2952 _ij1[0] = ij1; _ij1[1] = -1;
2953 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
2954 {
2955 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
2956 {
2957  j1valid[iij1]=false; _ij1[1] = iij1; break;
2958 }
2959 }
2960 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2961 {
2962 IkReal evalcond[6];
2963 IkReal x1094=IKcos(j1);
2964 IkReal x1095=IKsin(j1);
2965 IkReal x1096=((0.62)*cj3);
2966 IkReal x1097=((0.12)*cj3);
2967 IkReal x1098=((1.0)*pz);
2968 IkReal x1099=((0.62)*sj3);
2969 IkReal x1100=((1.0)*j0);
2970 IkReal x1101=((0.12)*sj3);
2971 IkReal x1102=((1.0)*px);
2972 IkReal x1103=(sj2*x1095);
2973 IkReal x1104=(cj2*x1095);
2974 IkReal x1105=(cj2*x1094);
2975 IkReal x1106=(sj2*x1094);
2976 IkReal x1107=((0.36)*x1095);
2977 evalcond[0]=(((j0*x1094))+(((-1.0)*x1094*x1098))+((py*x1095)));
2978 evalcond[1]=((0.6)+x1101+x1096+(((0.18)*cj2))+(((-1.0)*sj2*x1102))+(((-1.0)*x1100*x1104))+((py*x1105))+((pz*x1104)));
2979 evalcond[2]=((((-1.0)*x1098*x1103))+x1099+(((-0.18)*sj2))+(((-1.0)*x1097))+(((-1.0)*cj2*x1102))+((j0*x1103))+(((-1.0)*py*x1106)));
2980 evalcond[3]=((((0.18)*x1094))+(((0.6)*x1105))+((x1097*x1106))+((x1101*x1105))+py+(((-1.0)*x1099*x1106))+((x1096*x1105)));
2981 evalcond[4]=((((0.18)*x1095))+(((0.6)*x1104))+((x1097*x1103))+((x1101*x1104))+pz+(((-1.0)*x1099*x1103))+((x1096*x1104))+(((-1.0)*x1100)));
2982 evalcond[5]=((0.0064)+(((-0.36)*py*x1094))+(((1.2)*px*sj2))+(((1.2)*j0*x1104))+(((-1.0)*pp))+(((2.0)*j0*pz))+(((-1.2)*py*x1105))+(((-1.0)*pz*x1107))+((j0*x1107))+(((-1.2)*pz*x1104))+(((-1.0)*j0*x1100))+(((-0.216)*cj2)));
2983 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
2984 {
2985 continue;
2986 }
2987 }
2988 
2989 rotationfunction0(solutions);
2990 }
2991 }
2992 
2993 }
2994 
2995 }
2996 
2997 } else
2998 {
2999 {
3000 IkReal j1array[1], cj1array[1], sj1array[1];
3001 bool j1valid[1]={false};
3002 _nj1 = 1;
3003 IkReal x1108=(pz*sj2);
3004 IkReal x1109=((1.0)*sj2);
3005 IkReal x1110=((0.18)*sj2);
3006 IkReal x1111=(cj2*px);
3007 IkReal x1112=((0.12)*cj3);
3008 IkReal x1113=((0.62)*sj3);
3009 CheckValue<IkReal> x1114=IKPowWithIntegerCheck(IKsign(((((-1.0)*pp*x1109))+((sj2*(px*px)))+(((2.0)*j0*x1108))+(((-1.0)*x1109*(j0*j0))))),-1);
3010 if(!x1114.valid){
3011 continue;
3012 }
3013 CheckValue<IkReal> x1115 = IKatan2WithCheck(IkReal(((((0.18)*x1108))+(((-1.0)*j0*x1111))+(((-1.0)*pz*x1113))+((pz*x1112))+((pz*x1111))+(((-1.0)*j0*x1112))+(((-1.0)*j0*x1110))+((j0*x1113)))),IkReal(((((-1.0)*py*x1113))+((py*x1110))+((py*x1111))+((py*x1112)))),IKFAST_ATAN2_MAGTHRESH);
3014 if(!x1115.valid){
3015 continue;
3016 }
3017 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1114.value)))+(x1115.value));
3018 sj1array[0]=IKsin(j1array[0]);
3019 cj1array[0]=IKcos(j1array[0]);
3020 if( j1array[0] > IKPI )
3021 {
3022  j1array[0]-=IK2PI;
3023 }
3024 else if( j1array[0] < -IKPI )
3025 { j1array[0]+=IK2PI;
3026 }
3027 j1valid[0] = true;
3028 for(int ij1 = 0; ij1 < 1; ++ij1)
3029 {
3030 if( !j1valid[ij1] )
3031 {
3032  continue;
3033 }
3034 _ij1[0] = ij1; _ij1[1] = -1;
3035 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
3036 {
3037 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
3038 {
3039  j1valid[iij1]=false; _ij1[1] = iij1; break;
3040 }
3041 }
3042 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3043 {
3044 IkReal evalcond[6];
3045 IkReal x1116=IKcos(j1);
3046 IkReal x1117=IKsin(j1);
3047 IkReal x1118=((0.62)*cj3);
3048 IkReal x1119=((0.12)*cj3);
3049 IkReal x1120=((1.0)*pz);
3050 IkReal x1121=((0.62)*sj3);
3051 IkReal x1122=((1.0)*j0);
3052 IkReal x1123=((0.12)*sj3);
3053 IkReal x1124=((1.0)*px);
3054 IkReal x1125=(sj2*x1117);
3055 IkReal x1126=(cj2*x1117);
3056 IkReal x1127=(cj2*x1116);
3057 IkReal x1128=(sj2*x1116);
3058 IkReal x1129=((0.36)*x1117);
3059 evalcond[0]=((((-1.0)*x1116*x1120))+((py*x1117))+((j0*x1116)));
3060 evalcond[1]=((0.6)+x1118+x1123+(((-1.0)*sj2*x1124))+(((0.18)*cj2))+(((-1.0)*x1122*x1126))+((pz*x1126))+((py*x1127)));
3061 evalcond[2]=(x1121+(((-0.18)*sj2))+(((-1.0)*py*x1128))+((j0*x1125))+(((-1.0)*cj2*x1124))+(((-1.0)*x1120*x1125))+(((-1.0)*x1119)));
3062 evalcond[3]=((((0.18)*x1116))+(((-1.0)*x1121*x1128))+((x1119*x1128))+(((0.6)*x1127))+py+((x1123*x1127))+((x1118*x1127)));
3063 evalcond[4]=((((0.18)*x1117))+(((-1.0)*x1121*x1125))+((x1119*x1125))+(((0.6)*x1126))+pz+(((-1.0)*x1122))+((x1123*x1126))+((x1118*x1126)));
3064 evalcond[5]=((0.0064)+(((-1.0)*j0*x1122))+(((-0.36)*py*x1116))+(((1.2)*px*sj2))+(((1.2)*j0*x1126))+(((-1.2)*py*x1127))+(((-1.0)*pz*x1129))+(((-1.0)*pp))+(((2.0)*j0*pz))+((j0*x1129))+(((-0.216)*cj2))+(((-1.2)*pz*x1126)));
3065 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
3066 {
3067 continue;
3068 }
3069 }
3070 
3071 rotationfunction0(solutions);
3072 }
3073 }
3074 
3075 }
3076 
3077 }
3078 
3079 } else
3080 {
3081 {
3082 IkReal j1array[1], cj1array[1], sj1array[1];
3083 bool j1valid[1]={false};
3084 _nj1 = 1;
3085 CheckValue<IkReal> x1130 = IKatan2WithCheck(IkReal(((((-1.0)*pz))+j0)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
3086 if(!x1130.valid){
3087 continue;
3088 }
3089 CheckValue<IkReal> x1131=IKPowWithIntegerCheck(IKsign(((0.18)+(((0.12)*cj2*sj3))+(((0.6)*cj2))+(((-0.62)*sj2*sj3))+(((0.62)*cj2*cj3))+(((0.12)*cj3*sj2)))),-1);
3090 if(!x1131.valid){
3091 continue;
3092 }
3093 j1array[0]=((-1.5707963267949)+(x1130.value)+(((1.5707963267949)*(x1131.value))));
3094 sj1array[0]=IKsin(j1array[0]);
3095 cj1array[0]=IKcos(j1array[0]);
3096 if( j1array[0] > IKPI )
3097 {
3098  j1array[0]-=IK2PI;
3099 }
3100 else if( j1array[0] < -IKPI )
3101 { j1array[0]+=IK2PI;
3102 }
3103 j1valid[0] = true;
3104 for(int ij1 = 0; ij1 < 1; ++ij1)
3105 {
3106 if( !j1valid[ij1] )
3107 {
3108  continue;
3109 }
3110 _ij1[0] = ij1; _ij1[1] = -1;
3111 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
3112 {
3113 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
3114 {
3115  j1valid[iij1]=false; _ij1[1] = iij1; break;
3116 }
3117 }
3118 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3119 {
3120 IkReal evalcond[6];
3121 IkReal x1132=IKcos(j1);
3122 IkReal x1133=IKsin(j1);
3123 IkReal x1134=((0.62)*cj3);
3124 IkReal x1135=((0.12)*cj3);
3125 IkReal x1136=((1.0)*pz);
3126 IkReal x1137=((0.62)*sj3);
3127 IkReal x1138=((1.0)*j0);
3128 IkReal x1139=((0.12)*sj3);
3129 IkReal x1140=((1.0)*px);
3130 IkReal x1141=(sj2*x1133);
3131 IkReal x1142=(cj2*x1133);
3132 IkReal x1143=(cj2*x1132);
3133 IkReal x1144=(sj2*x1132);
3134 IkReal x1145=((0.36)*x1133);
3135 evalcond[0]=(((py*x1133))+((j0*x1132))+(((-1.0)*x1132*x1136)));
3136 evalcond[1]=((0.6)+x1139+x1134+((py*x1143))+(((0.18)*cj2))+((pz*x1142))+(((-1.0)*sj2*x1140))+(((-1.0)*x1138*x1142)));
3137 evalcond[2]=((((-1.0)*x1136*x1141))+x1137+(((-1.0)*py*x1144))+(((-0.18)*sj2))+((j0*x1141))+(((-1.0)*x1135))+(((-1.0)*cj2*x1140)));
3138 evalcond[3]=((((-1.0)*x1137*x1144))+((x1134*x1143))+(((0.18)*x1132))+py+((x1139*x1143))+(((0.6)*x1143))+((x1135*x1144)));
3139 evalcond[4]=((((-1.0)*x1137*x1141))+((x1134*x1142))+(((0.18)*x1133))+(((-1.0)*x1138))+pz+((x1139*x1142))+(((0.6)*x1142))+((x1135*x1141)));
3140 evalcond[5]=((0.0064)+(((-0.36)*py*x1132))+(((1.2)*j0*x1142))+(((-1.0)*j0*x1138))+((j0*x1145))+(((1.2)*px*sj2))+(((-1.0)*pp))+(((2.0)*j0*pz))+(((-1.2)*pz*x1142))+(((-1.2)*py*x1143))+(((-0.216)*cj2))+(((-1.0)*pz*x1145)));
3141 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
3142 {
3143 continue;
3144 }
3145 }
3146 
3147 rotationfunction0(solutions);
3148 }
3149 }
3150 
3151 }
3152 
3153 }
3154 }
3155 }
3156 }
3157 }
3158 
3159 }
3160 
3161 }
3162 }
3163 return solutions.GetNumSolutions()>0;
3164 }
3165 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
3166 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
3167 IkReal x76=((1.0)*r10);
3168 IkReal x77=((1.0)*cj2);
3169 IkReal x78=((1.0)*sj1);
3170 IkReal x79=(sj2*sj3);
3171 IkReal x80=((1.0)*cj1*r11);
3172 IkReal x81=((1.0)*cj1*r12);
3173 IkReal x82=(((cj2*cj3))+(((-1.0)*x79)));
3174 IkReal x83=((((-1.0)*cj3*x77))+x79);
3175 IkReal x84=((((-1.0)*sj3*x77))+(((-1.0)*cj3*sj2)));
3176 IkReal x85=((1.0)*x84);
3177 IkReal x86=(cj1*x84);
3178 IkReal x87=(cj1*x85);
3179 new_r00=((((-1.0)*sj1*x76))+((cj1*r20)));
3180 new_r01=(((cj1*r21))+(((-1.0)*r11*x78)));
3181 new_r02=(((cj1*r22))+(((-1.0)*r12*x78)));
3182 new_r10=((((-1.0)*r20*x78*x84))+((r00*x82))+(((-1.0)*x76*x86)));
3183 new_r11=((((-1.0)*x80*x84))+((r01*x82))+(((-1.0)*r21*x78*x84)));
3184 new_r12=((((-1.0)*x81*x84))+(((-1.0)*r22*x78*x84))+((r02*x82)));
3185 new_r20=((((-1.0)*r20*x78*x83))+((r00*x84))+(((-1.0)*cj1*x76*x83)));
3186 new_r21=((((-1.0)*x80*x83))+((r01*x84))+(((-1.0)*r21*x78*x83)));
3187 new_r22=((((-1.0)*x81*x83))+(((-1.0)*r22*x78*x83))+((r02*x84)));
3188 {
3189 IkReal j5array[2], cj5array[2], sj5array[2];
3190 bool j5valid[2]={false};
3191 _nj5 = 2;
3192 cj5array[0]=new_r22;
3193 if( cj5array[0] >= -1-IKFAST_SINCOS_THRESH && cj5array[0] <= 1+IKFAST_SINCOS_THRESH )
3194 {
3195  j5valid[0] = j5valid[1] = true;
3196  j5array[0] = IKacos(cj5array[0]);
3197  sj5array[0] = IKsin(j5array[0]);
3198  cj5array[1] = cj5array[0];
3199  j5array[1] = -j5array[0];
3200  sj5array[1] = -sj5array[0];
3201 }
3202 else if( isnan(cj5array[0]) )
3203 {
3204  // probably any value will work
3205  j5valid[0] = true;
3206  cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
3207 }
3208 for(int ij5 = 0; ij5 < 2; ++ij5)
3209 {
3210 if( !j5valid[ij5] )
3211 {
3212  continue;
3213 }
3214 _ij5[0] = ij5; _ij5[1] = -1;
3215 for(int iij5 = ij5+1; iij5 < 2; ++iij5)
3216 {
3217 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3218 {
3219  j5valid[iij5]=false; _ij5[1] = iij5; break;
3220 }
3221 }
3222 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3223 
3224 {
3225 IkReal j6eval[3];
3226 j6eval[0]=sj5;
3227 j6eval[1]=IKsign(sj5);
3228 j6eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
3229 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
3230 {
3231 {
3232 IkReal j4eval[3];
3233 j4eval[0]=sj5;
3234 j4eval[1]=((IKabs(new_r12))+(IKabs(new_r02)));
3235 j4eval[2]=IKsign(sj5);
3236 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
3237 {
3238 {
3239 IkReal j4eval[2];
3240 j4eval[0]=new_r02;
3241 j4eval[1]=sj5;
3242 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 )
3243 {
3244 {
3245 IkReal evalcond[5];
3246 bool bgotonextstatement = true;
3247 do
3248 {
3249 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
3250 evalcond[1]=new_r12;
3251 evalcond[2]=new_r02;
3252 evalcond[3]=new_r20;
3253 evalcond[4]=new_r21;
3254 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
3255 {
3256 bgotonextstatement=false;
3257 IkReal j6mul = 1;
3258 j6=0;
3259 j4mul=-1.0;
3260 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
3261  continue;
3262 j4=IKatan2(((-1.0)*new_r01), new_r00);
3263 {
3264 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3265 vinfos[0].jointtype = 17;
3266 vinfos[0].foffset = j0;
3267 vinfos[0].indices[0] = _ij0[0];
3268 vinfos[0].indices[1] = _ij0[1];
3269 vinfos[0].maxsolutions = _nj0;
3270 vinfos[1].jointtype = 1;
3271 vinfos[1].foffset = j1;
3272 vinfos[1].indices[0] = _ij1[0];
3273 vinfos[1].indices[1] = _ij1[1];
3274 vinfos[1].maxsolutions = _nj1;
3275 vinfos[2].jointtype = 1;
3276 vinfos[2].foffset = j2;
3277 vinfos[2].indices[0] = _ij2[0];
3278 vinfos[2].indices[1] = _ij2[1];
3279 vinfos[2].maxsolutions = _nj2;
3280 vinfos[3].jointtype = 1;
3281 vinfos[3].foffset = j3;
3282 vinfos[3].indices[0] = _ij3[0];
3283 vinfos[3].indices[1] = _ij3[1];
3284 vinfos[3].maxsolutions = _nj3;
3285 vinfos[4].jointtype = 1;
3286 vinfos[4].foffset = j4;
3287 vinfos[4].fmul = j4mul;
3288 vinfos[4].freeind = 0;
3289 vinfos[4].maxsolutions = 0;
3290 vinfos[5].jointtype = 1;
3291 vinfos[5].foffset = j5;
3292 vinfos[5].indices[0] = _ij5[0];
3293 vinfos[5].indices[1] = _ij5[1];
3294 vinfos[5].maxsolutions = _nj5;
3295 vinfos[6].jointtype = 1;
3296 vinfos[6].foffset = j6;
3297 vinfos[6].fmul = j6mul;
3298 vinfos[6].freeind = 0;
3299 vinfos[6].maxsolutions = 0;
3300 std::vector<int> vfree(1);
3301 vfree[0] = 6;
3302 solutions.AddSolution(vinfos,vfree);
3303 }
3304 
3305 }
3306 } while(0);
3307 if( bgotonextstatement )
3308 {
3309 bool bgotonextstatement = true;
3310 do
3311 {
3312 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
3313 evalcond[1]=new_r12;
3314 evalcond[2]=new_r02;
3315 evalcond[3]=new_r20;
3316 evalcond[4]=new_r21;
3317 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
3318 {
3319 bgotonextstatement=false;
3320 IkReal j6mul = 1;
3321 j6=0;
3322 j4mul=1.0;
3323 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
3324  continue;
3325 j4=IKatan2(new_r01, ((-1.0)*new_r11));
3326 {
3327 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3328 vinfos[0].jointtype = 17;
3329 vinfos[0].foffset = j0;
3330 vinfos[0].indices[0] = _ij0[0];
3331 vinfos[0].indices[1] = _ij0[1];
3332 vinfos[0].maxsolutions = _nj0;
3333 vinfos[1].jointtype = 1;
3334 vinfos[1].foffset = j1;
3335 vinfos[1].indices[0] = _ij1[0];
3336 vinfos[1].indices[1] = _ij1[1];
3337 vinfos[1].maxsolutions = _nj1;
3338 vinfos[2].jointtype = 1;
3339 vinfos[2].foffset = j2;
3340 vinfos[2].indices[0] = _ij2[0];
3341 vinfos[2].indices[1] = _ij2[1];
3342 vinfos[2].maxsolutions = _nj2;
3343 vinfos[3].jointtype = 1;
3344 vinfos[3].foffset = j3;
3345 vinfos[3].indices[0] = _ij3[0];
3346 vinfos[3].indices[1] = _ij3[1];
3347 vinfos[3].maxsolutions = _nj3;
3348 vinfos[4].jointtype = 1;
3349 vinfos[4].foffset = j4;
3350 vinfos[4].fmul = j4mul;
3351 vinfos[4].freeind = 0;
3352 vinfos[4].maxsolutions = 0;
3353 vinfos[5].jointtype = 1;
3354 vinfos[5].foffset = j5;
3355 vinfos[5].indices[0] = _ij5[0];
3356 vinfos[5].indices[1] = _ij5[1];
3357 vinfos[5].maxsolutions = _nj5;
3358 vinfos[6].jointtype = 1;
3359 vinfos[6].foffset = j6;
3360 vinfos[6].fmul = j6mul;
3361 vinfos[6].freeind = 0;
3362 vinfos[6].maxsolutions = 0;
3363 std::vector<int> vfree(1);
3364 vfree[0] = 6;
3365 solutions.AddSolution(vinfos,vfree);
3366 }
3367 
3368 }
3369 } while(0);
3370 if( bgotonextstatement )
3371 {
3372 bool bgotonextstatement = true;
3373 do
3374 {
3375 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3376 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3377 {
3378 bgotonextstatement=false;
3379 {
3380 IkReal j4eval[1];
3381 new_r21=0;
3382 new_r20=0;
3383 new_r02=0;
3384 new_r12=0;
3385 IkReal x88=new_r22*new_r22;
3386 IkReal x89=((16.0)*new_r10);
3387 IkReal x90=((16.0)*new_r01);
3388 IkReal x91=((16.0)*new_r00);
3389 IkReal x92=(new_r11*new_r22);
3390 IkReal x93=((8.0)*new_r00);
3391 IkReal x94=(x88*x89);
3392 IkReal x95=(x88*x90);
3393 j4eval[0]=((IKabs(((((-8.0)*x92))+((x88*x93)))))+(IKabs((x89+(((-1.0)*x94)))))+(IKabs((x90+(((-1.0)*x95)))))+(IKabs((((new_r22*x91))+(((16.0)*new_r11))+(((-32.0)*new_r11*x88)))))+(IKabs((x95+(((-1.0)*x90)))))+(IKabs((x94+(((-1.0)*x89)))))+(IKabs((((new_r22*x93))+(((-8.0)*new_r11)))))+(IKabs(((((32.0)*new_r00))+(((-16.0)*x92))+(((-1.0)*x88*x91))))));
3394 if( IKabs(j4eval[0]) < 0.0000000100000000 )
3395 {
3396 continue; // no branches [j4, j6]
3397 
3398 } else
3399 {
3400 IkReal op[4+1], zeror[4];
3401 int numroots;
3402 IkReal j4evalpoly[1];
3403 IkReal x96=new_r22*new_r22;
3404 IkReal x97=((16.0)*new_r01);
3405 IkReal x98=(new_r00*new_r22);
3406 IkReal x99=(x96*x97);
3407 IkReal x100=((((8.0)*x98))+(((-8.0)*new_r11)));
3408 op[0]=x100;
3409 op[1]=(x99+(((-1.0)*x97)));
3410 op[2]=((((-32.0)*new_r11*x96))+(((16.0)*x98))+(((16.0)*new_r11)));
3411 op[3]=(x97+(((-1.0)*x99)));
3412 op[4]=x100;
3413 polyroots4(op,zeror,numroots);
3414 IkReal j4array[4], cj4array[4], sj4array[4], tempj4array[1];
3415 int numsolutions = 0;
3416 for(int ij4 = 0; ij4 < numroots; ++ij4)
3417 {
3418 IkReal htj4 = zeror[ij4];
3419 tempj4array[0]=((2.0)*(atan(htj4)));
3420 for(int kj4 = 0; kj4 < 1; ++kj4)
3421 {
3422 j4array[numsolutions] = tempj4array[kj4];
3423 if( j4array[numsolutions] > IKPI )
3424 {
3425  j4array[numsolutions]-=IK2PI;
3426 }
3427 else if( j4array[numsolutions] < -IKPI )
3428 {
3429  j4array[numsolutions]+=IK2PI;
3430 }
3431 sj4array[numsolutions] = IKsin(j4array[numsolutions]);
3432 cj4array[numsolutions] = IKcos(j4array[numsolutions]);
3433 numsolutions++;
3434 }
3435 }
3436 bool j4valid[4]={true,true,true,true};
3437 _nj4 = 4;
3438 for(int ij4 = 0; ij4 < numsolutions; ++ij4)
3439  {
3440 if( !j4valid[ij4] )
3441 {
3442  continue;
3443 }
3444  j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
3445 htj4 = IKtan(j4/2);
3446 
3447 IkReal x101=new_r22*new_r22;
3448 IkReal x102=((16.0)*new_r10);
3449 IkReal x103=(new_r11*new_r22);
3450 IkReal x104=((8.0)*x103);
3451 IkReal x105=(new_r00*x101);
3452 IkReal x106=(x101*x102);
3453 IkReal x107=((8.0)*x105);
3454 j4evalpoly[0]=((((htj4*htj4)*(((((32.0)*new_r00))+(((-16.0)*x103))+(((-16.0)*x105))))))+(((-1.0)*x104))+x107+((htj4*(((((-1.0)*x102))+x106))))+(((htj4*htj4*htj4)*(((((-1.0)*x106))+x102))))+(((htj4*htj4*htj4*htj4)*(((((-1.0)*x104))+x107)))));
3455 if( IKabs(j4evalpoly[0]) > 0.0000001000000000 )
3456 {
3457  continue;
3458 }
3459 _ij4[0] = ij4; _ij4[1] = -1;
3460 for(int iij4 = ij4+1; iij4 < numsolutions; ++iij4)
3461 {
3462 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
3463 {
3464  j4valid[iij4]=false; _ij4[1] = iij4; break;
3465 }
3466 }
3467 {
3468 IkReal j6eval[3];
3469 new_r21=0;
3470 new_r20=0;
3471 new_r02=0;
3472 new_r12=0;
3473 IkReal x108=new_r22*new_r22;
3474 IkReal x109=cj4*cj4;
3475 IkReal x110=(new_r22*sj4);
3476 IkReal x111=(((x108*x109))+(((-1.0)*x108))+(((-1.0)*x109)));
3477 j6eval[0]=x111;
3478 j6eval[1]=IKsign(x111);
3479 j6eval[2]=((IKabs(((((-1.0)*cj4*new_r00))+((new_r01*x110)))))+(IKabs((((cj4*new_r01))+((new_r00*x110))))));
3480 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
3481 {
3482 {
3483 IkReal j6eval[1];
3484 new_r21=0;
3485 new_r20=0;
3486 new_r02=0;
3487 new_r12=0;
3488 j6eval[0]=new_r22;
3489 if( IKabs(j6eval[0]) < 0.0000010000000000 )
3490 {
3491 {
3492 IkReal j6eval[1];
3493 new_r21=0;
3494 new_r20=0;
3495 new_r02=0;
3496 new_r12=0;
3497 j6eval[0]=cj4;
3498 if( IKabs(j6eval[0]) < 0.0000010000000000 )
3499 {
3500 {
3501 IkReal evalcond[1];
3502 bool bgotonextstatement = true;
3503 do
3504 {
3505 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
3506 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3507 {
3508 bgotonextstatement=false;
3509 {
3510 IkReal j6array[1], cj6array[1], sj6array[1];
3511 bool j6valid[1]={false};
3512 _nj6 = 1;
3513 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
3514  continue;
3515 j6array[0]=IKatan2(((-1.0)*new_r11), new_r10);
3516 sj6array[0]=IKsin(j6array[0]);
3517 cj6array[0]=IKcos(j6array[0]);
3518 if( j6array[0] > IKPI )
3519 {
3520  j6array[0]-=IK2PI;
3521 }
3522 else if( j6array[0] < -IKPI )
3523 { j6array[0]+=IK2PI;
3524 }
3525 j6valid[0] = true;
3526 for(int ij6 = 0; ij6 < 1; ++ij6)
3527 {
3528 if( !j6valid[ij6] )
3529 {
3530  continue;
3531 }
3532 _ij6[0] = ij6; _ij6[1] = -1;
3533 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
3534 {
3535 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
3536 {
3537  j6valid[iij6]=false; _ij6[1] = iij6; break;
3538 }
3539 }
3540 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3541 {
3542 IkReal evalcond[4];
3543 IkReal x112=IKsin(j6);
3544 IkReal x113=IKcos(j6);
3545 evalcond[0]=(x112+new_r11);
3546 evalcond[1]=((-1.0)*x112);
3547 evalcond[2]=((-1.0)*x113);
3548 evalcond[3]=(new_r10+(((-1.0)*x113)));
3549 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3550 {
3551 continue;
3552 }
3553 }
3554 
3555 {
3556 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3557 vinfos[0].jointtype = 17;
3558 vinfos[0].foffset = j0;
3559 vinfos[0].indices[0] = _ij0[0];
3560 vinfos[0].indices[1] = _ij0[1];
3561 vinfos[0].maxsolutions = _nj0;
3562 vinfos[1].jointtype = 1;
3563 vinfos[1].foffset = j1;
3564 vinfos[1].indices[0] = _ij1[0];
3565 vinfos[1].indices[1] = _ij1[1];
3566 vinfos[1].maxsolutions = _nj1;
3567 vinfos[2].jointtype = 1;
3568 vinfos[2].foffset = j2;
3569 vinfos[2].indices[0] = _ij2[0];
3570 vinfos[2].indices[1] = _ij2[1];
3571 vinfos[2].maxsolutions = _nj2;
3572 vinfos[3].jointtype = 1;
3573 vinfos[3].foffset = j3;
3574 vinfos[3].indices[0] = _ij3[0];
3575 vinfos[3].indices[1] = _ij3[1];
3576 vinfos[3].maxsolutions = _nj3;
3577 vinfos[4].jointtype = 1;
3578 vinfos[4].foffset = j4;
3579 vinfos[4].indices[0] = _ij4[0];
3580 vinfos[4].indices[1] = _ij4[1];
3581 vinfos[4].maxsolutions = _nj4;
3582 vinfos[5].jointtype = 1;
3583 vinfos[5].foffset = j5;
3584 vinfos[5].indices[0] = _ij5[0];
3585 vinfos[5].indices[1] = _ij5[1];
3586 vinfos[5].maxsolutions = _nj5;
3587 vinfos[6].jointtype = 1;
3588 vinfos[6].foffset = j6;
3589 vinfos[6].indices[0] = _ij6[0];
3590 vinfos[6].indices[1] = _ij6[1];
3591 vinfos[6].maxsolutions = _nj6;
3592 std::vector<int> vfree(0);
3593 solutions.AddSolution(vinfos,vfree);
3594 }
3595 }
3596 }
3597 
3598 }
3599 } while(0);
3600 if( bgotonextstatement )
3601 {
3602 bool bgotonextstatement = true;
3603 do
3604 {
3605 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
3606 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3607 {
3608 bgotonextstatement=false;
3609 {
3610 IkReal j6array[1], cj6array[1], sj6array[1];
3611 bool j6valid[1]={false};
3612 _nj6 = 1;
3613 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
3614  continue;
3615 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
3616 sj6array[0]=IKsin(j6array[0]);
3617 cj6array[0]=IKcos(j6array[0]);
3618 if( j6array[0] > IKPI )
3619 {
3620  j6array[0]-=IK2PI;
3621 }
3622 else if( j6array[0] < -IKPI )
3623 { j6array[0]+=IK2PI;
3624 }
3625 j6valid[0] = true;
3626 for(int ij6 = 0; ij6 < 1; ++ij6)
3627 {
3628 if( !j6valid[ij6] )
3629 {
3630  continue;
3631 }
3632 _ij6[0] = ij6; _ij6[1] = -1;
3633 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
3634 {
3635 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
3636 {
3637  j6valid[iij6]=false; _ij6[1] = iij6; break;
3638 }
3639 }
3640 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3641 {
3642 IkReal evalcond[4];
3643 IkReal x114=IKsin(j6);
3644 IkReal x115=IKcos(j6);
3645 evalcond[0]=((-1.0)*x114);
3646 evalcond[1]=((-1.0)*x115);
3647 evalcond[2]=(x114+(((-1.0)*new_r11)));
3648 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x115)));
3649 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3650 {
3651 continue;
3652 }
3653 }
3654 
3655 {
3656 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3657 vinfos[0].jointtype = 17;
3658 vinfos[0].foffset = j0;
3659 vinfos[0].indices[0] = _ij0[0];
3660 vinfos[0].indices[1] = _ij0[1];
3661 vinfos[0].maxsolutions = _nj0;
3662 vinfos[1].jointtype = 1;
3663 vinfos[1].foffset = j1;
3664 vinfos[1].indices[0] = _ij1[0];
3665 vinfos[1].indices[1] = _ij1[1];
3666 vinfos[1].maxsolutions = _nj1;
3667 vinfos[2].jointtype = 1;
3668 vinfos[2].foffset = j2;
3669 vinfos[2].indices[0] = _ij2[0];
3670 vinfos[2].indices[1] = _ij2[1];
3671 vinfos[2].maxsolutions = _nj2;
3672 vinfos[3].jointtype = 1;
3673 vinfos[3].foffset = j3;
3674 vinfos[3].indices[0] = _ij3[0];
3675 vinfos[3].indices[1] = _ij3[1];
3676 vinfos[3].maxsolutions = _nj3;
3677 vinfos[4].jointtype = 1;
3678 vinfos[4].foffset = j4;
3679 vinfos[4].indices[0] = _ij4[0];
3680 vinfos[4].indices[1] = _ij4[1];
3681 vinfos[4].maxsolutions = _nj4;
3682 vinfos[5].jointtype = 1;
3683 vinfos[5].foffset = j5;
3684 vinfos[5].indices[0] = _ij5[0];
3685 vinfos[5].indices[1] = _ij5[1];
3686 vinfos[5].maxsolutions = _nj5;
3687 vinfos[6].jointtype = 1;
3688 vinfos[6].foffset = j6;
3689 vinfos[6].indices[0] = _ij6[0];
3690 vinfos[6].indices[1] = _ij6[1];
3691 vinfos[6].maxsolutions = _nj6;
3692 std::vector<int> vfree(0);
3693 solutions.AddSolution(vinfos,vfree);
3694 }
3695 }
3696 }
3697 
3698 }
3699 } while(0);
3700 if( bgotonextstatement )
3701 {
3702 bool bgotonextstatement = true;
3703 do
3704 {
3705 IkReal x116=new_r22*new_r22;
3706 CheckValue<IkReal> x117=IKPowWithIntegerCheck(((-1.0)+x116),-1);
3707 if(!x117.valid){
3708 continue;
3709 }
3710 if(((x116*(x117.value))) < -0.00001)
3711 continue;
3712 IkReal gconst30=IKsqrt((x116*(x117.value)));
3713 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj4)))))+(IKabs((cj4+(((-1.0)*gconst30)))))), 6.28318530717959)));
3714 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3715 {
3716 bgotonextstatement=false;
3717 {
3718 IkReal j6eval[1];
3719 IkReal x118=new_r22*new_r22;
3720 new_r21=0;
3721 new_r20=0;
3722 new_r02=0;
3723 new_r12=0;
3724 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
3725 continue;
3726 sj4=IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))));
3727 cj4=gconst30;
3728 if( (gconst30) < -1-IKFAST_SINCOS_THRESH || (gconst30) > 1+IKFAST_SINCOS_THRESH )
3729  continue;
3730 j4=IKacos(gconst30);
3731 CheckValue<IkReal> x119=IKPowWithIntegerCheck(((-1.0)+x118),-1);
3732 if(!x119.valid){
3733 continue;
3734 }
3735 if(((x118*(x119.value))) < -0.00001)
3736 continue;
3737 IkReal gconst30=IKsqrt((x118*(x119.value)));
3738 j6eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
3739 if( IKabs(j6eval[0]) < 0.0000010000000000 )
3740 {
3741 {
3742 IkReal j6array[1], cj6array[1], sj6array[1];
3743 bool j6valid[1]={false};
3744 _nj6 = 1;
3745 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
3746 continue;
3747 CheckValue<IkReal> x120=IKPowWithIntegerCheck(gconst30,-1);
3748 if(!x120.valid){
3749 continue;
3750 }
3751 if( IKabs(((((-1.0)*gconst30*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x120.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst30*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30))))))))))+IKsqr((new_r00*(x120.value)))-1) <= IKFAST_SINCOS_THRESH )
3752  continue;
3753 j6array[0]=IKatan2(((((-1.0)*gconst30*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30))))))))), (new_r00*(x120.value)));
3754 sj6array[0]=IKsin(j6array[0]);
3755 cj6array[0]=IKcos(j6array[0]);
3756 if( j6array[0] > IKPI )
3757 {
3758  j6array[0]-=IK2PI;
3759 }
3760 else if( j6array[0] < -IKPI )
3761 { j6array[0]+=IK2PI;
3762 }
3763 j6valid[0] = true;
3764 for(int ij6 = 0; ij6 < 1; ++ij6)
3765 {
3766 if( !j6valid[ij6] )
3767 {
3768  continue;
3769 }
3770 _ij6[0] = ij6; _ij6[1] = -1;
3771 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
3772 {
3773 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
3774 {
3775  j6valid[iij6]=false; _ij6[1] = iij6; break;
3776 }
3777 }
3778 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3779 {
3780 IkReal evalcond[8];
3781 IkReal x121=IKsin(j6);
3782 IkReal x122=IKcos(j6);
3783 IkReal x123=((1.0)*x122);
3784 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
3785 continue;
3786 IkReal x124=IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))));
3787 evalcond[0]=((-1.0)*x121);
3788 evalcond[1]=((-1.0)*x122);
3789 evalcond[2]=(((gconst30*x121))+new_r01);
3790 evalcond[3]=((((-1.0)*gconst30*x123))+new_r00);
3791 evalcond[4]=(((x121*x124))+new_r11);
3792 evalcond[5]=((((-1.0)*x123*x124))+new_r10);
3793 evalcond[6]=(x121+((new_r11*x124))+((gconst30*new_r01)));
3794 evalcond[7]=((((-1.0)*x123))+((gconst30*new_r00))+((new_r10*x124)));
3795 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3796 {
3797 continue;
3798 }
3799 }
3800 
3801 {
3802 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3803 vinfos[0].jointtype = 17;
3804 vinfos[0].foffset = j0;
3805 vinfos[0].indices[0] = _ij0[0];
3806 vinfos[0].indices[1] = _ij0[1];
3807 vinfos[0].maxsolutions = _nj0;
3808 vinfos[1].jointtype = 1;
3809 vinfos[1].foffset = j1;
3810 vinfos[1].indices[0] = _ij1[0];
3811 vinfos[1].indices[1] = _ij1[1];
3812 vinfos[1].maxsolutions = _nj1;
3813 vinfos[2].jointtype = 1;
3814 vinfos[2].foffset = j2;
3815 vinfos[2].indices[0] = _ij2[0];
3816 vinfos[2].indices[1] = _ij2[1];
3817 vinfos[2].maxsolutions = _nj2;
3818 vinfos[3].jointtype = 1;
3819 vinfos[3].foffset = j3;
3820 vinfos[3].indices[0] = _ij3[0];
3821 vinfos[3].indices[1] = _ij3[1];
3822 vinfos[3].maxsolutions = _nj3;
3823 vinfos[4].jointtype = 1;
3824 vinfos[4].foffset = j4;
3825 vinfos[4].indices[0] = _ij4[0];
3826 vinfos[4].indices[1] = _ij4[1];
3827 vinfos[4].maxsolutions = _nj4;
3828 vinfos[5].jointtype = 1;
3829 vinfos[5].foffset = j5;
3830 vinfos[5].indices[0] = _ij5[0];
3831 vinfos[5].indices[1] = _ij5[1];
3832 vinfos[5].maxsolutions = _nj5;
3833 vinfos[6].jointtype = 1;
3834 vinfos[6].foffset = j6;
3835 vinfos[6].indices[0] = _ij6[0];
3836 vinfos[6].indices[1] = _ij6[1];
3837 vinfos[6].maxsolutions = _nj6;
3838 std::vector<int> vfree(0);
3839 solutions.AddSolution(vinfos,vfree);
3840 }
3841 }
3842 }
3843 
3844 } else
3845 {
3846 {
3847 IkReal j6array[1], cj6array[1], sj6array[1];
3848 bool j6valid[1]={false};
3849 _nj6 = 1;
3850 CheckValue<IkReal> x125=IKPowWithIntegerCheck(IKsign(gconst30),-1);
3851 if(!x125.valid){
3852 continue;
3853 }
3854 CheckValue<IkReal> x126 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
3855 if(!x126.valid){
3856 continue;
3857 }
3858 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x125.value)))+(x126.value));
3859 sj6array[0]=IKsin(j6array[0]);
3860 cj6array[0]=IKcos(j6array[0]);
3861 if( j6array[0] > IKPI )
3862 {
3863  j6array[0]-=IK2PI;
3864 }
3865 else if( j6array[0] < -IKPI )
3866 { j6array[0]+=IK2PI;
3867 }
3868 j6valid[0] = true;
3869 for(int ij6 = 0; ij6 < 1; ++ij6)
3870 {
3871 if( !j6valid[ij6] )
3872 {
3873  continue;
3874 }
3875 _ij6[0] = ij6; _ij6[1] = -1;
3876 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
3877 {
3878 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
3879 {
3880  j6valid[iij6]=false; _ij6[1] = iij6; break;
3881 }
3882 }
3883 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3884 {
3885 IkReal evalcond[8];
3886 IkReal x127=IKsin(j6);
3887 IkReal x128=IKcos(j6);
3888 IkReal x129=((1.0)*x128);
3889 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
3890 continue;
3891 IkReal x130=IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))));
3892 evalcond[0]=((-1.0)*x127);
3893 evalcond[1]=((-1.0)*x128);
3894 evalcond[2]=(((gconst30*x127))+new_r01);
3895 evalcond[3]=((((-1.0)*gconst30*x129))+new_r00);
3896 evalcond[4]=(((x127*x130))+new_r11);
3897 evalcond[5]=((((-1.0)*x129*x130))+new_r10);
3898 evalcond[6]=(x127+((gconst30*new_r01))+((new_r11*x130)));
3899 evalcond[7]=((((-1.0)*x129))+((new_r10*x130))+((gconst30*new_r00)));
3900 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3901 {
3902 continue;
3903 }
3904 }
3905 
3906 {
3907 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3908 vinfos[0].jointtype = 17;
3909 vinfos[0].foffset = j0;
3910 vinfos[0].indices[0] = _ij0[0];
3911 vinfos[0].indices[1] = _ij0[1];
3912 vinfos[0].maxsolutions = _nj0;
3913 vinfos[1].jointtype = 1;
3914 vinfos[1].foffset = j1;
3915 vinfos[1].indices[0] = _ij1[0];
3916 vinfos[1].indices[1] = _ij1[1];
3917 vinfos[1].maxsolutions = _nj1;
3918 vinfos[2].jointtype = 1;
3919 vinfos[2].foffset = j2;
3920 vinfos[2].indices[0] = _ij2[0];
3921 vinfos[2].indices[1] = _ij2[1];
3922 vinfos[2].maxsolutions = _nj2;
3923 vinfos[3].jointtype = 1;
3924 vinfos[3].foffset = j3;
3925 vinfos[3].indices[0] = _ij3[0];
3926 vinfos[3].indices[1] = _ij3[1];
3927 vinfos[3].maxsolutions = _nj3;
3928 vinfos[4].jointtype = 1;
3929 vinfos[4].foffset = j4;
3930 vinfos[4].indices[0] = _ij4[0];
3931 vinfos[4].indices[1] = _ij4[1];
3932 vinfos[4].maxsolutions = _nj4;
3933 vinfos[5].jointtype = 1;
3934 vinfos[5].foffset = j5;
3935 vinfos[5].indices[0] = _ij5[0];
3936 vinfos[5].indices[1] = _ij5[1];
3937 vinfos[5].maxsolutions = _nj5;
3938 vinfos[6].jointtype = 1;
3939 vinfos[6].foffset = j6;
3940 vinfos[6].indices[0] = _ij6[0];
3941 vinfos[6].indices[1] = _ij6[1];
3942 vinfos[6].maxsolutions = _nj6;
3943 std::vector<int> vfree(0);
3944 solutions.AddSolution(vinfos,vfree);
3945 }
3946 }
3947 }
3948 
3949 }
3950 
3951 }
3952 
3953 }
3954 } while(0);
3955 if( bgotonextstatement )
3956 {
3957 bool bgotonextstatement = true;
3958 do
3959 {
3960 IkReal x131=new_r22*new_r22;
3961 CheckValue<IkReal> x132=IKPowWithIntegerCheck(((-1.0)+x131),-1);
3962 if(!x132.valid){
3963 continue;
3964 }
3965 if(((x131*(x132.value))) < -0.00001)
3966 continue;
3967 IkReal gconst30=IKsqrt((x131*(x132.value)));
3968 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj4+(((-1.0)*gconst30)))))+(IKabs(((1.0)+(IKsign(sj4)))))), 6.28318530717959)));
3969 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3970 {
3971 bgotonextstatement=false;
3972 {
3973 IkReal j6eval[1];
3974 IkReal x133=new_r22*new_r22;
3975 new_r21=0;
3976 new_r20=0;
3977 new_r02=0;
3978 new_r12=0;
3979 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
3980 continue;
3981 sj4=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))))));
3982 cj4=gconst30;
3983 if( (gconst30) < -1-IKFAST_SINCOS_THRESH || (gconst30) > 1+IKFAST_SINCOS_THRESH )
3984  continue;
3985 j4=((-1.0)*(IKacos(gconst30)));
3986 CheckValue<IkReal> x134=IKPowWithIntegerCheck(((-1.0)+x133),-1);
3987 if(!x134.valid){
3988 continue;
3989 }
3990 if(((x133*(x134.value))) < -0.00001)
3991 continue;
3992 IkReal gconst30=IKsqrt((x133*(x134.value)));
3993 j6eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
3994 if( IKabs(j6eval[0]) < 0.0000010000000000 )
3995 {
3996 {
3997 IkReal j6array[1], cj6array[1], sj6array[1];
3998 bool j6valid[1]={false};
3999 _nj6 = 1;
4000 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
4001 continue;
4002 CheckValue<IkReal> x135=IKPowWithIntegerCheck(gconst30,-1);
4003 if(!x135.valid){
4004 continue;
4005 }
4006 if( IKabs((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30))))))))+(((-1.0)*gconst30*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x135.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30))))))))+(((-1.0)*gconst30*new_r01))))+IKsqr((new_r00*(x135.value)))-1) <= IKFAST_SINCOS_THRESH )
4007  continue;
4008 j6array[0]=IKatan2((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30))))))))+(((-1.0)*gconst30*new_r01))), (new_r00*(x135.value)));
4009 sj6array[0]=IKsin(j6array[0]);
4010 cj6array[0]=IKcos(j6array[0]);
4011 if( j6array[0] > IKPI )
4012 {
4013  j6array[0]-=IK2PI;
4014 }
4015 else if( j6array[0] < -IKPI )
4016 { j6array[0]+=IK2PI;
4017 }
4018 j6valid[0] = true;
4019 for(int ij6 = 0; ij6 < 1; ++ij6)
4020 {
4021 if( !j6valid[ij6] )
4022 {
4023  continue;
4024 }
4025 _ij6[0] = ij6; _ij6[1] = -1;
4026 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4027 {
4028 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4029 {
4030  j6valid[iij6]=false; _ij6[1] = iij6; break;
4031 }
4032 }
4033 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4034 {
4035 IkReal evalcond[8];
4036 IkReal x136=IKsin(j6);
4037 IkReal x137=IKcos(j6);
4038 IkReal x138=((1.0)*x137);
4039 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
4040 continue;
4041 IkReal x139=IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))));
4042 IkReal x140=((1.0)*x139);
4043 evalcond[0]=((-1.0)*x136);
4044 evalcond[1]=((-1.0)*x137);
4045 evalcond[2]=(((gconst30*x136))+new_r01);
4046 evalcond[3]=((((-1.0)*gconst30*x138))+new_r00);
4047 evalcond[4]=(((x137*x139))+new_r10);
4048 evalcond[5]=(new_r11+(((-1.0)*x136*x140)));
4049 evalcond[6]=((((-1.0)*new_r11*x140))+x136+((gconst30*new_r01)));
4050 evalcond[7]=((((-1.0)*x138))+(((-1.0)*new_r10*x140))+((gconst30*new_r00)));
4051 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4052 {
4053 continue;
4054 }
4055 }
4056 
4057 {
4058 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4059 vinfos[0].jointtype = 17;
4060 vinfos[0].foffset = j0;
4061 vinfos[0].indices[0] = _ij0[0];
4062 vinfos[0].indices[1] = _ij0[1];
4063 vinfos[0].maxsolutions = _nj0;
4064 vinfos[1].jointtype = 1;
4065 vinfos[1].foffset = j1;
4066 vinfos[1].indices[0] = _ij1[0];
4067 vinfos[1].indices[1] = _ij1[1];
4068 vinfos[1].maxsolutions = _nj1;
4069 vinfos[2].jointtype = 1;
4070 vinfos[2].foffset = j2;
4071 vinfos[2].indices[0] = _ij2[0];
4072 vinfos[2].indices[1] = _ij2[1];
4073 vinfos[2].maxsolutions = _nj2;
4074 vinfos[3].jointtype = 1;
4075 vinfos[3].foffset = j3;
4076 vinfos[3].indices[0] = _ij3[0];
4077 vinfos[3].indices[1] = _ij3[1];
4078 vinfos[3].maxsolutions = _nj3;
4079 vinfos[4].jointtype = 1;
4080 vinfos[4].foffset = j4;
4081 vinfos[4].indices[0] = _ij4[0];
4082 vinfos[4].indices[1] = _ij4[1];
4083 vinfos[4].maxsolutions = _nj4;
4084 vinfos[5].jointtype = 1;
4085 vinfos[5].foffset = j5;
4086 vinfos[5].indices[0] = _ij5[0];
4087 vinfos[5].indices[1] = _ij5[1];
4088 vinfos[5].maxsolutions = _nj5;
4089 vinfos[6].jointtype = 1;
4090 vinfos[6].foffset = j6;
4091 vinfos[6].indices[0] = _ij6[0];
4092 vinfos[6].indices[1] = _ij6[1];
4093 vinfos[6].maxsolutions = _nj6;
4094 std::vector<int> vfree(0);
4095 solutions.AddSolution(vinfos,vfree);
4096 }
4097 }
4098 }
4099 
4100 } else
4101 {
4102 {
4103 IkReal j6array[1], cj6array[1], sj6array[1];
4104 bool j6valid[1]={false};
4105 _nj6 = 1;
4106 CheckValue<IkReal> x141=IKPowWithIntegerCheck(IKsign(gconst30),-1);
4107 if(!x141.valid){
4108 continue;
4109 }
4110 CheckValue<IkReal> x142 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
4111 if(!x142.valid){
4112 continue;
4113 }
4114 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x141.value)))+(x142.value));
4115 sj6array[0]=IKsin(j6array[0]);
4116 cj6array[0]=IKcos(j6array[0]);
4117 if( j6array[0] > IKPI )
4118 {
4119  j6array[0]-=IK2PI;
4120 }
4121 else if( j6array[0] < -IKPI )
4122 { j6array[0]+=IK2PI;
4123 }
4124 j6valid[0] = true;
4125 for(int ij6 = 0; ij6 < 1; ++ij6)
4126 {
4127 if( !j6valid[ij6] )
4128 {
4129  continue;
4130 }
4131 _ij6[0] = ij6; _ij6[1] = -1;
4132 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4133 {
4134 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4135 {
4136  j6valid[iij6]=false; _ij6[1] = iij6; break;
4137 }
4138 }
4139 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4140 {
4141 IkReal evalcond[8];
4142 IkReal x143=IKsin(j6);
4143 IkReal x144=IKcos(j6);
4144 IkReal x145=((1.0)*x144);
4145 if((((1.0)+(((-1.0)*(gconst30*gconst30))))) < -0.00001)
4146 continue;
4147 IkReal x146=IKsqrt(((1.0)+(((-1.0)*(gconst30*gconst30)))));
4148 IkReal x147=((1.0)*x146);
4149 evalcond[0]=((-1.0)*x143);
4150 evalcond[1]=((-1.0)*x144);
4151 evalcond[2]=(((gconst30*x143))+new_r01);
4152 evalcond[3]=((((-1.0)*gconst30*x145))+new_r00);
4153 evalcond[4]=(((x144*x146))+new_r10);
4154 evalcond[5]=((((-1.0)*x143*x147))+new_r11);
4155 evalcond[6]=((((-1.0)*new_r11*x147))+x143+((gconst30*new_r01)));
4156 evalcond[7]=((((-1.0)*x145))+(((-1.0)*new_r10*x147))+((gconst30*new_r00)));
4157 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4158 {
4159 continue;
4160 }
4161 }
4162 
4163 {
4164 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4165 vinfos[0].jointtype = 17;
4166 vinfos[0].foffset = j0;
4167 vinfos[0].indices[0] = _ij0[0];
4168 vinfos[0].indices[1] = _ij0[1];
4169 vinfos[0].maxsolutions = _nj0;
4170 vinfos[1].jointtype = 1;
4171 vinfos[1].foffset = j1;
4172 vinfos[1].indices[0] = _ij1[0];
4173 vinfos[1].indices[1] = _ij1[1];
4174 vinfos[1].maxsolutions = _nj1;
4175 vinfos[2].jointtype = 1;
4176 vinfos[2].foffset = j2;
4177 vinfos[2].indices[0] = _ij2[0];
4178 vinfos[2].indices[1] = _ij2[1];
4179 vinfos[2].maxsolutions = _nj2;
4180 vinfos[3].jointtype = 1;
4181 vinfos[3].foffset = j3;
4182 vinfos[3].indices[0] = _ij3[0];
4183 vinfos[3].indices[1] = _ij3[1];
4184 vinfos[3].maxsolutions = _nj3;
4185 vinfos[4].jointtype = 1;
4186 vinfos[4].foffset = j4;
4187 vinfos[4].indices[0] = _ij4[0];
4188 vinfos[4].indices[1] = _ij4[1];
4189 vinfos[4].maxsolutions = _nj4;
4190 vinfos[5].jointtype = 1;
4191 vinfos[5].foffset = j5;
4192 vinfos[5].indices[0] = _ij5[0];
4193 vinfos[5].indices[1] = _ij5[1];
4194 vinfos[5].maxsolutions = _nj5;
4195 vinfos[6].jointtype = 1;
4196 vinfos[6].foffset = j6;
4197 vinfos[6].indices[0] = _ij6[0];
4198 vinfos[6].indices[1] = _ij6[1];
4199 vinfos[6].maxsolutions = _nj6;
4200 std::vector<int> vfree(0);
4201 solutions.AddSolution(vinfos,vfree);
4202 }
4203 }
4204 }
4205 
4206 }
4207 
4208 }
4209 
4210 }
4211 } while(0);
4212 if( bgotonextstatement )
4213 {
4214 bool bgotonextstatement = true;
4215 do
4216 {
4217 IkReal x148=new_r22*new_r22;
4218 CheckValue<IkReal> x149=IKPowWithIntegerCheck(((-1.0)+x148),-1);
4219 if(!x149.valid){
4220 continue;
4221 }
4222 if(((x148*(x149.value))) < -0.00001)
4223 continue;
4224 IkReal gconst31=((-1.0)*(IKsqrt((x148*(x149.value)))));
4225 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj4)))))+(IKabs((cj4+(((-1.0)*gconst31)))))), 6.28318530717959)));
4226 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4227 {
4228 bgotonextstatement=false;
4229 {
4230 IkReal j6eval[1];
4231 IkReal x150=new_r22*new_r22;
4232 new_r21=0;
4233 new_r20=0;
4234 new_r02=0;
4235 new_r12=0;
4236 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4237 continue;
4238 sj4=IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))));
4239 cj4=gconst31;
4240 if( (gconst31) < -1-IKFAST_SINCOS_THRESH || (gconst31) > 1+IKFAST_SINCOS_THRESH )
4241  continue;
4242 j4=IKacos(gconst31);
4243 CheckValue<IkReal> x151=IKPowWithIntegerCheck(((-1.0)+x150),-1);
4244 if(!x151.valid){
4245 continue;
4246 }
4247 if(((x150*(x151.value))) < -0.00001)
4248 continue;
4249 IkReal gconst31=((-1.0)*(IKsqrt((x150*(x151.value)))));
4250 j6eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
4251 if( IKabs(j6eval[0]) < 0.0000010000000000 )
4252 {
4253 {
4254 IkReal j6array[1], cj6array[1], sj6array[1];
4255 bool j6valid[1]={false};
4256 _nj6 = 1;
4257 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4258 continue;
4259 CheckValue<IkReal> x152=IKPowWithIntegerCheck(gconst31,-1);
4260 if(!x152.valid){
4261 continue;
4262 }
4263 if( IKabs(((((-1.0)*gconst31*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x152.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst31*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31))))))))))+IKsqr((new_r00*(x152.value)))-1) <= IKFAST_SINCOS_THRESH )
4264  continue;
4265 j6array[0]=IKatan2(((((-1.0)*gconst31*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31))))))))), (new_r00*(x152.value)));
4266 sj6array[0]=IKsin(j6array[0]);
4267 cj6array[0]=IKcos(j6array[0]);
4268 if( j6array[0] > IKPI )
4269 {
4270  j6array[0]-=IK2PI;
4271 }
4272 else if( j6array[0] < -IKPI )
4273 { j6array[0]+=IK2PI;
4274 }
4275 j6valid[0] = true;
4276 for(int ij6 = 0; ij6 < 1; ++ij6)
4277 {
4278 if( !j6valid[ij6] )
4279 {
4280  continue;
4281 }
4282 _ij6[0] = ij6; _ij6[1] = -1;
4283 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4284 {
4285 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4286 {
4287  j6valid[iij6]=false; _ij6[1] = iij6; break;
4288 }
4289 }
4290 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4291 {
4292 IkReal evalcond[8];
4293 IkReal x153=IKsin(j6);
4294 IkReal x154=IKcos(j6);
4295 IkReal x155=((1.0)*x154);
4296 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4297 continue;
4298 IkReal x156=IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))));
4299 evalcond[0]=((-1.0)*x153);
4300 evalcond[1]=((-1.0)*x154);
4301 evalcond[2]=(((gconst31*x153))+new_r01);
4302 evalcond[3]=(new_r00+(((-1.0)*gconst31*x155)));
4303 evalcond[4]=(((x153*x156))+new_r11);
4304 evalcond[5]=((((-1.0)*x155*x156))+new_r10);
4305 evalcond[6]=(((new_r11*x156))+x153+((gconst31*new_r01)));
4306 evalcond[7]=((((-1.0)*x155))+((new_r10*x156))+((gconst31*new_r00)));
4307 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4308 {
4309 continue;
4310 }
4311 }
4312 
4313 {
4314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4315 vinfos[0].jointtype = 17;
4316 vinfos[0].foffset = j0;
4317 vinfos[0].indices[0] = _ij0[0];
4318 vinfos[0].indices[1] = _ij0[1];
4319 vinfos[0].maxsolutions = _nj0;
4320 vinfos[1].jointtype = 1;
4321 vinfos[1].foffset = j1;
4322 vinfos[1].indices[0] = _ij1[0];
4323 vinfos[1].indices[1] = _ij1[1];
4324 vinfos[1].maxsolutions = _nj1;
4325 vinfos[2].jointtype = 1;
4326 vinfos[2].foffset = j2;
4327 vinfos[2].indices[0] = _ij2[0];
4328 vinfos[2].indices[1] = _ij2[1];
4329 vinfos[2].maxsolutions = _nj2;
4330 vinfos[3].jointtype = 1;
4331 vinfos[3].foffset = j3;
4332 vinfos[3].indices[0] = _ij3[0];
4333 vinfos[3].indices[1] = _ij3[1];
4334 vinfos[3].maxsolutions = _nj3;
4335 vinfos[4].jointtype = 1;
4336 vinfos[4].foffset = j4;
4337 vinfos[4].indices[0] = _ij4[0];
4338 vinfos[4].indices[1] = _ij4[1];
4339 vinfos[4].maxsolutions = _nj4;
4340 vinfos[5].jointtype = 1;
4341 vinfos[5].foffset = j5;
4342 vinfos[5].indices[0] = _ij5[0];
4343 vinfos[5].indices[1] = _ij5[1];
4344 vinfos[5].maxsolutions = _nj5;
4345 vinfos[6].jointtype = 1;
4346 vinfos[6].foffset = j6;
4347 vinfos[6].indices[0] = _ij6[0];
4348 vinfos[6].indices[1] = _ij6[1];
4349 vinfos[6].maxsolutions = _nj6;
4350 std::vector<int> vfree(0);
4351 solutions.AddSolution(vinfos,vfree);
4352 }
4353 }
4354 }
4355 
4356 } else
4357 {
4358 {
4359 IkReal j6array[1], cj6array[1], sj6array[1];
4360 bool j6valid[1]={false};
4361 _nj6 = 1;
4362 CheckValue<IkReal> x157 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
4363 if(!x157.valid){
4364 continue;
4365 }
4366 CheckValue<IkReal> x158=IKPowWithIntegerCheck(IKsign(gconst31),-1);
4367 if(!x158.valid){
4368 continue;
4369 }
4370 j6array[0]=((-1.5707963267949)+(x157.value)+(((1.5707963267949)*(x158.value))));
4371 sj6array[0]=IKsin(j6array[0]);
4372 cj6array[0]=IKcos(j6array[0]);
4373 if( j6array[0] > IKPI )
4374 {
4375  j6array[0]-=IK2PI;
4376 }
4377 else if( j6array[0] < -IKPI )
4378 { j6array[0]+=IK2PI;
4379 }
4380 j6valid[0] = true;
4381 for(int ij6 = 0; ij6 < 1; ++ij6)
4382 {
4383 if( !j6valid[ij6] )
4384 {
4385  continue;
4386 }
4387 _ij6[0] = ij6; _ij6[1] = -1;
4388 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4389 {
4390 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4391 {
4392  j6valid[iij6]=false; _ij6[1] = iij6; break;
4393 }
4394 }
4395 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4396 {
4397 IkReal evalcond[8];
4398 IkReal x159=IKsin(j6);
4399 IkReal x160=IKcos(j6);
4400 IkReal x161=((1.0)*x160);
4401 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4402 continue;
4403 IkReal x162=IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))));
4404 evalcond[0]=((-1.0)*x159);
4405 evalcond[1]=((-1.0)*x160);
4406 evalcond[2]=(((gconst31*x159))+new_r01);
4407 evalcond[3]=((((-1.0)*gconst31*x161))+new_r00);
4408 evalcond[4]=(new_r11+((x159*x162)));
4409 evalcond[5]=((((-1.0)*x161*x162))+new_r10);
4410 evalcond[6]=(((new_r11*x162))+x159+((gconst31*new_r01)));
4411 evalcond[7]=((((-1.0)*x161))+((new_r10*x162))+((gconst31*new_r00)));
4412 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4413 {
4414 continue;
4415 }
4416 }
4417 
4418 {
4419 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4420 vinfos[0].jointtype = 17;
4421 vinfos[0].foffset = j0;
4422 vinfos[0].indices[0] = _ij0[0];
4423 vinfos[0].indices[1] = _ij0[1];
4424 vinfos[0].maxsolutions = _nj0;
4425 vinfos[1].jointtype = 1;
4426 vinfos[1].foffset = j1;
4427 vinfos[1].indices[0] = _ij1[0];
4428 vinfos[1].indices[1] = _ij1[1];
4429 vinfos[1].maxsolutions = _nj1;
4430 vinfos[2].jointtype = 1;
4431 vinfos[2].foffset = j2;
4432 vinfos[2].indices[0] = _ij2[0];
4433 vinfos[2].indices[1] = _ij2[1];
4434 vinfos[2].maxsolutions = _nj2;
4435 vinfos[3].jointtype = 1;
4436 vinfos[3].foffset = j3;
4437 vinfos[3].indices[0] = _ij3[0];
4438 vinfos[3].indices[1] = _ij3[1];
4439 vinfos[3].maxsolutions = _nj3;
4440 vinfos[4].jointtype = 1;
4441 vinfos[4].foffset = j4;
4442 vinfos[4].indices[0] = _ij4[0];
4443 vinfos[4].indices[1] = _ij4[1];
4444 vinfos[4].maxsolutions = _nj4;
4445 vinfos[5].jointtype = 1;
4446 vinfos[5].foffset = j5;
4447 vinfos[5].indices[0] = _ij5[0];
4448 vinfos[5].indices[1] = _ij5[1];
4449 vinfos[5].maxsolutions = _nj5;
4450 vinfos[6].jointtype = 1;
4451 vinfos[6].foffset = j6;
4452 vinfos[6].indices[0] = _ij6[0];
4453 vinfos[6].indices[1] = _ij6[1];
4454 vinfos[6].maxsolutions = _nj6;
4455 std::vector<int> vfree(0);
4456 solutions.AddSolution(vinfos,vfree);
4457 }
4458 }
4459 }
4460 
4461 }
4462 
4463 }
4464 
4465 }
4466 } while(0);
4467 if( bgotonextstatement )
4468 {
4469 bool bgotonextstatement = true;
4470 do
4471 {
4472 IkReal x163=new_r22*new_r22;
4473 CheckValue<IkReal> x164=IKPowWithIntegerCheck(((-1.0)+x163),-1);
4474 if(!x164.valid){
4475 continue;
4476 }
4477 if(((x163*(x164.value))) < -0.00001)
4478 continue;
4479 IkReal gconst31=((-1.0)*(IKsqrt((x163*(x164.value)))));
4480 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj4)))))+(IKabs((cj4+(((-1.0)*gconst31)))))), 6.28318530717959)));
4481 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4482 {
4483 bgotonextstatement=false;
4484 {
4485 IkReal j6eval[1];
4486 IkReal x165=new_r22*new_r22;
4487 new_r21=0;
4488 new_r20=0;
4489 new_r02=0;
4490 new_r12=0;
4491 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4492 continue;
4493 sj4=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))))));
4494 cj4=gconst31;
4495 if( (gconst31) < -1-IKFAST_SINCOS_THRESH || (gconst31) > 1+IKFAST_SINCOS_THRESH )
4496  continue;
4497 j4=((-1.0)*(IKacos(gconst31)));
4498 CheckValue<IkReal> x166=IKPowWithIntegerCheck(((-1.0)+x165),-1);
4499 if(!x166.valid){
4500 continue;
4501 }
4502 if(((x165*(x166.value))) < -0.00001)
4503 continue;
4504 IkReal gconst31=((-1.0)*(IKsqrt((x165*(x166.value)))));
4505 j6eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
4506 if( IKabs(j6eval[0]) < 0.0000010000000000 )
4507 {
4508 {
4509 IkReal j6array[1], cj6array[1], sj6array[1];
4510 bool j6valid[1]={false};
4511 _nj6 = 1;
4512 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4513 continue;
4514 CheckValue<IkReal> x167=IKPowWithIntegerCheck(gconst31,-1);
4515 if(!x167.valid){
4516 continue;
4517 }
4518 if( IKabs(((((-1.0)*gconst31*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x167.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst31*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31))))))))))+IKsqr((new_r00*(x167.value)))-1) <= IKFAST_SINCOS_THRESH )
4519  continue;
4520 j6array[0]=IKatan2(((((-1.0)*gconst31*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31))))))))), (new_r00*(x167.value)));
4521 sj6array[0]=IKsin(j6array[0]);
4522 cj6array[0]=IKcos(j6array[0]);
4523 if( j6array[0] > IKPI )
4524 {
4525  j6array[0]-=IK2PI;
4526 }
4527 else if( j6array[0] < -IKPI )
4528 { j6array[0]+=IK2PI;
4529 }
4530 j6valid[0] = true;
4531 for(int ij6 = 0; ij6 < 1; ++ij6)
4532 {
4533 if( !j6valid[ij6] )
4534 {
4535  continue;
4536 }
4537 _ij6[0] = ij6; _ij6[1] = -1;
4538 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4539 {
4540 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4541 {
4542  j6valid[iij6]=false; _ij6[1] = iij6; break;
4543 }
4544 }
4545 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4546 {
4547 IkReal evalcond[8];
4548 IkReal x168=IKsin(j6);
4549 IkReal x169=IKcos(j6);
4550 IkReal x170=((1.0)*x169);
4551 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4552 continue;
4553 IkReal x171=IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))));
4554 IkReal x172=((1.0)*x171);
4555 evalcond[0]=((-1.0)*x168);
4556 evalcond[1]=((-1.0)*x169);
4557 evalcond[2]=(((gconst31*x168))+new_r01);
4558 evalcond[3]=((((-1.0)*gconst31*x170))+new_r00);
4559 evalcond[4]=(((x169*x171))+new_r10);
4560 evalcond[5]=((((-1.0)*x168*x172))+new_r11);
4561 evalcond[6]=((((-1.0)*new_r11*x172))+x168+((gconst31*new_r01)));
4562 evalcond[7]=((((-1.0)*x170))+(((-1.0)*new_r10*x172))+((gconst31*new_r00)));
4563 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4564 {
4565 continue;
4566 }
4567 }
4568 
4569 {
4570 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4571 vinfos[0].jointtype = 17;
4572 vinfos[0].foffset = j0;
4573 vinfos[0].indices[0] = _ij0[0];
4574 vinfos[0].indices[1] = _ij0[1];
4575 vinfos[0].maxsolutions = _nj0;
4576 vinfos[1].jointtype = 1;
4577 vinfos[1].foffset = j1;
4578 vinfos[1].indices[0] = _ij1[0];
4579 vinfos[1].indices[1] = _ij1[1];
4580 vinfos[1].maxsolutions = _nj1;
4581 vinfos[2].jointtype = 1;
4582 vinfos[2].foffset = j2;
4583 vinfos[2].indices[0] = _ij2[0];
4584 vinfos[2].indices[1] = _ij2[1];
4585 vinfos[2].maxsolutions = _nj2;
4586 vinfos[3].jointtype = 1;
4587 vinfos[3].foffset = j3;
4588 vinfos[3].indices[0] = _ij3[0];
4589 vinfos[3].indices[1] = _ij3[1];
4590 vinfos[3].maxsolutions = _nj3;
4591 vinfos[4].jointtype = 1;
4592 vinfos[4].foffset = j4;
4593 vinfos[4].indices[0] = _ij4[0];
4594 vinfos[4].indices[1] = _ij4[1];
4595 vinfos[4].maxsolutions = _nj4;
4596 vinfos[5].jointtype = 1;
4597 vinfos[5].foffset = j5;
4598 vinfos[5].indices[0] = _ij5[0];
4599 vinfos[5].indices[1] = _ij5[1];
4600 vinfos[5].maxsolutions = _nj5;
4601 vinfos[6].jointtype = 1;
4602 vinfos[6].foffset = j6;
4603 vinfos[6].indices[0] = _ij6[0];
4604 vinfos[6].indices[1] = _ij6[1];
4605 vinfos[6].maxsolutions = _nj6;
4606 std::vector<int> vfree(0);
4607 solutions.AddSolution(vinfos,vfree);
4608 }
4609 }
4610 }
4611 
4612 } else
4613 {
4614 {
4615 IkReal j6array[1], cj6array[1], sj6array[1];
4616 bool j6valid[1]={false};
4617 _nj6 = 1;
4618 CheckValue<IkReal> x173 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
4619 if(!x173.valid){
4620 continue;
4621 }
4622 CheckValue<IkReal> x174=IKPowWithIntegerCheck(IKsign(gconst31),-1);
4623 if(!x174.valid){
4624 continue;
4625 }
4626 j6array[0]=((-1.5707963267949)+(x173.value)+(((1.5707963267949)*(x174.value))));
4627 sj6array[0]=IKsin(j6array[0]);
4628 cj6array[0]=IKcos(j6array[0]);
4629 if( j6array[0] > IKPI )
4630 {
4631  j6array[0]-=IK2PI;
4632 }
4633 else if( j6array[0] < -IKPI )
4634 { j6array[0]+=IK2PI;
4635 }
4636 j6valid[0] = true;
4637 for(int ij6 = 0; ij6 < 1; ++ij6)
4638 {
4639 if( !j6valid[ij6] )
4640 {
4641  continue;
4642 }
4643 _ij6[0] = ij6; _ij6[1] = -1;
4644 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4645 {
4646 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4647 {
4648  j6valid[iij6]=false; _ij6[1] = iij6; break;
4649 }
4650 }
4651 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4652 {
4653 IkReal evalcond[8];
4654 IkReal x175=IKsin(j6);
4655 IkReal x176=IKcos(j6);
4656 IkReal x177=((1.0)*x176);
4657 if((((1.0)+(((-1.0)*(gconst31*gconst31))))) < -0.00001)
4658 continue;
4659 IkReal x178=IKsqrt(((1.0)+(((-1.0)*(gconst31*gconst31)))));
4660 IkReal x179=((1.0)*x178);
4661 evalcond[0]=((-1.0)*x175);
4662 evalcond[1]=((-1.0)*x176);
4663 evalcond[2]=(((gconst31*x175))+new_r01);
4664 evalcond[3]=((((-1.0)*gconst31*x177))+new_r00);
4665 evalcond[4]=(((x176*x178))+new_r10);
4666 evalcond[5]=((((-1.0)*x175*x179))+new_r11);
4667 evalcond[6]=((((-1.0)*new_r11*x179))+x175+((gconst31*new_r01)));
4668 evalcond[7]=((((-1.0)*x177))+(((-1.0)*new_r10*x179))+((gconst31*new_r00)));
4669 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4670 {
4671 continue;
4672 }
4673 }
4674 
4675 {
4676 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4677 vinfos[0].jointtype = 17;
4678 vinfos[0].foffset = j0;
4679 vinfos[0].indices[0] = _ij0[0];
4680 vinfos[0].indices[1] = _ij0[1];
4681 vinfos[0].maxsolutions = _nj0;
4682 vinfos[1].jointtype = 1;
4683 vinfos[1].foffset = j1;
4684 vinfos[1].indices[0] = _ij1[0];
4685 vinfos[1].indices[1] = _ij1[1];
4686 vinfos[1].maxsolutions = _nj1;
4687 vinfos[2].jointtype = 1;
4688 vinfos[2].foffset = j2;
4689 vinfos[2].indices[0] = _ij2[0];
4690 vinfos[2].indices[1] = _ij2[1];
4691 vinfos[2].maxsolutions = _nj2;
4692 vinfos[3].jointtype = 1;
4693 vinfos[3].foffset = j3;
4694 vinfos[3].indices[0] = _ij3[0];
4695 vinfos[3].indices[1] = _ij3[1];
4696 vinfos[3].maxsolutions = _nj3;
4697 vinfos[4].jointtype = 1;
4698 vinfos[4].foffset = j4;
4699 vinfos[4].indices[0] = _ij4[0];
4700 vinfos[4].indices[1] = _ij4[1];
4701 vinfos[4].maxsolutions = _nj4;
4702 vinfos[5].jointtype = 1;
4703 vinfos[5].foffset = j5;
4704 vinfos[5].indices[0] = _ij5[0];
4705 vinfos[5].indices[1] = _ij5[1];
4706 vinfos[5].maxsolutions = _nj5;
4707 vinfos[6].jointtype = 1;
4708 vinfos[6].foffset = j6;
4709 vinfos[6].indices[0] = _ij6[0];
4710 vinfos[6].indices[1] = _ij6[1];
4711 vinfos[6].maxsolutions = _nj6;
4712 std::vector<int> vfree(0);
4713 solutions.AddSolution(vinfos,vfree);
4714 }
4715 }
4716 }
4717 
4718 }
4719 
4720 }
4721 
4722 }
4723 } while(0);
4724 if( bgotonextstatement )
4725 {
4726 bool bgotonextstatement = true;
4727 do
4728 {
4729 if( 1 )
4730 {
4731 bgotonextstatement=false;
4732 continue; // branch miss [j6]
4733 
4734 }
4735 } while(0);
4736 if( bgotonextstatement )
4737 {
4738 }
4739 }
4740 }
4741 }
4742 }
4743 }
4744 }
4745 }
4746 
4747 } else
4748 {
4749 {
4750 IkReal j6array[1], cj6array[1], sj6array[1];
4751 bool j6valid[1]={false};
4752 _nj6 = 1;
4753 IkReal x180=(new_r11*new_r22);
4754 IkReal x181=((1.0)*cj4*new_r01);
4755 CheckValue<IkReal> x182=IKPowWithIntegerCheck(cj4,-1);
4756 if(!x182.valid){
4757 continue;
4758 }
4759 if( IKabs(((((-1.0)*x181))+(((-1.0)*new_r11*sj4)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x182.value)*(((((-1.0)*x180))+(((-1.0)*new_r22*sj4*x181))+new_r00+((x180*(cj4*cj4))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x181))+(((-1.0)*new_r11*sj4))))+IKsqr(((x182.value)*(((((-1.0)*x180))+(((-1.0)*new_r22*sj4*x181))+new_r00+((x180*(cj4*cj4)))))))-1) <= IKFAST_SINCOS_THRESH )
4760  continue;
4761 j6array[0]=IKatan2(((((-1.0)*x181))+(((-1.0)*new_r11*sj4))), ((x182.value)*(((((-1.0)*x180))+(((-1.0)*new_r22*sj4*x181))+new_r00+((x180*(cj4*cj4)))))));
4762 sj6array[0]=IKsin(j6array[0]);
4763 cj6array[0]=IKcos(j6array[0]);
4764 if( j6array[0] > IKPI )
4765 {
4766  j6array[0]-=IK2PI;
4767 }
4768 else if( j6array[0] < -IKPI )
4769 { j6array[0]+=IK2PI;
4770 }
4771 j6valid[0] = true;
4772 for(int ij6 = 0; ij6 < 1; ++ij6)
4773 {
4774 if( !j6valid[ij6] )
4775 {
4776  continue;
4777 }
4778 _ij6[0] = ij6; _ij6[1] = -1;
4779 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4780 {
4781 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4782 {
4783  j6valid[iij6]=false; _ij6[1] = iij6; break;
4784 }
4785 }
4786 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4787 {
4788 IkReal evalcond[10];
4789 IkReal x183=IKcos(j6);
4790 IkReal x184=IKsin(j6);
4791 IkReal x185=(cj4*new_r22);
4792 IkReal x186=((1.0)*sj4);
4793 IkReal x187=((1.0)*x183);
4794 IkReal x188=(sj4*x184);
4795 IkReal x189=((1.0)*x184);
4796 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x184);
4797 evalcond[1]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x187)));
4798 evalcond[2]=(new_r01+((new_r22*sj4*x183))+((cj4*x184)));
4799 evalcond[3]=((((-1.0)*cj4*x187))+((new_r22*x188))+new_r00);
4800 evalcond[4]=(x188+(((-1.0)*x185*x187))+new_r11);
4801 evalcond[5]=(((cj4*new_r10))+(((-1.0)*new_r22*x189))+(((-1.0)*new_r00*x186)));
4802 evalcond[6]=(((cj4*new_r11))+(((-1.0)*new_r22*x187))+(((-1.0)*new_r01*x186)));
4803 evalcond[7]=((((-1.0)*x185*x189))+new_r10+(((-1.0)*x183*x186)));
4804 evalcond[8]=(((new_r10*x185))+(((-1.0)*x189))+(((-1.0)*new_r00*new_r22*x186)));
4805 evalcond[9]=(((new_r11*x185))+(((-1.0)*x187))+(((-1.0)*new_r01*new_r22*x186)));
4806 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
4807 {
4808 continue;
4809 }
4810 }
4811 
4812 {
4813 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4814 vinfos[0].jointtype = 17;
4815 vinfos[0].foffset = j0;
4816 vinfos[0].indices[0] = _ij0[0];
4817 vinfos[0].indices[1] = _ij0[1];
4818 vinfos[0].maxsolutions = _nj0;
4819 vinfos[1].jointtype = 1;
4820 vinfos[1].foffset = j1;
4821 vinfos[1].indices[0] = _ij1[0];
4822 vinfos[1].indices[1] = _ij1[1];
4823 vinfos[1].maxsolutions = _nj1;
4824 vinfos[2].jointtype = 1;
4825 vinfos[2].foffset = j2;
4826 vinfos[2].indices[0] = _ij2[0];
4827 vinfos[2].indices[1] = _ij2[1];
4828 vinfos[2].maxsolutions = _nj2;
4829 vinfos[3].jointtype = 1;
4830 vinfos[3].foffset = j3;
4831 vinfos[3].indices[0] = _ij3[0];
4832 vinfos[3].indices[1] = _ij3[1];
4833 vinfos[3].maxsolutions = _nj3;
4834 vinfos[4].jointtype = 1;
4835 vinfos[4].foffset = j4;
4836 vinfos[4].indices[0] = _ij4[0];
4837 vinfos[4].indices[1] = _ij4[1];
4838 vinfos[4].maxsolutions = _nj4;
4839 vinfos[5].jointtype = 1;
4840 vinfos[5].foffset = j5;
4841 vinfos[5].indices[0] = _ij5[0];
4842 vinfos[5].indices[1] = _ij5[1];
4843 vinfos[5].maxsolutions = _nj5;
4844 vinfos[6].jointtype = 1;
4845 vinfos[6].foffset = j6;
4846 vinfos[6].indices[0] = _ij6[0];
4847 vinfos[6].indices[1] = _ij6[1];
4848 vinfos[6].maxsolutions = _nj6;
4849 std::vector<int> vfree(0);
4850 solutions.AddSolution(vinfos,vfree);
4851 }
4852 }
4853 }
4854 
4855 }
4856 
4857 }
4858 
4859 } else
4860 {
4861 {
4862 IkReal j6array[1], cj6array[1], sj6array[1];
4863 bool j6valid[1]={false};
4864 _nj6 = 1;
4865 IkReal x190=((1.0)*sj4);
4866 CheckValue<IkReal> x191=IKPowWithIntegerCheck(new_r22,-1);
4867 if(!x191.valid){
4868 continue;
4869 }
4870 if( IKabs(((((-1.0)*cj4*new_r01))+(((-1.0)*new_r11*x190)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x191.value)*((((cj4*new_r11))+(((-1.0)*new_r01*x190)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj4*new_r01))+(((-1.0)*new_r11*x190))))+IKsqr(((x191.value)*((((cj4*new_r11))+(((-1.0)*new_r01*x190))))))-1) <= IKFAST_SINCOS_THRESH )
4871  continue;
4872 j6array[0]=IKatan2(((((-1.0)*cj4*new_r01))+(((-1.0)*new_r11*x190))), ((x191.value)*((((cj4*new_r11))+(((-1.0)*new_r01*x190))))));
4873 sj6array[0]=IKsin(j6array[0]);
4874 cj6array[0]=IKcos(j6array[0]);
4875 if( j6array[0] > IKPI )
4876 {
4877  j6array[0]-=IK2PI;
4878 }
4879 else if( j6array[0] < -IKPI )
4880 { j6array[0]+=IK2PI;
4881 }
4882 j6valid[0] = true;
4883 for(int ij6 = 0; ij6 < 1; ++ij6)
4884 {
4885 if( !j6valid[ij6] )
4886 {
4887  continue;
4888 }
4889 _ij6[0] = ij6; _ij6[1] = -1;
4890 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
4891 {
4892 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
4893 {
4894  j6valid[iij6]=false; _ij6[1] = iij6; break;
4895 }
4896 }
4897 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4898 {
4899 IkReal evalcond[10];
4900 IkReal x192=IKcos(j6);
4901 IkReal x193=IKsin(j6);
4902 IkReal x194=(cj4*new_r22);
4903 IkReal x195=((1.0)*sj4);
4904 IkReal x196=((1.0)*x192);
4905 IkReal x197=(sj4*x193);
4906 IkReal x198=((1.0)*x193);
4907 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x193);
4908 evalcond[1]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x196)));
4909 evalcond[2]=(((cj4*x193))+new_r01+((new_r22*sj4*x192)));
4910 evalcond[3]=((((-1.0)*cj4*x196))+((new_r22*x197))+new_r00);
4911 evalcond[4]=((((-1.0)*x194*x196))+x197+new_r11);
4912 evalcond[5]=(((cj4*new_r10))+(((-1.0)*new_r22*x198))+(((-1.0)*new_r00*x195)));
4913 evalcond[6]=(((cj4*new_r11))+(((-1.0)*new_r22*x196))+(((-1.0)*new_r01*x195)));
4914 evalcond[7]=((((-1.0)*x194*x198))+(((-1.0)*x192*x195))+new_r10);
4915 evalcond[8]=(((new_r10*x194))+(((-1.0)*new_r00*new_r22*x195))+(((-1.0)*x198)));
4916 evalcond[9]=(((new_r11*x194))+(((-1.0)*x196))+(((-1.0)*new_r01*new_r22*x195)));
4917 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
4918 {
4919 continue;
4920 }
4921 }
4922 
4923 {
4924 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4925 vinfos[0].jointtype = 17;
4926 vinfos[0].foffset = j0;
4927 vinfos[0].indices[0] = _ij0[0];
4928 vinfos[0].indices[1] = _ij0[1];
4929 vinfos[0].maxsolutions = _nj0;
4930 vinfos[1].jointtype = 1;
4931 vinfos[1].foffset = j1;
4932 vinfos[1].indices[0] = _ij1[0];
4933 vinfos[1].indices[1] = _ij1[1];
4934 vinfos[1].maxsolutions = _nj1;
4935 vinfos[2].jointtype = 1;
4936 vinfos[2].foffset = j2;
4937 vinfos[2].indices[0] = _ij2[0];
4938 vinfos[2].indices[1] = _ij2[1];
4939 vinfos[2].maxsolutions = _nj2;
4940 vinfos[3].jointtype = 1;
4941 vinfos[3].foffset = j3;
4942 vinfos[3].indices[0] = _ij3[0];
4943 vinfos[3].indices[1] = _ij3[1];
4944 vinfos[3].maxsolutions = _nj3;
4945 vinfos[4].jointtype = 1;
4946 vinfos[4].foffset = j4;
4947 vinfos[4].indices[0] = _ij4[0];
4948 vinfos[4].indices[1] = _ij4[1];
4949 vinfos[4].maxsolutions = _nj4;
4950 vinfos[5].jointtype = 1;
4951 vinfos[5].foffset = j5;
4952 vinfos[5].indices[0] = _ij5[0];
4953 vinfos[5].indices[1] = _ij5[1];
4954 vinfos[5].maxsolutions = _nj5;
4955 vinfos[6].jointtype = 1;
4956 vinfos[6].foffset = j6;
4957 vinfos[6].indices[0] = _ij6[0];
4958 vinfos[6].indices[1] = _ij6[1];
4959 vinfos[6].maxsolutions = _nj6;
4960 std::vector<int> vfree(0);
4961 solutions.AddSolution(vinfos,vfree);
4962 }
4963 }
4964 }
4965 
4966 }
4967 
4968 }
4969 
4970 } else
4971 {
4972 {
4973 IkReal j6array[1], cj6array[1], sj6array[1];
4974 bool j6valid[1]={false};
4975 _nj6 = 1;
4976 IkReal x199=new_r22*new_r22;
4977 IkReal x200=cj4*cj4;
4978 IkReal x201=(new_r22*sj4);
4979 CheckValue<IkReal> x202=IKPowWithIntegerCheck(IKsign(((((-1.0)*x199))+(((-1.0)*x200))+((x199*x200)))),-1);
4980 if(!x202.valid){
4981 continue;
4982 }
4983 CheckValue<IkReal> x203 = IKatan2WithCheck(IkReal((((cj4*new_r01))+((new_r00*x201)))),IkReal(((((-1.0)*cj4*new_r00))+((new_r01*x201)))),IKFAST_ATAN2_MAGTHRESH);
4984 if(!x203.valid){
4985 continue;
4986 }
4987 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x202.value)))+(x203.value));
4988 sj6array[0]=IKsin(j6array[0]);
4989 cj6array[0]=IKcos(j6array[0]);
4990 if( j6array[0] > IKPI )
4991 {
4992  j6array[0]-=IK2PI;
4993 }
4994 else if( j6array[0] < -IKPI )
4995 { j6array[0]+=IK2PI;
4996 }
4997 j6valid[0] = true;
4998 for(int ij6 = 0; ij6 < 1; ++ij6)
4999 {
5000 if( !j6valid[ij6] )
5001 {
5002  continue;
5003 }
5004 _ij6[0] = ij6; _ij6[1] = -1;
5005 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5006 {
5007 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5008 {
5009  j6valid[iij6]=false; _ij6[1] = iij6; break;
5010 }
5011 }
5012 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5013 {
5014 IkReal evalcond[10];
5015 IkReal x204=IKcos(j6);
5016 IkReal x205=IKsin(j6);
5017 IkReal x206=(cj4*new_r22);
5018 IkReal x207=((1.0)*sj4);
5019 IkReal x208=((1.0)*x204);
5020 IkReal x209=(sj4*x205);
5021 IkReal x210=((1.0)*x205);
5022 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x205);
5023 evalcond[1]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x208)));
5024 evalcond[2]=(((new_r22*sj4*x204))+((cj4*x205))+new_r01);
5025 evalcond[3]=((((-1.0)*cj4*x208))+((new_r22*x209))+new_r00);
5026 evalcond[4]=((((-1.0)*x206*x208))+x209+new_r11);
5027 evalcond[5]=((((-1.0)*new_r00*x207))+((cj4*new_r10))+(((-1.0)*new_r22*x210)));
5028 evalcond[6]=((((-1.0)*new_r22*x208))+((cj4*new_r11))+(((-1.0)*new_r01*x207)));
5029 evalcond[7]=((((-1.0)*x204*x207))+(((-1.0)*x206*x210))+new_r10);
5030 evalcond[8]=(((new_r10*x206))+(((-1.0)*new_r00*new_r22*x207))+(((-1.0)*x210)));
5031 evalcond[9]=(((new_r11*x206))+(((-1.0)*x208))+(((-1.0)*new_r01*new_r22*x207)));
5032 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
5033 {
5034 continue;
5035 }
5036 }
5037 
5038 {
5039 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5040 vinfos[0].jointtype = 17;
5041 vinfos[0].foffset = j0;
5042 vinfos[0].indices[0] = _ij0[0];
5043 vinfos[0].indices[1] = _ij0[1];
5044 vinfos[0].maxsolutions = _nj0;
5045 vinfos[1].jointtype = 1;
5046 vinfos[1].foffset = j1;
5047 vinfos[1].indices[0] = _ij1[0];
5048 vinfos[1].indices[1] = _ij1[1];
5049 vinfos[1].maxsolutions = _nj1;
5050 vinfos[2].jointtype = 1;
5051 vinfos[2].foffset = j2;
5052 vinfos[2].indices[0] = _ij2[0];
5053 vinfos[2].indices[1] = _ij2[1];
5054 vinfos[2].maxsolutions = _nj2;
5055 vinfos[3].jointtype = 1;
5056 vinfos[3].foffset = j3;
5057 vinfos[3].indices[0] = _ij3[0];
5058 vinfos[3].indices[1] = _ij3[1];
5059 vinfos[3].maxsolutions = _nj3;
5060 vinfos[4].jointtype = 1;
5061 vinfos[4].foffset = j4;
5062 vinfos[4].indices[0] = _ij4[0];
5063 vinfos[4].indices[1] = _ij4[1];
5064 vinfos[4].maxsolutions = _nj4;
5065 vinfos[5].jointtype = 1;
5066 vinfos[5].foffset = j5;
5067 vinfos[5].indices[0] = _ij5[0];
5068 vinfos[5].indices[1] = _ij5[1];
5069 vinfos[5].maxsolutions = _nj5;
5070 vinfos[6].jointtype = 1;
5071 vinfos[6].foffset = j6;
5072 vinfos[6].indices[0] = _ij6[0];
5073 vinfos[6].indices[1] = _ij6[1];
5074 vinfos[6].maxsolutions = _nj6;
5075 std::vector<int> vfree(0);
5076 solutions.AddSolution(vinfos,vfree);
5077 }
5078 }
5079 }
5080 
5081 }
5082 
5083 }
5084  }
5085 
5086 }
5087 
5088 }
5089 
5090 }
5091 } while(0);
5092 if( bgotonextstatement )
5093 {
5094 bool bgotonextstatement = true;
5095 do
5096 {
5097 if( 1 )
5098 {
5099 bgotonextstatement=false;
5100 continue; // branch miss [j4, j6]
5101 
5102 }
5103 } while(0);
5104 if( bgotonextstatement )
5105 {
5106 }
5107 }
5108 }
5109 }
5110 }
5111 
5112 } else
5113 {
5114 {
5115 IkReal j4array[1], cj4array[1], sj4array[1];
5116 bool j4valid[1]={false};
5117 _nj4 = 1;
5118 CheckValue<IkReal> x212=IKPowWithIntegerCheck(sj5,-1);
5119 if(!x212.valid){
5120 continue;
5121 }
5122 IkReal x211=x212.value;
5123 CheckValue<IkReal> x213=IKPowWithIntegerCheck(new_r02,-1);
5124 if(!x213.valid){
5125 continue;
5126 }
5127 if( IKabs((x211*(x213.value)*(((1.0)+(((-1.0)*(cj5*cj5)))+(((-1.0)*(new_r12*new_r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x211)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x211*(x213.value)*(((1.0)+(((-1.0)*(cj5*cj5)))+(((-1.0)*(new_r12*new_r12)))))))+IKsqr(((-1.0)*new_r12*x211))-1) <= IKFAST_SINCOS_THRESH )
5128  continue;
5129 j4array[0]=IKatan2((x211*(x213.value)*(((1.0)+(((-1.0)*(cj5*cj5)))+(((-1.0)*(new_r12*new_r12)))))), ((-1.0)*new_r12*x211));
5130 sj4array[0]=IKsin(j4array[0]);
5131 cj4array[0]=IKcos(j4array[0]);
5132 if( j4array[0] > IKPI )
5133 {
5134  j4array[0]-=IK2PI;
5135 }
5136 else if( j4array[0] < -IKPI )
5137 { j4array[0]+=IK2PI;
5138 }
5139 j4valid[0] = true;
5140 for(int ij4 = 0; ij4 < 1; ++ij4)
5141 {
5142 if( !j4valid[ij4] )
5143 {
5144  continue;
5145 }
5146 _ij4[0] = ij4; _ij4[1] = -1;
5147 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
5148 {
5149 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
5150 {
5151  j4valid[iij4]=false; _ij4[1] = iij4; break;
5152 }
5153 }
5154 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
5155 {
5156 IkReal evalcond[8];
5157 IkReal x214=IKcos(j4);
5158 IkReal x215=IKsin(j4);
5159 IkReal x216=((1.0)*cj5);
5160 IkReal x217=((1.0)*x215);
5161 IkReal x218=(sj5*x214);
5162 IkReal x219=(new_r12*x214);
5163 evalcond[0]=(x218+new_r12);
5164 evalcond[1]=((((-1.0)*sj5*x217))+new_r02);
5165 evalcond[2]=(((new_r02*x214))+((new_r12*x215)));
5166 evalcond[3]=(sj5+(((-1.0)*new_r02*x217))+x219);
5167 evalcond[4]=(((cj5*x219))+((new_r22*sj5))+(((-1.0)*new_r02*x215*x216)));
5168 evalcond[5]=((((-1.0)*new_r00*sj5*x217))+(((-1.0)*new_r20*x216))+((new_r10*x218)));
5169 evalcond[6]=((((-1.0)*new_r21*x216))+((new_r11*x218))+(((-1.0)*new_r01*sj5*x217)));
5170 evalcond[7]=((1.0)+(((-1.0)*new_r22*x216))+(((-1.0)*new_r02*sj5*x217))+((new_r12*x218)));
5171 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5172 {
5173 continue;
5174 }
5175 }
5176 
5177 {
5178 IkReal j6eval[3];
5179 j6eval[0]=sj5;
5180 j6eval[1]=IKsign(sj5);
5181 j6eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
5182 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
5183 {
5184 {
5185 IkReal j6eval[2];
5186 j6eval[0]=sj5;
5187 j6eval[1]=cj4;
5188 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 )
5189 {
5190 {
5191 IkReal j6eval[3];
5192 j6eval[0]=sj5;
5193 j6eval[1]=cj5;
5194 j6eval[2]=sj4;
5195 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
5196 {
5197 {
5198 IkReal evalcond[5];
5199 bool bgotonextstatement = true;
5200 do
5201 {
5202 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
5203 evalcond[1]=new_r12;
5204 evalcond[2]=new_r02;
5205 evalcond[3]=new_r20;
5206 evalcond[4]=new_r21;
5207 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
5208 {
5209 bgotonextstatement=false;
5210 {
5211 IkReal j6array[1], cj6array[1], sj6array[1];
5212 bool j6valid[1]={false};
5213 _nj6 = 1;
5214 IkReal x220=((1.0)*new_r01);
5215 if( IKabs(((((-1.0)*cj4*x220))+(((-1.0)*new_r00*sj4)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj4*new_r00))+(((-1.0)*sj4*x220)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj4*x220))+(((-1.0)*new_r00*sj4))))+IKsqr((((cj4*new_r00))+(((-1.0)*sj4*x220))))-1) <= IKFAST_SINCOS_THRESH )
5216  continue;
5217 j6array[0]=IKatan2(((((-1.0)*cj4*x220))+(((-1.0)*new_r00*sj4))), (((cj4*new_r00))+(((-1.0)*sj4*x220))));
5218 sj6array[0]=IKsin(j6array[0]);
5219 cj6array[0]=IKcos(j6array[0]);
5220 if( j6array[0] > IKPI )
5221 {
5222  j6array[0]-=IK2PI;
5223 }
5224 else if( j6array[0] < -IKPI )
5225 { j6array[0]+=IK2PI;
5226 }
5227 j6valid[0] = true;
5228 for(int ij6 = 0; ij6 < 1; ++ij6)
5229 {
5230 if( !j6valid[ij6] )
5231 {
5232  continue;
5233 }
5234 _ij6[0] = ij6; _ij6[1] = -1;
5235 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5236 {
5237 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5238 {
5239  j6valid[iij6]=false; _ij6[1] = iij6; break;
5240 }
5241 }
5242 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5243 {
5244 IkReal evalcond[8];
5245 IkReal x221=IKcos(j6);
5246 IkReal x222=IKsin(j6);
5247 IkReal x223=((1.0)*sj4);
5248 IkReal x224=(sj4*x222);
5249 IkReal x225=((1.0)*x221);
5250 IkReal x226=(sj4*x221);
5251 IkReal x227=(cj4*x222);
5252 IkReal x228=(cj4*x225);
5253 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x222);
5254 evalcond[1]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x225)));
5255 evalcond[2]=(x227+x226+new_r01);
5256 evalcond[3]=(x224+new_r00+(((-1.0)*x228)));
5257 evalcond[4]=(x224+new_r11+(((-1.0)*x228)));
5258 evalcond[5]=(((cj4*new_r10))+(((-1.0)*new_r00*x223))+(((-1.0)*x222)));
5259 evalcond[6]=((((-1.0)*new_r01*x223))+((cj4*new_r11))+(((-1.0)*x225)));
5260 evalcond[7]=((((-1.0)*x221*x223))+new_r10+(((-1.0)*x227)));
5261 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5262 {
5263 continue;
5264 }
5265 }
5266 
5267 {
5268 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5269 vinfos[0].jointtype = 17;
5270 vinfos[0].foffset = j0;
5271 vinfos[0].indices[0] = _ij0[0];
5272 vinfos[0].indices[1] = _ij0[1];
5273 vinfos[0].maxsolutions = _nj0;
5274 vinfos[1].jointtype = 1;
5275 vinfos[1].foffset = j1;
5276 vinfos[1].indices[0] = _ij1[0];
5277 vinfos[1].indices[1] = _ij1[1];
5278 vinfos[1].maxsolutions = _nj1;
5279 vinfos[2].jointtype = 1;
5280 vinfos[2].foffset = j2;
5281 vinfos[2].indices[0] = _ij2[0];
5282 vinfos[2].indices[1] = _ij2[1];
5283 vinfos[2].maxsolutions = _nj2;
5284 vinfos[3].jointtype = 1;
5285 vinfos[3].foffset = j3;
5286 vinfos[3].indices[0] = _ij3[0];
5287 vinfos[3].indices[1] = _ij3[1];
5288 vinfos[3].maxsolutions = _nj3;
5289 vinfos[4].jointtype = 1;
5290 vinfos[4].foffset = j4;
5291 vinfos[4].indices[0] = _ij4[0];
5292 vinfos[4].indices[1] = _ij4[1];
5293 vinfos[4].maxsolutions = _nj4;
5294 vinfos[5].jointtype = 1;
5295 vinfos[5].foffset = j5;
5296 vinfos[5].indices[0] = _ij5[0];
5297 vinfos[5].indices[1] = _ij5[1];
5298 vinfos[5].maxsolutions = _nj5;
5299 vinfos[6].jointtype = 1;
5300 vinfos[6].foffset = j6;
5301 vinfos[6].indices[0] = _ij6[0];
5302 vinfos[6].indices[1] = _ij6[1];
5303 vinfos[6].maxsolutions = _nj6;
5304 std::vector<int> vfree(0);
5305 solutions.AddSolution(vinfos,vfree);
5306 }
5307 }
5308 }
5309 
5310 }
5311 } while(0);
5312 if( bgotonextstatement )
5313 {
5314 bool bgotonextstatement = true;
5315 do
5316 {
5317 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
5318 evalcond[1]=new_r12;
5319 evalcond[2]=new_r02;
5320 evalcond[3]=new_r20;
5321 evalcond[4]=new_r21;
5322 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
5323 {
5324 bgotonextstatement=false;
5325 {
5326 IkReal j6array[1], cj6array[1], sj6array[1];
5327 bool j6valid[1]={false};
5328 _nj6 = 1;
5329 IkReal x229=((1.0)*new_r11);
5330 if( IKabs(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x229)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj4*x229))+((new_r01*sj4)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x229))))+IKsqr(((((-1.0)*cj4*x229))+((new_r01*sj4))))-1) <= IKFAST_SINCOS_THRESH )
5331  continue;
5332 j6array[0]=IKatan2(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x229))), ((((-1.0)*cj4*x229))+((new_r01*sj4))));
5333 sj6array[0]=IKsin(j6array[0]);
5334 cj6array[0]=IKcos(j6array[0]);
5335 if( j6array[0] > IKPI )
5336 {
5337  j6array[0]-=IK2PI;
5338 }
5339 else if( j6array[0] < -IKPI )
5340 { j6array[0]+=IK2PI;
5341 }
5342 j6valid[0] = true;
5343 for(int ij6 = 0; ij6 < 1; ++ij6)
5344 {
5345 if( !j6valid[ij6] )
5346 {
5347  continue;
5348 }
5349 _ij6[0] = ij6; _ij6[1] = -1;
5350 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5351 {
5352 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5353 {
5354  j6valid[iij6]=false; _ij6[1] = iij6; break;
5355 }
5356 }
5357 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5358 {
5359 IkReal evalcond[8];
5360 IkReal x230=IKsin(j6);
5361 IkReal x231=IKcos(j6);
5362 IkReal x232=((1.0)*sj4);
5363 IkReal x233=(cj4*x230);
5364 IkReal x234=((1.0)*x231);
5365 IkReal x235=(x231*x232);
5366 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x230);
5367 evalcond[1]=(((cj4*new_r10))+x230+(((-1.0)*new_r00*x232)));
5368 evalcond[2]=(((cj4*new_r11))+x231+(((-1.0)*new_r01*x232)));
5369 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x234)));
5370 evalcond[4]=(((sj4*x230))+((cj4*x231))+new_r11);
5371 evalcond[5]=((((-1.0)*x235))+x233+new_r01);
5372 evalcond[6]=((((-1.0)*x235))+x233+new_r10);
5373 evalcond[7]=((((-1.0)*x230*x232))+(((-1.0)*cj4*x234))+new_r00);
5374 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5375 {
5376 continue;
5377 }
5378 }
5379 
5380 {
5381 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5382 vinfos[0].jointtype = 17;
5383 vinfos[0].foffset = j0;
5384 vinfos[0].indices[0] = _ij0[0];
5385 vinfos[0].indices[1] = _ij0[1];
5386 vinfos[0].maxsolutions = _nj0;
5387 vinfos[1].jointtype = 1;
5388 vinfos[1].foffset = j1;
5389 vinfos[1].indices[0] = _ij1[0];
5390 vinfos[1].indices[1] = _ij1[1];
5391 vinfos[1].maxsolutions = _nj1;
5392 vinfos[2].jointtype = 1;
5393 vinfos[2].foffset = j2;
5394 vinfos[2].indices[0] = _ij2[0];
5395 vinfos[2].indices[1] = _ij2[1];
5396 vinfos[2].maxsolutions = _nj2;
5397 vinfos[3].jointtype = 1;
5398 vinfos[3].foffset = j3;
5399 vinfos[3].indices[0] = _ij3[0];
5400 vinfos[3].indices[1] = _ij3[1];
5401 vinfos[3].maxsolutions = _nj3;
5402 vinfos[4].jointtype = 1;
5403 vinfos[4].foffset = j4;
5404 vinfos[4].indices[0] = _ij4[0];
5405 vinfos[4].indices[1] = _ij4[1];
5406 vinfos[4].maxsolutions = _nj4;
5407 vinfos[5].jointtype = 1;
5408 vinfos[5].foffset = j5;
5409 vinfos[5].indices[0] = _ij5[0];
5410 vinfos[5].indices[1] = _ij5[1];
5411 vinfos[5].maxsolutions = _nj5;
5412 vinfos[6].jointtype = 1;
5413 vinfos[6].foffset = j6;
5414 vinfos[6].indices[0] = _ij6[0];
5415 vinfos[6].indices[1] = _ij6[1];
5416 vinfos[6].maxsolutions = _nj6;
5417 std::vector<int> vfree(0);
5418 solutions.AddSolution(vinfos,vfree);
5419 }
5420 }
5421 }
5422 
5423 }
5424 } while(0);
5425 if( bgotonextstatement )
5426 {
5427 bool bgotonextstatement = true;
5428 do
5429 {
5430 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
5431 evalcond[1]=new_r22;
5432 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5433 {
5434 bgotonextstatement=false;
5435 {
5436 IkReal j6array[1], cj6array[1], sj6array[1];
5437 bool j6valid[1]={false};
5438 _nj6 = 1;
5439 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
5440  continue;
5441 j6array[0]=IKatan2(new_r20, new_r21);
5442 sj6array[0]=IKsin(j6array[0]);
5443 cj6array[0]=IKcos(j6array[0]);
5444 if( j6array[0] > IKPI )
5445 {
5446  j6array[0]-=IK2PI;
5447 }
5448 else if( j6array[0] < -IKPI )
5449 { j6array[0]+=IK2PI;
5450 }
5451 j6valid[0] = true;
5452 for(int ij6 = 0; ij6 < 1; ++ij6)
5453 {
5454 if( !j6valid[ij6] )
5455 {
5456  continue;
5457 }
5458 _ij6[0] = ij6; _ij6[1] = -1;
5459 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5460 {
5461 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5462 {
5463  j6valid[iij6]=false; _ij6[1] = iij6; break;
5464 }
5465 }
5466 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5467 {
5468 IkReal evalcond[8];
5469 IkReal x236=IKsin(j6);
5470 IkReal x237=IKcos(j6);
5471 IkReal x238=((1.0)*x237);
5472 evalcond[0]=(new_r20+(((-1.0)*x236)));
5473 evalcond[1]=((((-1.0)*x238))+new_r21);
5474 evalcond[2]=(((cj4*x236))+new_r01);
5475 evalcond[3]=(((sj4*x236))+new_r11);
5476 evalcond[4]=((((-1.0)*cj4*x238))+new_r00);
5477 evalcond[5]=((((-1.0)*new_r02*x238))+new_r10);
5478 evalcond[6]=(((cj4*new_r01))+((new_r11*sj4))+x236);
5479 evalcond[7]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x238)));
5480 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5481 {
5482 continue;
5483 }
5484 }
5485 
5486 {
5487 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5488 vinfos[0].jointtype = 17;
5489 vinfos[0].foffset = j0;
5490 vinfos[0].indices[0] = _ij0[0];
5491 vinfos[0].indices[1] = _ij0[1];
5492 vinfos[0].maxsolutions = _nj0;
5493 vinfos[1].jointtype = 1;
5494 vinfos[1].foffset = j1;
5495 vinfos[1].indices[0] = _ij1[0];
5496 vinfos[1].indices[1] = _ij1[1];
5497 vinfos[1].maxsolutions = _nj1;
5498 vinfos[2].jointtype = 1;
5499 vinfos[2].foffset = j2;
5500 vinfos[2].indices[0] = _ij2[0];
5501 vinfos[2].indices[1] = _ij2[1];
5502 vinfos[2].maxsolutions = _nj2;
5503 vinfos[3].jointtype = 1;
5504 vinfos[3].foffset = j3;
5505 vinfos[3].indices[0] = _ij3[0];
5506 vinfos[3].indices[1] = _ij3[1];
5507 vinfos[3].maxsolutions = _nj3;
5508 vinfos[4].jointtype = 1;
5509 vinfos[4].foffset = j4;
5510 vinfos[4].indices[0] = _ij4[0];
5511 vinfos[4].indices[1] = _ij4[1];
5512 vinfos[4].maxsolutions = _nj4;
5513 vinfos[5].jointtype = 1;
5514 vinfos[5].foffset = j5;
5515 vinfos[5].indices[0] = _ij5[0];
5516 vinfos[5].indices[1] = _ij5[1];
5517 vinfos[5].maxsolutions = _nj5;
5518 vinfos[6].jointtype = 1;
5519 vinfos[6].foffset = j6;
5520 vinfos[6].indices[0] = _ij6[0];
5521 vinfos[6].indices[1] = _ij6[1];
5522 vinfos[6].maxsolutions = _nj6;
5523 std::vector<int> vfree(0);
5524 solutions.AddSolution(vinfos,vfree);
5525 }
5526 }
5527 }
5528 
5529 }
5530 } while(0);
5531 if( bgotonextstatement )
5532 {
5533 bool bgotonextstatement = true;
5534 do
5535 {
5536 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
5537 evalcond[1]=new_r22;
5538 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5539 {
5540 bgotonextstatement=false;
5541 {
5542 IkReal j6array[1], cj6array[1], sj6array[1];
5543 bool j6valid[1]={false};
5544 _nj6 = 1;
5545 if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
5546  continue;
5547 j6array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21));
5548 sj6array[0]=IKsin(j6array[0]);
5549 cj6array[0]=IKcos(j6array[0]);
5550 if( j6array[0] > IKPI )
5551 {
5552  j6array[0]-=IK2PI;
5553 }
5554 else if( j6array[0] < -IKPI )
5555 { j6array[0]+=IK2PI;
5556 }
5557 j6valid[0] = true;
5558 for(int ij6 = 0; ij6 < 1; ++ij6)
5559 {
5560 if( !j6valid[ij6] )
5561 {
5562  continue;
5563 }
5564 _ij6[0] = ij6; _ij6[1] = -1;
5565 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5566 {
5567 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5568 {
5569  j6valid[iij6]=false; _ij6[1] = iij6; break;
5570 }
5571 }
5572 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5573 {
5574 IkReal evalcond[8];
5575 IkReal x239=IKsin(j6);
5576 IkReal x240=IKcos(j6);
5577 IkReal x241=((1.0)*x240);
5578 evalcond[0]=(x239+new_r20);
5579 evalcond[1]=(x240+new_r21);
5580 evalcond[2]=(((cj4*x239))+new_r01);
5581 evalcond[3]=(((sj4*x239))+new_r11);
5582 evalcond[4]=(new_r10+((new_r02*x240)));
5583 evalcond[5]=((((-1.0)*cj4*x241))+new_r00);
5584 evalcond[6]=(((cj4*new_r01))+((new_r11*sj4))+x239);
5585 evalcond[7]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x241)));
5586 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5587 {
5588 continue;
5589 }
5590 }
5591 
5592 {
5593 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5594 vinfos[0].jointtype = 17;
5595 vinfos[0].foffset = j0;
5596 vinfos[0].indices[0] = _ij0[0];
5597 vinfos[0].indices[1] = _ij0[1];
5598 vinfos[0].maxsolutions = _nj0;
5599 vinfos[1].jointtype = 1;
5600 vinfos[1].foffset = j1;
5601 vinfos[1].indices[0] = _ij1[0];
5602 vinfos[1].indices[1] = _ij1[1];
5603 vinfos[1].maxsolutions = _nj1;
5604 vinfos[2].jointtype = 1;
5605 vinfos[2].foffset = j2;
5606 vinfos[2].indices[0] = _ij2[0];
5607 vinfos[2].indices[1] = _ij2[1];
5608 vinfos[2].maxsolutions = _nj2;
5609 vinfos[3].jointtype = 1;
5610 vinfos[3].foffset = j3;
5611 vinfos[3].indices[0] = _ij3[0];
5612 vinfos[3].indices[1] = _ij3[1];
5613 vinfos[3].maxsolutions = _nj3;
5614 vinfos[4].jointtype = 1;
5615 vinfos[4].foffset = j4;
5616 vinfos[4].indices[0] = _ij4[0];
5617 vinfos[4].indices[1] = _ij4[1];
5618 vinfos[4].maxsolutions = _nj4;
5619 vinfos[5].jointtype = 1;
5620 vinfos[5].foffset = j5;
5621 vinfos[5].indices[0] = _ij5[0];
5622 vinfos[5].indices[1] = _ij5[1];
5623 vinfos[5].maxsolutions = _nj5;
5624 vinfos[6].jointtype = 1;
5625 vinfos[6].foffset = j6;
5626 vinfos[6].indices[0] = _ij6[0];
5627 vinfos[6].indices[1] = _ij6[1];
5628 vinfos[6].maxsolutions = _nj6;
5629 std::vector<int> vfree(0);
5630 solutions.AddSolution(vinfos,vfree);
5631 }
5632 }
5633 }
5634 
5635 }
5636 } while(0);
5637 if( bgotonextstatement )
5638 {
5639 bool bgotonextstatement = true;
5640 do
5641 {
5642 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5643 evalcond[1]=new_r02;
5644 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5645 {
5646 bgotonextstatement=false;
5647 {
5648 IkReal j6array[1], cj6array[1], sj6array[1];
5649 bool j6valid[1]={false};
5650 _nj6 = 1;
5651 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
5652  continue;
5653 j6array[0]=IKatan2(((-1.0)*new_r01), new_r00);
5654 sj6array[0]=IKsin(j6array[0]);
5655 cj6array[0]=IKcos(j6array[0]);
5656 if( j6array[0] > IKPI )
5657 {
5658  j6array[0]-=IK2PI;
5659 }
5660 else if( j6array[0] < -IKPI )
5661 { j6array[0]+=IK2PI;
5662 }
5663 j6valid[0] = true;
5664 for(int ij6 = 0; ij6 < 1; ++ij6)
5665 {
5666 if( !j6valid[ij6] )
5667 {
5668  continue;
5669 }
5670 _ij6[0] = ij6; _ij6[1] = -1;
5671 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5672 {
5673 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5674 {
5675  j6valid[iij6]=false; _ij6[1] = iij6; break;
5676 }
5677 }
5678 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5679 {
5680 IkReal evalcond[8];
5681 IkReal x242=IKsin(j6);
5682 IkReal x243=IKcos(j6);
5683 IkReal x244=((1.0)*x243);
5684 IkReal x245=((1.0)*x242);
5685 evalcond[0]=(x242+new_r01);
5686 evalcond[1]=(((new_r12*x242))+new_r20);
5687 evalcond[2]=(((new_r12*x243))+new_r21);
5688 evalcond[3]=((((-1.0)*x244))+new_r00);
5689 evalcond[4]=((((-1.0)*cj5*x244))+new_r11);
5690 evalcond[5]=((((-1.0)*cj5*x245))+new_r10);
5691 evalcond[6]=(((new_r20*sj5))+((cj5*new_r10))+(((-1.0)*x245)));
5692 evalcond[7]=(((cj5*new_r11))+(((-1.0)*x244))+((new_r21*sj5)));
5693 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5694 {
5695 continue;
5696 }
5697 }
5698 
5699 {
5700 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5701 vinfos[0].jointtype = 17;
5702 vinfos[0].foffset = j0;
5703 vinfos[0].indices[0] = _ij0[0];
5704 vinfos[0].indices[1] = _ij0[1];
5705 vinfos[0].maxsolutions = _nj0;
5706 vinfos[1].jointtype = 1;
5707 vinfos[1].foffset = j1;
5708 vinfos[1].indices[0] = _ij1[0];
5709 vinfos[1].indices[1] = _ij1[1];
5710 vinfos[1].maxsolutions = _nj1;
5711 vinfos[2].jointtype = 1;
5712 vinfos[2].foffset = j2;
5713 vinfos[2].indices[0] = _ij2[0];
5714 vinfos[2].indices[1] = _ij2[1];
5715 vinfos[2].maxsolutions = _nj2;
5716 vinfos[3].jointtype = 1;
5717 vinfos[3].foffset = j3;
5718 vinfos[3].indices[0] = _ij3[0];
5719 vinfos[3].indices[1] = _ij3[1];
5720 vinfos[3].maxsolutions = _nj3;
5721 vinfos[4].jointtype = 1;
5722 vinfos[4].foffset = j4;
5723 vinfos[4].indices[0] = _ij4[0];
5724 vinfos[4].indices[1] = _ij4[1];
5725 vinfos[4].maxsolutions = _nj4;
5726 vinfos[5].jointtype = 1;
5727 vinfos[5].foffset = j5;
5728 vinfos[5].indices[0] = _ij5[0];
5729 vinfos[5].indices[1] = _ij5[1];
5730 vinfos[5].maxsolutions = _nj5;
5731 vinfos[6].jointtype = 1;
5732 vinfos[6].foffset = j6;
5733 vinfos[6].indices[0] = _ij6[0];
5734 vinfos[6].indices[1] = _ij6[1];
5735 vinfos[6].maxsolutions = _nj6;
5736 std::vector<int> vfree(0);
5737 solutions.AddSolution(vinfos,vfree);
5738 }
5739 }
5740 }
5741 
5742 }
5743 } while(0);
5744 if( bgotonextstatement )
5745 {
5746 bool bgotonextstatement = true;
5747 do
5748 {
5749 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
5750 evalcond[1]=new_r02;
5751 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5752 {
5753 bgotonextstatement=false;
5754 {
5755 IkReal j6array[1], cj6array[1], sj6array[1];
5756 bool j6valid[1]={false};
5757 _nj6 = 1;
5758 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
5759  continue;
5760 j6array[0]=IKatan2(new_r01, ((-1.0)*new_r00));
5761 sj6array[0]=IKsin(j6array[0]);
5762 cj6array[0]=IKcos(j6array[0]);
5763 if( j6array[0] > IKPI )
5764 {
5765  j6array[0]-=IK2PI;
5766 }
5767 else if( j6array[0] < -IKPI )
5768 { j6array[0]+=IK2PI;
5769 }
5770 j6valid[0] = true;
5771 for(int ij6 = 0; ij6 < 1; ++ij6)
5772 {
5773 if( !j6valid[ij6] )
5774 {
5775  continue;
5776 }
5777 _ij6[0] = ij6; _ij6[1] = -1;
5778 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5779 {
5780 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5781 {
5782  j6valid[iij6]=false; _ij6[1] = iij6; break;
5783 }
5784 }
5785 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5786 {
5787 IkReal evalcond[8];
5788 IkReal x246=IKsin(j6);
5789 IkReal x247=IKcos(j6);
5790 IkReal x248=((1.0)*cj5);
5791 IkReal x249=((1.0)*x247);
5792 IkReal x250=((1.0)*x246);
5793 evalcond[0]=(x246+(((-1.0)*new_r01)));
5794 evalcond[1]=(((cj5*x247))+new_r11);
5795 evalcond[2]=(new_r20+(((-1.0)*new_r12*x250)));
5796 evalcond[3]=((((-1.0)*new_r12*x249))+new_r21);
5797 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x249)));
5798 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x246*x248)));
5799 evalcond[6]=(((new_r20*sj5))+(((-1.0)*new_r10*x248))+(((-1.0)*x250)));
5800 evalcond[7]=((((-1.0)*new_r11*x248))+(((-1.0)*x249))+((new_r21*sj5)));
5801 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5802 {
5803 continue;
5804 }
5805 }
5806 
5807 {
5808 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5809 vinfos[0].jointtype = 17;
5810 vinfos[0].foffset = j0;
5811 vinfos[0].indices[0] = _ij0[0];
5812 vinfos[0].indices[1] = _ij0[1];
5813 vinfos[0].maxsolutions = _nj0;
5814 vinfos[1].jointtype = 1;
5815 vinfos[1].foffset = j1;
5816 vinfos[1].indices[0] = _ij1[0];
5817 vinfos[1].indices[1] = _ij1[1];
5818 vinfos[1].maxsolutions = _nj1;
5819 vinfos[2].jointtype = 1;
5820 vinfos[2].foffset = j2;
5821 vinfos[2].indices[0] = _ij2[0];
5822 vinfos[2].indices[1] = _ij2[1];
5823 vinfos[2].maxsolutions = _nj2;
5824 vinfos[3].jointtype = 1;
5825 vinfos[3].foffset = j3;
5826 vinfos[3].indices[0] = _ij3[0];
5827 vinfos[3].indices[1] = _ij3[1];
5828 vinfos[3].maxsolutions = _nj3;
5829 vinfos[4].jointtype = 1;
5830 vinfos[4].foffset = j4;
5831 vinfos[4].indices[0] = _ij4[0];
5832 vinfos[4].indices[1] = _ij4[1];
5833 vinfos[4].maxsolutions = _nj4;
5834 vinfos[5].jointtype = 1;
5835 vinfos[5].foffset = j5;
5836 vinfos[5].indices[0] = _ij5[0];
5837 vinfos[5].indices[1] = _ij5[1];
5838 vinfos[5].maxsolutions = _nj5;
5839 vinfos[6].jointtype = 1;
5840 vinfos[6].foffset = j6;
5841 vinfos[6].indices[0] = _ij6[0];
5842 vinfos[6].indices[1] = _ij6[1];
5843 vinfos[6].maxsolutions = _nj6;
5844 std::vector<int> vfree(0);
5845 solutions.AddSolution(vinfos,vfree);
5846 }
5847 }
5848 }
5849 
5850 }
5851 } while(0);
5852 if( bgotonextstatement )
5853 {
5854 bool bgotonextstatement = true;
5855 do
5856 {
5857 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
5858 evalcond[1]=new_r12;
5859 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5860 {
5861 bgotonextstatement=false;
5862 {
5863 IkReal j6array[1], cj6array[1], sj6array[1];
5864 bool j6valid[1]={false};
5865 _nj6 = 1;
5866 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
5867  continue;
5868 j6array[0]=IKatan2(((-1.0)*new_r11), new_r10);
5869 sj6array[0]=IKsin(j6array[0]);
5870 cj6array[0]=IKcos(j6array[0]);
5871 if( j6array[0] > IKPI )
5872 {
5873  j6array[0]-=IK2PI;
5874 }
5875 else if( j6array[0] < -IKPI )
5876 { j6array[0]+=IK2PI;
5877 }
5878 j6valid[0] = true;
5879 for(int ij6 = 0; ij6 < 1; ++ij6)
5880 {
5881 if( !j6valid[ij6] )
5882 {
5883  continue;
5884 }
5885 _ij6[0] = ij6; _ij6[1] = -1;
5886 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
5887 {
5888 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
5889 {
5890  j6valid[iij6]=false; _ij6[1] = iij6; break;
5891 }
5892 }
5893 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5894 {
5895 IkReal evalcond[8];
5896 IkReal x251=IKcos(j6);
5897 IkReal x252=IKsin(j6);
5898 IkReal x253=((1.0)*cj5);
5899 IkReal x254=((1.0)*x251);
5900 IkReal x255=((1.0)*x252);
5901 evalcond[0]=(x252+new_r11);
5902 evalcond[1]=(new_r10+(((-1.0)*x254)));
5903 evalcond[2]=(((cj5*x251))+new_r01);
5904 evalcond[3]=(((cj5*x252))+new_r00);
5905 evalcond[4]=(new_r20+(((-1.0)*new_r02*x255)));
5906 evalcond[5]=(new_r21+(((-1.0)*new_r02*x254)));
5907 evalcond[6]=(((new_r20*sj5))+(((-1.0)*new_r00*x253))+(((-1.0)*x255)));
5908 evalcond[7]=((((-1.0)*new_r01*x253))+((new_r21*sj5))+(((-1.0)*x254)));
5909 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5910 {
5911 continue;
5912 }
5913 }
5914 
5915 {
5916 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5917 vinfos[0].jointtype = 17;
5918 vinfos[0].foffset = j0;
5919 vinfos[0].indices[0] = _ij0[0];
5920 vinfos[0].indices[1] = _ij0[1];
5921 vinfos[0].maxsolutions = _nj0;
5922 vinfos[1].jointtype = 1;
5923 vinfos[1].foffset = j1;
5924 vinfos[1].indices[0] = _ij1[0];
5925 vinfos[1].indices[1] = _ij1[1];
5926 vinfos[1].maxsolutions = _nj1;
5927 vinfos[2].jointtype = 1;
5928 vinfos[2].foffset = j2;
5929 vinfos[2].indices[0] = _ij2[0];
5930 vinfos[2].indices[1] = _ij2[1];
5931 vinfos[2].maxsolutions = _nj2;
5932 vinfos[3].jointtype = 1;
5933 vinfos[3].foffset = j3;
5934 vinfos[3].indices[0] = _ij3[0];
5935 vinfos[3].indices[1] = _ij3[1];
5936 vinfos[3].maxsolutions = _nj3;
5937 vinfos[4].jointtype = 1;
5938 vinfos[4].foffset = j4;
5939 vinfos[4].indices[0] = _ij4[0];
5940 vinfos[4].indices[1] = _ij4[1];
5941 vinfos[4].maxsolutions = _nj4;
5942 vinfos[5].jointtype = 1;
5943 vinfos[5].foffset = j5;
5944 vinfos[5].indices[0] = _ij5[0];
5945 vinfos[5].indices[1] = _ij5[1];
5946 vinfos[5].maxsolutions = _nj5;
5947 vinfos[6].jointtype = 1;
5948 vinfos[6].foffset = j6;
5949 vinfos[6].indices[0] = _ij6[0];
5950 vinfos[6].indices[1] = _ij6[1];
5951 vinfos[6].maxsolutions = _nj6;
5952 std::vector<int> vfree(0);
5953 solutions.AddSolution(vinfos,vfree);
5954 }
5955 }
5956 }
5957 
5958 }
5959 } while(0);
5960 if( bgotonextstatement )
5961 {
5962 bool bgotonextstatement = true;
5963 do
5964 {
5965 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
5966 evalcond[1]=new_r12;
5967 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5968 {
5969 bgotonextstatement=false;
5970 {
5971 IkReal j6eval[3];
5972 sj4=-1.0;
5973 cj4=0;
5974 j4=-1.5707963267949;
5975 j6eval[0]=new_r02;
5976 j6eval[1]=IKsign(new_r02);
5977 j6eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
5978 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
5979 {
5980 {
5981 IkReal j6eval[1];
5982 sj4=-1.0;
5983 cj4=0;
5984 j4=-1.5707963267949;
5985 j6eval[0]=new_r02;
5986 if( IKabs(j6eval[0]) < 0.0000010000000000 )
5987 {
5988 {
5989 IkReal j6eval[2];
5990 sj4=-1.0;
5991 cj4=0;
5992 j4=-1.5707963267949;
5993 j6eval[0]=new_r02;
5994 j6eval[1]=cj5;
5995 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 )
5996 {
5997 {
5998 IkReal evalcond[4];
5999 bool bgotonextstatement = true;
6000 do
6001 {
6002 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
6003 evalcond[1]=new_r22;
6004 evalcond[2]=new_r01;
6005 evalcond[3]=new_r00;
6006 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
6007 {
6008 bgotonextstatement=false;
6009 {
6010 IkReal j6array[1], cj6array[1], sj6array[1];
6011 bool j6valid[1]={false};
6012 _nj6 = 1;
6013 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
6014  continue;
6015 j6array[0]=IKatan2(new_r20, new_r21);
6016 sj6array[0]=IKsin(j6array[0]);
6017 cj6array[0]=IKcos(j6array[0]);
6018 if( j6array[0] > IKPI )
6019 {
6020  j6array[0]-=IK2PI;
6021 }
6022 else if( j6array[0] < -IKPI )
6023 { j6array[0]+=IK2PI;
6024 }
6025 j6valid[0] = true;
6026 for(int ij6 = 0; ij6 < 1; ++ij6)
6027 {
6028 if( !j6valid[ij6] )
6029 {
6030  continue;
6031 }
6032 _ij6[0] = ij6; _ij6[1] = -1;
6033 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6034 {
6035 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6036 {
6037  j6valid[iij6]=false; _ij6[1] = iij6; break;
6038 }
6039 }
6040 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6041 {
6042 IkReal evalcond[4];
6043 IkReal x256=IKsin(j6);
6044 IkReal x257=((1.0)*(IKcos(j6)));
6045 evalcond[0]=(new_r20+(((-1.0)*x256)));
6046 evalcond[1]=(new_r21+(((-1.0)*x257)));
6047 evalcond[2]=(x256+(((-1.0)*new_r11)));
6048 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x257)));
6049 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
6050 {
6051 continue;
6052 }
6053 }
6054 
6055 {
6056 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6057 vinfos[0].jointtype = 17;
6058 vinfos[0].foffset = j0;
6059 vinfos[0].indices[0] = _ij0[0];
6060 vinfos[0].indices[1] = _ij0[1];
6061 vinfos[0].maxsolutions = _nj0;
6062 vinfos[1].jointtype = 1;
6063 vinfos[1].foffset = j1;
6064 vinfos[1].indices[0] = _ij1[0];
6065 vinfos[1].indices[1] = _ij1[1];
6066 vinfos[1].maxsolutions = _nj1;
6067 vinfos[2].jointtype = 1;
6068 vinfos[2].foffset = j2;
6069 vinfos[2].indices[0] = _ij2[0];
6070 vinfos[2].indices[1] = _ij2[1];
6071 vinfos[2].maxsolutions = _nj2;
6072 vinfos[3].jointtype = 1;
6073 vinfos[3].foffset = j3;
6074 vinfos[3].indices[0] = _ij3[0];
6075 vinfos[3].indices[1] = _ij3[1];
6076 vinfos[3].maxsolutions = _nj3;
6077 vinfos[4].jointtype = 1;
6078 vinfos[4].foffset = j4;
6079 vinfos[4].indices[0] = _ij4[0];
6080 vinfos[4].indices[1] = _ij4[1];
6081 vinfos[4].maxsolutions = _nj4;
6082 vinfos[5].jointtype = 1;
6083 vinfos[5].foffset = j5;
6084 vinfos[5].indices[0] = _ij5[0];
6085 vinfos[5].indices[1] = _ij5[1];
6086 vinfos[5].maxsolutions = _nj5;
6087 vinfos[6].jointtype = 1;
6088 vinfos[6].foffset = j6;
6089 vinfos[6].indices[0] = _ij6[0];
6090 vinfos[6].indices[1] = _ij6[1];
6091 vinfos[6].maxsolutions = _nj6;
6092 std::vector<int> vfree(0);
6093 solutions.AddSolution(vinfos,vfree);
6094 }
6095 }
6096 }
6097 
6098 }
6099 } while(0);
6100 if( bgotonextstatement )
6101 {
6102 bool bgotonextstatement = true;
6103 do
6104 {
6105 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
6106 evalcond[1]=new_r22;
6107 evalcond[2]=new_r01;
6108 evalcond[3]=new_r00;
6109 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
6110 {
6111 bgotonextstatement=false;
6112 {
6113 IkReal j6array[1], cj6array[1], sj6array[1];
6114 bool j6valid[1]={false};
6115 _nj6 = 1;
6116 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
6117  continue;
6118 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r21));
6119 sj6array[0]=IKsin(j6array[0]);
6120 cj6array[0]=IKcos(j6array[0]);
6121 if( j6array[0] > IKPI )
6122 {
6123  j6array[0]-=IK2PI;
6124 }
6125 else if( j6array[0] < -IKPI )
6126 { j6array[0]+=IK2PI;
6127 }
6128 j6valid[0] = true;
6129 for(int ij6 = 0; ij6 < 1; ++ij6)
6130 {
6131 if( !j6valid[ij6] )
6132 {
6133  continue;
6134 }
6135 _ij6[0] = ij6; _ij6[1] = -1;
6136 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6137 {
6138 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6139 {
6140  j6valid[iij6]=false; _ij6[1] = iij6; break;
6141 }
6142 }
6143 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6144 {
6145 IkReal evalcond[4];
6146 IkReal x258=IKsin(j6);
6147 IkReal x259=IKcos(j6);
6148 evalcond[0]=(x258+new_r20);
6149 evalcond[1]=(x259+new_r21);
6150 evalcond[2]=(x258+(((-1.0)*new_r11)));
6151 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x259)));
6152 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
6153 {
6154 continue;
6155 }
6156 }
6157 
6158 {
6159 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6160 vinfos[0].jointtype = 17;
6161 vinfos[0].foffset = j0;
6162 vinfos[0].indices[0] = _ij0[0];
6163 vinfos[0].indices[1] = _ij0[1];
6164 vinfos[0].maxsolutions = _nj0;
6165 vinfos[1].jointtype = 1;
6166 vinfos[1].foffset = j1;
6167 vinfos[1].indices[0] = _ij1[0];
6168 vinfos[1].indices[1] = _ij1[1];
6169 vinfos[1].maxsolutions = _nj1;
6170 vinfos[2].jointtype = 1;
6171 vinfos[2].foffset = j2;
6172 vinfos[2].indices[0] = _ij2[0];
6173 vinfos[2].indices[1] = _ij2[1];
6174 vinfos[2].maxsolutions = _nj2;
6175 vinfos[3].jointtype = 1;
6176 vinfos[3].foffset = j3;
6177 vinfos[3].indices[0] = _ij3[0];
6178 vinfos[3].indices[1] = _ij3[1];
6179 vinfos[3].maxsolutions = _nj3;
6180 vinfos[4].jointtype = 1;
6181 vinfos[4].foffset = j4;
6182 vinfos[4].indices[0] = _ij4[0];
6183 vinfos[4].indices[1] = _ij4[1];
6184 vinfos[4].maxsolutions = _nj4;
6185 vinfos[5].jointtype = 1;
6186 vinfos[5].foffset = j5;
6187 vinfos[5].indices[0] = _ij5[0];
6188 vinfos[5].indices[1] = _ij5[1];
6189 vinfos[5].maxsolutions = _nj5;
6190 vinfos[6].jointtype = 1;
6191 vinfos[6].foffset = j6;
6192 vinfos[6].indices[0] = _ij6[0];
6193 vinfos[6].indices[1] = _ij6[1];
6194 vinfos[6].maxsolutions = _nj6;
6195 std::vector<int> vfree(0);
6196 solutions.AddSolution(vinfos,vfree);
6197 }
6198 }
6199 }
6200 
6201 }
6202 } while(0);
6203 if( bgotonextstatement )
6204 {
6205 bool bgotonextstatement = true;
6206 do
6207 {
6208 evalcond[0]=IKabs(new_r02);
6209 evalcond[1]=new_r20;
6210 evalcond[2]=new_r21;
6211 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
6212 {
6213 bgotonextstatement=false;
6214 {
6215 IkReal j6array[1], cj6array[1], sj6array[1];
6216 bool j6valid[1]={false};
6217 _nj6 = 1;
6218 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
6219  continue;
6220 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
6221 sj6array[0]=IKsin(j6array[0]);
6222 cj6array[0]=IKcos(j6array[0]);
6223 if( j6array[0] > IKPI )
6224 {
6225  j6array[0]-=IK2PI;
6226 }
6227 else if( j6array[0] < -IKPI )
6228 { j6array[0]+=IK2PI;
6229 }
6230 j6valid[0] = true;
6231 for(int ij6 = 0; ij6 < 1; ++ij6)
6232 {
6233 if( !j6valid[ij6] )
6234 {
6235  continue;
6236 }
6237 _ij6[0] = ij6; _ij6[1] = -1;
6238 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6239 {
6240 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6241 {
6242  j6valid[iij6]=false; _ij6[1] = iij6; break;
6243 }
6244 }
6245 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6246 {
6247 IkReal evalcond[6];
6248 IkReal x260=IKsin(j6);
6249 IkReal x261=IKcos(j6);
6250 IkReal x262=((1.0)*x261);
6251 IkReal x263=((1.0)*x260);
6252 evalcond[0]=(x260+(((-1.0)*new_r11)));
6253 evalcond[1]=((((-1.0)*cj5*x262))+new_r01);
6254 evalcond[2]=((((-1.0)*cj5*x263))+new_r00);
6255 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x262)));
6256 evalcond[4]=(((cj5*new_r00))+(((-1.0)*x263)));
6257 evalcond[5]=(((cj5*new_r01))+(((-1.0)*x262)));
6258 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6259 {
6260 continue;
6261 }
6262 }
6263 
6264 {
6265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6266 vinfos[0].jointtype = 17;
6267 vinfos[0].foffset = j0;
6268 vinfos[0].indices[0] = _ij0[0];
6269 vinfos[0].indices[1] = _ij0[1];
6270 vinfos[0].maxsolutions = _nj0;
6271 vinfos[1].jointtype = 1;
6272 vinfos[1].foffset = j1;
6273 vinfos[1].indices[0] = _ij1[0];
6274 vinfos[1].indices[1] = _ij1[1];
6275 vinfos[1].maxsolutions = _nj1;
6276 vinfos[2].jointtype = 1;
6277 vinfos[2].foffset = j2;
6278 vinfos[2].indices[0] = _ij2[0];
6279 vinfos[2].indices[1] = _ij2[1];
6280 vinfos[2].maxsolutions = _nj2;
6281 vinfos[3].jointtype = 1;
6282 vinfos[3].foffset = j3;
6283 vinfos[3].indices[0] = _ij3[0];
6284 vinfos[3].indices[1] = _ij3[1];
6285 vinfos[3].maxsolutions = _nj3;
6286 vinfos[4].jointtype = 1;
6287 vinfos[4].foffset = j4;
6288 vinfos[4].indices[0] = _ij4[0];
6289 vinfos[4].indices[1] = _ij4[1];
6290 vinfos[4].maxsolutions = _nj4;
6291 vinfos[5].jointtype = 1;
6292 vinfos[5].foffset = j5;
6293 vinfos[5].indices[0] = _ij5[0];
6294 vinfos[5].indices[1] = _ij5[1];
6295 vinfos[5].maxsolutions = _nj5;
6296 vinfos[6].jointtype = 1;
6297 vinfos[6].foffset = j6;
6298 vinfos[6].indices[0] = _ij6[0];
6299 vinfos[6].indices[1] = _ij6[1];
6300 vinfos[6].maxsolutions = _nj6;
6301 std::vector<int> vfree(0);
6302 solutions.AddSolution(vinfos,vfree);
6303 }
6304 }
6305 }
6306 
6307 }
6308 } while(0);
6309 if( bgotonextstatement )
6310 {
6311 bool bgotonextstatement = true;
6312 do
6313 {
6314 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
6315 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6316 {
6317 bgotonextstatement=false;
6318 {
6319 IkReal j6array[1], cj6array[1], sj6array[1];
6320 bool j6valid[1]={false};
6321 _nj6 = 1;
6322 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
6323  continue;
6324 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
6325 sj6array[0]=IKsin(j6array[0]);
6326 cj6array[0]=IKcos(j6array[0]);
6327 if( j6array[0] > IKPI )
6328 {
6329  j6array[0]-=IK2PI;
6330 }
6331 else if( j6array[0] < -IKPI )
6332 { j6array[0]+=IK2PI;
6333 }
6334 j6valid[0] = true;
6335 for(int ij6 = 0; ij6 < 1; ++ij6)
6336 {
6337 if( !j6valid[ij6] )
6338 {
6339  continue;
6340 }
6341 _ij6[0] = ij6; _ij6[1] = -1;
6342 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6343 {
6344 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6345 {
6346  j6valid[iij6]=false; _ij6[1] = iij6; break;
6347 }
6348 }
6349 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6350 {
6351 IkReal evalcond[6];
6352 IkReal x264=IKcos(j6);
6353 IkReal x265=IKsin(j6);
6354 IkReal x266=((-1.0)*x264);
6355 IkReal x267=((-1.0)*x265);
6356 evalcond[0]=x267;
6357 evalcond[1]=x266;
6358 evalcond[2]=(new_r22*x266);
6359 evalcond[3]=(new_r22*x267);
6360 evalcond[4]=(x265+(((-1.0)*new_r11)));
6361 evalcond[5]=((((-1.0)*x264))+(((-1.0)*new_r10)));
6362 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6363 {
6364 continue;
6365 }
6366 }
6367 
6368 {
6369 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6370 vinfos[0].jointtype = 17;
6371 vinfos[0].foffset = j0;
6372 vinfos[0].indices[0] = _ij0[0];
6373 vinfos[0].indices[1] = _ij0[1];
6374 vinfos[0].maxsolutions = _nj0;
6375 vinfos[1].jointtype = 1;
6376 vinfos[1].foffset = j1;
6377 vinfos[1].indices[0] = _ij1[0];
6378 vinfos[1].indices[1] = _ij1[1];
6379 vinfos[1].maxsolutions = _nj1;
6380 vinfos[2].jointtype = 1;
6381 vinfos[2].foffset = j2;
6382 vinfos[2].indices[0] = _ij2[0];
6383 vinfos[2].indices[1] = _ij2[1];
6384 vinfos[2].maxsolutions = _nj2;
6385 vinfos[3].jointtype = 1;
6386 vinfos[3].foffset = j3;
6387 vinfos[3].indices[0] = _ij3[0];
6388 vinfos[3].indices[1] = _ij3[1];
6389 vinfos[3].maxsolutions = _nj3;
6390 vinfos[4].jointtype = 1;
6391 vinfos[4].foffset = j4;
6392 vinfos[4].indices[0] = _ij4[0];
6393 vinfos[4].indices[1] = _ij4[1];
6394 vinfos[4].maxsolutions = _nj4;
6395 vinfos[5].jointtype = 1;
6396 vinfos[5].foffset = j5;
6397 vinfos[5].indices[0] = _ij5[0];
6398 vinfos[5].indices[1] = _ij5[1];
6399 vinfos[5].maxsolutions = _nj5;
6400 vinfos[6].jointtype = 1;
6401 vinfos[6].foffset = j6;
6402 vinfos[6].indices[0] = _ij6[0];
6403 vinfos[6].indices[1] = _ij6[1];
6404 vinfos[6].maxsolutions = _nj6;
6405 std::vector<int> vfree(0);
6406 solutions.AddSolution(vinfos,vfree);
6407 }
6408 }
6409 }
6410 
6411 }
6412 } while(0);
6413 if( bgotonextstatement )
6414 {
6415 bool bgotonextstatement = true;
6416 do
6417 {
6418 if( 1 )
6419 {
6420 bgotonextstatement=false;
6421 continue; // branch miss [j6]
6422 
6423 }
6424 } while(0);
6425 if( bgotonextstatement )
6426 {
6427 }
6428 }
6429 }
6430 }
6431 }
6432 }
6433 
6434 } else
6435 {
6436 {
6437 IkReal j6array[1], cj6array[1], sj6array[1];
6438 bool j6valid[1]={false};
6439 _nj6 = 1;
6440 CheckValue<IkReal> x268=IKPowWithIntegerCheck(new_r02,-1);
6441 if(!x268.valid){
6442 continue;
6443 }
6444 CheckValue<IkReal> x269=IKPowWithIntegerCheck(cj5,-1);
6445 if(!x269.valid){
6446 continue;
6447 }
6448 if( IKabs(((-1.0)*new_r20*(x268.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x269.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x268.value)))+IKsqr((new_r01*(x269.value)))-1) <= IKFAST_SINCOS_THRESH )
6449  continue;
6450 j6array[0]=IKatan2(((-1.0)*new_r20*(x268.value)), (new_r01*(x269.value)));
6451 sj6array[0]=IKsin(j6array[0]);
6452 cj6array[0]=IKcos(j6array[0]);
6453 if( j6array[0] > IKPI )
6454 {
6455  j6array[0]-=IK2PI;
6456 }
6457 else if( j6array[0] < -IKPI )
6458 { j6array[0]+=IK2PI;
6459 }
6460 j6valid[0] = true;
6461 for(int ij6 = 0; ij6 < 1; ++ij6)
6462 {
6463 if( !j6valid[ij6] )
6464 {
6465  continue;
6466 }
6467 _ij6[0] = ij6; _ij6[1] = -1;
6468 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6469 {
6470 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6471 {
6472  j6valid[iij6]=false; _ij6[1] = iij6; break;
6473 }
6474 }
6475 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6476 {
6477 IkReal evalcond[8];
6478 IkReal x270=IKsin(j6);
6479 IkReal x271=IKcos(j6);
6480 IkReal x272=((1.0)*x271);
6481 IkReal x273=((1.0)*x270);
6482 evalcond[0]=(new_r20+((new_r02*x270)));
6483 evalcond[1]=(new_r21+((new_r02*x271)));
6484 evalcond[2]=(x270+(((-1.0)*new_r11)));
6485 evalcond[3]=((((-1.0)*cj5*x272))+new_r01);
6486 evalcond[4]=((((-1.0)*cj5*x273))+new_r00);
6487 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x272)));
6488 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x273)));
6489 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x272))+((new_r21*sj5)));
6490 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6491 {
6492 continue;
6493 }
6494 }
6495 
6496 {
6497 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6498 vinfos[0].jointtype = 17;
6499 vinfos[0].foffset = j0;
6500 vinfos[0].indices[0] = _ij0[0];
6501 vinfos[0].indices[1] = _ij0[1];
6502 vinfos[0].maxsolutions = _nj0;
6503 vinfos[1].jointtype = 1;
6504 vinfos[1].foffset = j1;
6505 vinfos[1].indices[0] = _ij1[0];
6506 vinfos[1].indices[1] = _ij1[1];
6507 vinfos[1].maxsolutions = _nj1;
6508 vinfos[2].jointtype = 1;
6509 vinfos[2].foffset = j2;
6510 vinfos[2].indices[0] = _ij2[0];
6511 vinfos[2].indices[1] = _ij2[1];
6512 vinfos[2].maxsolutions = _nj2;
6513 vinfos[3].jointtype = 1;
6514 vinfos[3].foffset = j3;
6515 vinfos[3].indices[0] = _ij3[0];
6516 vinfos[3].indices[1] = _ij3[1];
6517 vinfos[3].maxsolutions = _nj3;
6518 vinfos[4].jointtype = 1;
6519 vinfos[4].foffset = j4;
6520 vinfos[4].indices[0] = _ij4[0];
6521 vinfos[4].indices[1] = _ij4[1];
6522 vinfos[4].maxsolutions = _nj4;
6523 vinfos[5].jointtype = 1;
6524 vinfos[5].foffset = j5;
6525 vinfos[5].indices[0] = _ij5[0];
6526 vinfos[5].indices[1] = _ij5[1];
6527 vinfos[5].maxsolutions = _nj5;
6528 vinfos[6].jointtype = 1;
6529 vinfos[6].foffset = j6;
6530 vinfos[6].indices[0] = _ij6[0];
6531 vinfos[6].indices[1] = _ij6[1];
6532 vinfos[6].maxsolutions = _nj6;
6533 std::vector<int> vfree(0);
6534 solutions.AddSolution(vinfos,vfree);
6535 }
6536 }
6537 }
6538 
6539 }
6540 
6541 }
6542 
6543 } else
6544 {
6545 {
6546 IkReal j6array[1], cj6array[1], sj6array[1];
6547 bool j6valid[1]={false};
6548 _nj6 = 1;
6549 CheckValue<IkReal> x274=IKPowWithIntegerCheck(new_r02,-1);
6550 if(!x274.valid){
6551 continue;
6552 }
6553 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x274.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x274.value)))-1) <= IKFAST_SINCOS_THRESH )
6554  continue;
6555 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x274.value)));
6556 sj6array[0]=IKsin(j6array[0]);
6557 cj6array[0]=IKcos(j6array[0]);
6558 if( j6array[0] > IKPI )
6559 {
6560  j6array[0]-=IK2PI;
6561 }
6562 else if( j6array[0] < -IKPI )
6563 { j6array[0]+=IK2PI;
6564 }
6565 j6valid[0] = true;
6566 for(int ij6 = 0; ij6 < 1; ++ij6)
6567 {
6568 if( !j6valid[ij6] )
6569 {
6570  continue;
6571 }
6572 _ij6[0] = ij6; _ij6[1] = -1;
6573 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6574 {
6575 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6576 {
6577  j6valid[iij6]=false; _ij6[1] = iij6; break;
6578 }
6579 }
6580 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6581 {
6582 IkReal evalcond[8];
6583 IkReal x275=IKsin(j6);
6584 IkReal x276=IKcos(j6);
6585 IkReal x277=((1.0)*x276);
6586 IkReal x278=((1.0)*x275);
6587 evalcond[0]=(new_r20+((new_r02*x275)));
6588 evalcond[1]=(new_r21+((new_r02*x276)));
6589 evalcond[2]=(x275+(((-1.0)*new_r11)));
6590 evalcond[3]=((((-1.0)*cj5*x277))+new_r01);
6591 evalcond[4]=((((-1.0)*cj5*x278))+new_r00);
6592 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x277)));
6593 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x278)));
6594 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x277))+((new_r21*sj5)));
6595 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6596 {
6597 continue;
6598 }
6599 }
6600 
6601 {
6602 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6603 vinfos[0].jointtype = 17;
6604 vinfos[0].foffset = j0;
6605 vinfos[0].indices[0] = _ij0[0];
6606 vinfos[0].indices[1] = _ij0[1];
6607 vinfos[0].maxsolutions = _nj0;
6608 vinfos[1].jointtype = 1;
6609 vinfos[1].foffset = j1;
6610 vinfos[1].indices[0] = _ij1[0];
6611 vinfos[1].indices[1] = _ij1[1];
6612 vinfos[1].maxsolutions = _nj1;
6613 vinfos[2].jointtype = 1;
6614 vinfos[2].foffset = j2;
6615 vinfos[2].indices[0] = _ij2[0];
6616 vinfos[2].indices[1] = _ij2[1];
6617 vinfos[2].maxsolutions = _nj2;
6618 vinfos[3].jointtype = 1;
6619 vinfos[3].foffset = j3;
6620 vinfos[3].indices[0] = _ij3[0];
6621 vinfos[3].indices[1] = _ij3[1];
6622 vinfos[3].maxsolutions = _nj3;
6623 vinfos[4].jointtype = 1;
6624 vinfos[4].foffset = j4;
6625 vinfos[4].indices[0] = _ij4[0];
6626 vinfos[4].indices[1] = _ij4[1];
6627 vinfos[4].maxsolutions = _nj4;
6628 vinfos[5].jointtype = 1;
6629 vinfos[5].foffset = j5;
6630 vinfos[5].indices[0] = _ij5[0];
6631 vinfos[5].indices[1] = _ij5[1];
6632 vinfos[5].maxsolutions = _nj5;
6633 vinfos[6].jointtype = 1;
6634 vinfos[6].foffset = j6;
6635 vinfos[6].indices[0] = _ij6[0];
6636 vinfos[6].indices[1] = _ij6[1];
6637 vinfos[6].maxsolutions = _nj6;
6638 std::vector<int> vfree(0);
6639 solutions.AddSolution(vinfos,vfree);
6640 }
6641 }
6642 }
6643 
6644 }
6645 
6646 }
6647 
6648 } else
6649 {
6650 {
6651 IkReal j6array[1], cj6array[1], sj6array[1];
6652 bool j6valid[1]={false};
6653 _nj6 = 1;
6654 CheckValue<IkReal> x279=IKPowWithIntegerCheck(IKsign(new_r02),-1);
6655 if(!x279.valid){
6656 continue;
6657 }
6658 CheckValue<IkReal> x280 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH);
6659 if(!x280.valid){
6660 continue;
6661 }
6662 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x279.value)))+(x280.value));
6663 sj6array[0]=IKsin(j6array[0]);
6664 cj6array[0]=IKcos(j6array[0]);
6665 if( j6array[0] > IKPI )
6666 {
6667  j6array[0]-=IK2PI;
6668 }
6669 else if( j6array[0] < -IKPI )
6670 { j6array[0]+=IK2PI;
6671 }
6672 j6valid[0] = true;
6673 for(int ij6 = 0; ij6 < 1; ++ij6)
6674 {
6675 if( !j6valid[ij6] )
6676 {
6677  continue;
6678 }
6679 _ij6[0] = ij6; _ij6[1] = -1;
6680 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6681 {
6682 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6683 {
6684  j6valid[iij6]=false; _ij6[1] = iij6; break;
6685 }
6686 }
6687 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6688 {
6689 IkReal evalcond[8];
6690 IkReal x281=IKsin(j6);
6691 IkReal x282=IKcos(j6);
6692 IkReal x283=((1.0)*x282);
6693 IkReal x284=((1.0)*x281);
6694 evalcond[0]=(((new_r02*x281))+new_r20);
6695 evalcond[1]=(((new_r02*x282))+new_r21);
6696 evalcond[2]=(x281+(((-1.0)*new_r11)));
6697 evalcond[3]=((((-1.0)*cj5*x283))+new_r01);
6698 evalcond[4]=((((-1.0)*cj5*x284))+new_r00);
6699 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x283)));
6700 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x284)));
6701 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x283))+((new_r21*sj5)));
6702 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6703 {
6704 continue;
6705 }
6706 }
6707 
6708 {
6709 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6710 vinfos[0].jointtype = 17;
6711 vinfos[0].foffset = j0;
6712 vinfos[0].indices[0] = _ij0[0];
6713 vinfos[0].indices[1] = _ij0[1];
6714 vinfos[0].maxsolutions = _nj0;
6715 vinfos[1].jointtype = 1;
6716 vinfos[1].foffset = j1;
6717 vinfos[1].indices[0] = _ij1[0];
6718 vinfos[1].indices[1] = _ij1[1];
6719 vinfos[1].maxsolutions = _nj1;
6720 vinfos[2].jointtype = 1;
6721 vinfos[2].foffset = j2;
6722 vinfos[2].indices[0] = _ij2[0];
6723 vinfos[2].indices[1] = _ij2[1];
6724 vinfos[2].maxsolutions = _nj2;
6725 vinfos[3].jointtype = 1;
6726 vinfos[3].foffset = j3;
6727 vinfos[3].indices[0] = _ij3[0];
6728 vinfos[3].indices[1] = _ij3[1];
6729 vinfos[3].maxsolutions = _nj3;
6730 vinfos[4].jointtype = 1;
6731 vinfos[4].foffset = j4;
6732 vinfos[4].indices[0] = _ij4[0];
6733 vinfos[4].indices[1] = _ij4[1];
6734 vinfos[4].maxsolutions = _nj4;
6735 vinfos[5].jointtype = 1;
6736 vinfos[5].foffset = j5;
6737 vinfos[5].indices[0] = _ij5[0];
6738 vinfos[5].indices[1] = _ij5[1];
6739 vinfos[5].maxsolutions = _nj5;
6740 vinfos[6].jointtype = 1;
6741 vinfos[6].foffset = j6;
6742 vinfos[6].indices[0] = _ij6[0];
6743 vinfos[6].indices[1] = _ij6[1];
6744 vinfos[6].maxsolutions = _nj6;
6745 std::vector<int> vfree(0);
6746 solutions.AddSolution(vinfos,vfree);
6747 }
6748 }
6749 }
6750 
6751 }
6752 
6753 }
6754 
6755 }
6756 } while(0);
6757 if( bgotonextstatement )
6758 {
6759 bool bgotonextstatement = true;
6760 do
6761 {
6762 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
6763 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6764 {
6765 bgotonextstatement=false;
6766 {
6767 IkReal j6eval[1];
6768 new_r21=0;
6769 new_r20=0;
6770 new_r02=0;
6771 new_r12=0;
6772 j6eval[0]=1.0;
6773 if( IKabs(j6eval[0]) < 0.0000000100000000 )
6774 {
6775 continue; // no branches [j6]
6776 
6777 } else
6778 {
6779 IkReal op[2+1], zeror[2];
6780 int numroots;
6781 op[0]=1.0;
6782 op[1]=0;
6783 op[2]=-1.0;
6784 polyroots2(op,zeror,numroots);
6785 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
6786 int numsolutions = 0;
6787 for(int ij6 = 0; ij6 < numroots; ++ij6)
6788 {
6789 IkReal htj6 = zeror[ij6];
6790 tempj6array[0]=((2.0)*(atan(htj6)));
6791 for(int kj6 = 0; kj6 < 1; ++kj6)
6792 {
6793 j6array[numsolutions] = tempj6array[kj6];
6794 if( j6array[numsolutions] > IKPI )
6795 {
6796  j6array[numsolutions]-=IK2PI;
6797 }
6798 else if( j6array[numsolutions] < -IKPI )
6799 {
6800  j6array[numsolutions]+=IK2PI;
6801 }
6802 sj6array[numsolutions] = IKsin(j6array[numsolutions]);
6803 cj6array[numsolutions] = IKcos(j6array[numsolutions]);
6804 numsolutions++;
6805 }
6806 }
6807 bool j6valid[2]={true,true};
6808 _nj6 = 2;
6809 for(int ij6 = 0; ij6 < numsolutions; ++ij6)
6810  {
6811 if( !j6valid[ij6] )
6812 {
6813  continue;
6814 }
6815  j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6816 htj6 = IKtan(j6/2);
6817 
6818 _ij6[0] = ij6; _ij6[1] = -1;
6819 for(int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
6820 {
6821 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6822 {
6823  j6valid[iij6]=false; _ij6[1] = iij6; break;
6824 }
6825 }
6826 {
6827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6828 vinfos[0].jointtype = 17;
6829 vinfos[0].foffset = j0;
6830 vinfos[0].indices[0] = _ij0[0];
6831 vinfos[0].indices[1] = _ij0[1];
6832 vinfos[0].maxsolutions = _nj0;
6833 vinfos[1].jointtype = 1;
6834 vinfos[1].foffset = j1;
6835 vinfos[1].indices[0] = _ij1[0];
6836 vinfos[1].indices[1] = _ij1[1];
6837 vinfos[1].maxsolutions = _nj1;
6838 vinfos[2].jointtype = 1;
6839 vinfos[2].foffset = j2;
6840 vinfos[2].indices[0] = _ij2[0];
6841 vinfos[2].indices[1] = _ij2[1];
6842 vinfos[2].maxsolutions = _nj2;
6843 vinfos[3].jointtype = 1;
6844 vinfos[3].foffset = j3;
6845 vinfos[3].indices[0] = _ij3[0];
6846 vinfos[3].indices[1] = _ij3[1];
6847 vinfos[3].maxsolutions = _nj3;
6848 vinfos[4].jointtype = 1;
6849 vinfos[4].foffset = j4;
6850 vinfos[4].indices[0] = _ij4[0];
6851 vinfos[4].indices[1] = _ij4[1];
6852 vinfos[4].maxsolutions = _nj4;
6853 vinfos[5].jointtype = 1;
6854 vinfos[5].foffset = j5;
6855 vinfos[5].indices[0] = _ij5[0];
6856 vinfos[5].indices[1] = _ij5[1];
6857 vinfos[5].maxsolutions = _nj5;
6858 vinfos[6].jointtype = 1;
6859 vinfos[6].foffset = j6;
6860 vinfos[6].indices[0] = _ij6[0];
6861 vinfos[6].indices[1] = _ij6[1];
6862 vinfos[6].maxsolutions = _nj6;
6863 std::vector<int> vfree(0);
6864 solutions.AddSolution(vinfos,vfree);
6865 }
6866  }
6867 
6868 }
6869 
6870 }
6871 
6872 }
6873 } while(0);
6874 if( bgotonextstatement )
6875 {
6876 bool bgotonextstatement = true;
6877 do
6878 {
6879 if( 1 )
6880 {
6881 bgotonextstatement=false;
6882 continue; // branch miss [j6]
6883 
6884 }
6885 } while(0);
6886 if( bgotonextstatement )
6887 {
6888 }
6889 }
6890 }
6891 }
6892 }
6893 }
6894 }
6895 }
6896 }
6897 }
6898 }
6899 
6900 } else
6901 {
6902 {
6903 IkReal j6array[1], cj6array[1], sj6array[1];
6904 bool j6valid[1]={false};
6905 _nj6 = 1;
6906 CheckValue<IkReal> x286=IKPowWithIntegerCheck(sj5,-1);
6907 if(!x286.valid){
6908 continue;
6909 }
6910 IkReal x285=x286.value;
6911 CheckValue<IkReal> x287=IKPowWithIntegerCheck(cj5,-1);
6912 if(!x287.valid){
6913 continue;
6914 }
6915 CheckValue<IkReal> x288=IKPowWithIntegerCheck(sj4,-1);
6916 if(!x288.valid){
6917 continue;
6918 }
6919 if( IKabs((new_r20*x285)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x285*(x287.value)*(x288.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x285))+IKsqr((x285*(x287.value)*(x288.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20))))))-1) <= IKFAST_SINCOS_THRESH )
6920  continue;
6921 j6array[0]=IKatan2((new_r20*x285), (x285*(x287.value)*(x288.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20))))));
6922 sj6array[0]=IKsin(j6array[0]);
6923 cj6array[0]=IKcos(j6array[0]);
6924 if( j6array[0] > IKPI )
6925 {
6926  j6array[0]-=IK2PI;
6927 }
6928 else if( j6array[0] < -IKPI )
6929 { j6array[0]+=IK2PI;
6930 }
6931 j6valid[0] = true;
6932 for(int ij6 = 0; ij6 < 1; ++ij6)
6933 {
6934 if( !j6valid[ij6] )
6935 {
6936  continue;
6937 }
6938 _ij6[0] = ij6; _ij6[1] = -1;
6939 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
6940 {
6941 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
6942 {
6943  j6valid[iij6]=false; _ij6[1] = iij6; break;
6944 }
6945 }
6946 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6947 {
6948 IkReal evalcond[12];
6949 IkReal x289=IKsin(j6);
6950 IkReal x290=IKcos(j6);
6951 IkReal x291=((1.0)*sj4);
6952 IkReal x292=(cj4*cj5);
6953 IkReal x293=((1.0)*x290);
6954 IkReal x294=((1.0)*x289);
6955 IkReal x295=(sj4*x289);
6956 evalcond[0]=((((-1.0)*sj5*x294))+new_r20);
6957 evalcond[1]=((((-1.0)*sj5*x293))+new_r21);
6958 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x289);
6959 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x293)));
6960 evalcond[4]=(((cj5*sj4*x290))+new_r01+((cj4*x289)));
6961 evalcond[5]=((((-1.0)*cj4*x293))+((cj5*x295))+new_r00);
6962 evalcond[6]=(x295+new_r11+(((-1.0)*x292*x293)));
6963 evalcond[7]=(((cj4*new_r10))+(((-1.0)*cj5*x294))+(((-1.0)*new_r00*x291)));
6964 evalcond[8]=((((-1.0)*new_r01*x291))+((cj4*new_r11))+(((-1.0)*cj5*x293)));
6965 evalcond[9]=((((-1.0)*x290*x291))+new_r10+(((-1.0)*x292*x294)));
6966 evalcond[10]=((((-1.0)*cj5*new_r00*x291))+((new_r20*sj5))+(((-1.0)*x294))+((new_r10*x292)));
6967 evalcond[11]=(((new_r11*x292))+(((-1.0)*x293))+(((-1.0)*cj5*new_r01*x291))+((new_r21*sj5)));
6968 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
6969 {
6970 continue;
6971 }
6972 }
6973 
6974 {
6975 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6976 vinfos[0].jointtype = 17;
6977 vinfos[0].foffset = j0;
6978 vinfos[0].indices[0] = _ij0[0];
6979 vinfos[0].indices[1] = _ij0[1];
6980 vinfos[0].maxsolutions = _nj0;
6981 vinfos[1].jointtype = 1;
6982 vinfos[1].foffset = j1;
6983 vinfos[1].indices[0] = _ij1[0];
6984 vinfos[1].indices[1] = _ij1[1];
6985 vinfos[1].maxsolutions = _nj1;
6986 vinfos[2].jointtype = 1;
6987 vinfos[2].foffset = j2;
6988 vinfos[2].indices[0] = _ij2[0];
6989 vinfos[2].indices[1] = _ij2[1];
6990 vinfos[2].maxsolutions = _nj2;
6991 vinfos[3].jointtype = 1;
6992 vinfos[3].foffset = j3;
6993 vinfos[3].indices[0] = _ij3[0];
6994 vinfos[3].indices[1] = _ij3[1];
6995 vinfos[3].maxsolutions = _nj3;
6996 vinfos[4].jointtype = 1;
6997 vinfos[4].foffset = j4;
6998 vinfos[4].indices[0] = _ij4[0];
6999 vinfos[4].indices[1] = _ij4[1];
7000 vinfos[4].maxsolutions = _nj4;
7001 vinfos[5].jointtype = 1;
7002 vinfos[5].foffset = j5;
7003 vinfos[5].indices[0] = _ij5[0];
7004 vinfos[5].indices[1] = _ij5[1];
7005 vinfos[5].maxsolutions = _nj5;
7006 vinfos[6].jointtype = 1;
7007 vinfos[6].foffset = j6;
7008 vinfos[6].indices[0] = _ij6[0];
7009 vinfos[6].indices[1] = _ij6[1];
7010 vinfos[6].maxsolutions = _nj6;
7011 std::vector<int> vfree(0);
7012 solutions.AddSolution(vinfos,vfree);
7013 }
7014 }
7015 }
7016 
7017 }
7018 
7019 }
7020 
7021 } else
7022 {
7023 {
7024 IkReal j6array[1], cj6array[1], sj6array[1];
7025 bool j6valid[1]={false};
7026 _nj6 = 1;
7027 CheckValue<IkReal> x297=IKPowWithIntegerCheck(sj5,-1);
7028 if(!x297.valid){
7029 continue;
7030 }
7031 IkReal x296=x297.value;
7032 CheckValue<IkReal> x298=IKPowWithIntegerCheck(cj4,-1);
7033 if(!x298.valid){
7034 continue;
7035 }
7036 if( IKabs((new_r20*x296)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x296*(x298.value)*((((new_r00*sj5))+((cj5*new_r20*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x296))+IKsqr((x296*(x298.value)*((((new_r00*sj5))+((cj5*new_r20*sj4))))))-1) <= IKFAST_SINCOS_THRESH )
7037  continue;
7038 j6array[0]=IKatan2((new_r20*x296), (x296*(x298.value)*((((new_r00*sj5))+((cj5*new_r20*sj4))))));
7039 sj6array[0]=IKsin(j6array[0]);
7040 cj6array[0]=IKcos(j6array[0]);
7041 if( j6array[0] > IKPI )
7042 {
7043  j6array[0]-=IK2PI;
7044 }
7045 else if( j6array[0] < -IKPI )
7046 { j6array[0]+=IK2PI;
7047 }
7048 j6valid[0] = true;
7049 for(int ij6 = 0; ij6 < 1; ++ij6)
7050 {
7051 if( !j6valid[ij6] )
7052 {
7053  continue;
7054 }
7055 _ij6[0] = ij6; _ij6[1] = -1;
7056 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7057 {
7058 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7059 {
7060  j6valid[iij6]=false; _ij6[1] = iij6; break;
7061 }
7062 }
7063 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7064 {
7065 IkReal evalcond[12];
7066 IkReal x299=IKsin(j6);
7067 IkReal x300=IKcos(j6);
7068 IkReal x301=((1.0)*sj4);
7069 IkReal x302=(cj4*cj5);
7070 IkReal x303=((1.0)*x300);
7071 IkReal x304=((1.0)*x299);
7072 IkReal x305=(sj4*x299);
7073 evalcond[0]=(new_r20+(((-1.0)*sj5*x304)));
7074 evalcond[1]=(new_r21+(((-1.0)*sj5*x303)));
7075 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x299);
7076 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x303)));
7077 evalcond[4]=(((cj5*sj4*x300))+((cj4*x299))+new_r01);
7078 evalcond[5]=((((-1.0)*cj4*x303))+new_r00+((cj5*x305)));
7079 evalcond[6]=((((-1.0)*x302*x303))+x305+new_r11);
7080 evalcond[7]=(((cj4*new_r10))+(((-1.0)*cj5*x304))+(((-1.0)*new_r00*x301)));
7081 evalcond[8]=(((cj4*new_r11))+(((-1.0)*cj5*x303))+(((-1.0)*new_r01*x301)));
7082 evalcond[9]=((((-1.0)*x302*x304))+new_r10+(((-1.0)*x300*x301)));
7083 evalcond[10]=((((-1.0)*cj5*new_r00*x301))+((new_r20*sj5))+((new_r10*x302))+(((-1.0)*x304)));
7084 evalcond[11]=(((new_r11*x302))+(((-1.0)*cj5*new_r01*x301))+((new_r21*sj5))+(((-1.0)*x303)));
7085 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
7086 {
7087 continue;
7088 }
7089 }
7090 
7091 {
7092 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7093 vinfos[0].jointtype = 17;
7094 vinfos[0].foffset = j0;
7095 vinfos[0].indices[0] = _ij0[0];
7096 vinfos[0].indices[1] = _ij0[1];
7097 vinfos[0].maxsolutions = _nj0;
7098 vinfos[1].jointtype = 1;
7099 vinfos[1].foffset = j1;
7100 vinfos[1].indices[0] = _ij1[0];
7101 vinfos[1].indices[1] = _ij1[1];
7102 vinfos[1].maxsolutions = _nj1;
7103 vinfos[2].jointtype = 1;
7104 vinfos[2].foffset = j2;
7105 vinfos[2].indices[0] = _ij2[0];
7106 vinfos[2].indices[1] = _ij2[1];
7107 vinfos[2].maxsolutions = _nj2;
7108 vinfos[3].jointtype = 1;
7109 vinfos[3].foffset = j3;
7110 vinfos[3].indices[0] = _ij3[0];
7111 vinfos[3].indices[1] = _ij3[1];
7112 vinfos[3].maxsolutions = _nj3;
7113 vinfos[4].jointtype = 1;
7114 vinfos[4].foffset = j4;
7115 vinfos[4].indices[0] = _ij4[0];
7116 vinfos[4].indices[1] = _ij4[1];
7117 vinfos[4].maxsolutions = _nj4;
7118 vinfos[5].jointtype = 1;
7119 vinfos[5].foffset = j5;
7120 vinfos[5].indices[0] = _ij5[0];
7121 vinfos[5].indices[1] = _ij5[1];
7122 vinfos[5].maxsolutions = _nj5;
7123 vinfos[6].jointtype = 1;
7124 vinfos[6].foffset = j6;
7125 vinfos[6].indices[0] = _ij6[0];
7126 vinfos[6].indices[1] = _ij6[1];
7127 vinfos[6].maxsolutions = _nj6;
7128 std::vector<int> vfree(0);
7129 solutions.AddSolution(vinfos,vfree);
7130 }
7131 }
7132 }
7133 
7134 }
7135 
7136 }
7137 
7138 } else
7139 {
7140 {
7141 IkReal j6array[1], cj6array[1], sj6array[1];
7142 bool j6valid[1]={false};
7143 _nj6 = 1;
7144 CheckValue<IkReal> x306=IKPowWithIntegerCheck(IKsign(sj5),-1);
7145 if(!x306.valid){
7146 continue;
7147 }
7148 CheckValue<IkReal> x307 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
7149 if(!x307.valid){
7150 continue;
7151 }
7152 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x306.value)))+(x307.value));
7153 sj6array[0]=IKsin(j6array[0]);
7154 cj6array[0]=IKcos(j6array[0]);
7155 if( j6array[0] > IKPI )
7156 {
7157  j6array[0]-=IK2PI;
7158 }
7159 else if( j6array[0] < -IKPI )
7160 { j6array[0]+=IK2PI;
7161 }
7162 j6valid[0] = true;
7163 for(int ij6 = 0; ij6 < 1; ++ij6)
7164 {
7165 if( !j6valid[ij6] )
7166 {
7167  continue;
7168 }
7169 _ij6[0] = ij6; _ij6[1] = -1;
7170 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7171 {
7172 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7173 {
7174  j6valid[iij6]=false; _ij6[1] = iij6; break;
7175 }
7176 }
7177 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7178 {
7179 IkReal evalcond[12];
7180 IkReal x308=IKsin(j6);
7181 IkReal x309=IKcos(j6);
7182 IkReal x310=((1.0)*sj4);
7183 IkReal x311=(cj4*cj5);
7184 IkReal x312=((1.0)*x309);
7185 IkReal x313=((1.0)*x308);
7186 IkReal x314=(sj4*x308);
7187 evalcond[0]=((((-1.0)*sj5*x313))+new_r20);
7188 evalcond[1]=((((-1.0)*sj5*x312))+new_r21);
7189 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x308);
7190 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x312)));
7191 evalcond[4]=(((cj5*sj4*x309))+new_r01+((cj4*x308)));
7192 evalcond[5]=(((cj5*x314))+(((-1.0)*cj4*x312))+new_r00);
7193 evalcond[6]=(x314+new_r11+(((-1.0)*x311*x312)));
7194 evalcond[7]=((((-1.0)*new_r00*x310))+((cj4*new_r10))+(((-1.0)*cj5*x313)));
7195 evalcond[8]=(((cj4*new_r11))+(((-1.0)*cj5*x312))+(((-1.0)*new_r01*x310)));
7196 evalcond[9]=(new_r10+(((-1.0)*x309*x310))+(((-1.0)*x311*x313)));
7197 evalcond[10]=(((new_r20*sj5))+(((-1.0)*cj5*new_r00*x310))+(((-1.0)*x313))+((new_r10*x311)));
7198 evalcond[11]=((((-1.0)*cj5*new_r01*x310))+(((-1.0)*x312))+((new_r11*x311))+((new_r21*sj5)));
7199 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
7200 {
7201 continue;
7202 }
7203 }
7204 
7205 {
7206 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7207 vinfos[0].jointtype = 17;
7208 vinfos[0].foffset = j0;
7209 vinfos[0].indices[0] = _ij0[0];
7210 vinfos[0].indices[1] = _ij0[1];
7211 vinfos[0].maxsolutions = _nj0;
7212 vinfos[1].jointtype = 1;
7213 vinfos[1].foffset = j1;
7214 vinfos[1].indices[0] = _ij1[0];
7215 vinfos[1].indices[1] = _ij1[1];
7216 vinfos[1].maxsolutions = _nj1;
7217 vinfos[2].jointtype = 1;
7218 vinfos[2].foffset = j2;
7219 vinfos[2].indices[0] = _ij2[0];
7220 vinfos[2].indices[1] = _ij2[1];
7221 vinfos[2].maxsolutions = _nj2;
7222 vinfos[3].jointtype = 1;
7223 vinfos[3].foffset = j3;
7224 vinfos[3].indices[0] = _ij3[0];
7225 vinfos[3].indices[1] = _ij3[1];
7226 vinfos[3].maxsolutions = _nj3;
7227 vinfos[4].jointtype = 1;
7228 vinfos[4].foffset = j4;
7229 vinfos[4].indices[0] = _ij4[0];
7230 vinfos[4].indices[1] = _ij4[1];
7231 vinfos[4].maxsolutions = _nj4;
7232 vinfos[5].jointtype = 1;
7233 vinfos[5].foffset = j5;
7234 vinfos[5].indices[0] = _ij5[0];
7235 vinfos[5].indices[1] = _ij5[1];
7236 vinfos[5].maxsolutions = _nj5;
7237 vinfos[6].jointtype = 1;
7238 vinfos[6].foffset = j6;
7239 vinfos[6].indices[0] = _ij6[0];
7240 vinfos[6].indices[1] = _ij6[1];
7241 vinfos[6].maxsolutions = _nj6;
7242 std::vector<int> vfree(0);
7243 solutions.AddSolution(vinfos,vfree);
7244 }
7245 }
7246 }
7247 
7248 }
7249 
7250 }
7251 }
7252 }
7253 
7254 }
7255 
7256 }
7257 
7258 } else
7259 {
7260 {
7261 IkReal j4array[1], cj4array[1], sj4array[1];
7262 bool j4valid[1]={false};
7263 _nj4 = 1;
7264 CheckValue<IkReal> x315=IKPowWithIntegerCheck(IKsign(sj5),-1);
7265 if(!x315.valid){
7266 continue;
7267 }
7268 CheckValue<IkReal> x316 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH);
7269 if(!x316.valid){
7270 continue;
7271 }
7272 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x315.value)))+(x316.value));
7273 sj4array[0]=IKsin(j4array[0]);
7274 cj4array[0]=IKcos(j4array[0]);
7275 if( j4array[0] > IKPI )
7276 {
7277  j4array[0]-=IK2PI;
7278 }
7279 else if( j4array[0] < -IKPI )
7280 { j4array[0]+=IK2PI;
7281 }
7282 j4valid[0] = true;
7283 for(int ij4 = 0; ij4 < 1; ++ij4)
7284 {
7285 if( !j4valid[ij4] )
7286 {
7287  continue;
7288 }
7289 _ij4[0] = ij4; _ij4[1] = -1;
7290 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
7291 {
7292 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
7293 {
7294  j4valid[iij4]=false; _ij4[1] = iij4; break;
7295 }
7296 }
7297 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
7298 {
7299 IkReal evalcond[8];
7300 IkReal x317=IKcos(j4);
7301 IkReal x318=IKsin(j4);
7302 IkReal x319=((1.0)*cj5);
7303 IkReal x320=((1.0)*x318);
7304 IkReal x321=(sj5*x317);
7305 IkReal x322=(new_r12*x317);
7306 evalcond[0]=(x321+new_r12);
7307 evalcond[1]=(new_r02+(((-1.0)*sj5*x320)));
7308 evalcond[2]=(((new_r02*x317))+((new_r12*x318)));
7309 evalcond[3]=(sj5+(((-1.0)*new_r02*x320))+x322);
7310 evalcond[4]=(((cj5*x322))+((new_r22*sj5))+(((-1.0)*new_r02*x318*x319)));
7311 evalcond[5]=((((-1.0)*new_r20*x319))+(((-1.0)*new_r00*sj5*x320))+((new_r10*x321)));
7312 evalcond[6]=((((-1.0)*new_r21*x319))+(((-1.0)*new_r01*sj5*x320))+((new_r11*x321)));
7313 evalcond[7]=((1.0)+((new_r12*x321))+(((-1.0)*new_r22*x319))+(((-1.0)*new_r02*sj5*x320)));
7314 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7315 {
7316 continue;
7317 }
7318 }
7319 
7320 {
7321 IkReal j6eval[3];
7322 j6eval[0]=sj5;
7323 j6eval[1]=IKsign(sj5);
7324 j6eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
7325 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
7326 {
7327 {
7328 IkReal j6eval[2];
7329 j6eval[0]=sj5;
7330 j6eval[1]=cj4;
7331 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 )
7332 {
7333 {
7334 IkReal j6eval[3];
7335 j6eval[0]=sj5;
7336 j6eval[1]=cj5;
7337 j6eval[2]=sj4;
7338 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
7339 {
7340 {
7341 IkReal evalcond[5];
7342 bool bgotonextstatement = true;
7343 do
7344 {
7345 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
7346 evalcond[1]=new_r12;
7347 evalcond[2]=new_r02;
7348 evalcond[3]=new_r20;
7349 evalcond[4]=new_r21;
7350 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
7351 {
7352 bgotonextstatement=false;
7353 {
7354 IkReal j6array[1], cj6array[1], sj6array[1];
7355 bool j6valid[1]={false};
7356 _nj6 = 1;
7357 IkReal x323=((1.0)*new_r01);
7358 if( IKabs(((((-1.0)*new_r00*sj4))+(((-1.0)*cj4*x323)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj4*new_r00))+(((-1.0)*sj4*x323)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*sj4))+(((-1.0)*cj4*x323))))+IKsqr((((cj4*new_r00))+(((-1.0)*sj4*x323))))-1) <= IKFAST_SINCOS_THRESH )
7359  continue;
7360 j6array[0]=IKatan2(((((-1.0)*new_r00*sj4))+(((-1.0)*cj4*x323))), (((cj4*new_r00))+(((-1.0)*sj4*x323))));
7361 sj6array[0]=IKsin(j6array[0]);
7362 cj6array[0]=IKcos(j6array[0]);
7363 if( j6array[0] > IKPI )
7364 {
7365  j6array[0]-=IK2PI;
7366 }
7367 else if( j6array[0] < -IKPI )
7368 { j6array[0]+=IK2PI;
7369 }
7370 j6valid[0] = true;
7371 for(int ij6 = 0; ij6 < 1; ++ij6)
7372 {
7373 if( !j6valid[ij6] )
7374 {
7375  continue;
7376 }
7377 _ij6[0] = ij6; _ij6[1] = -1;
7378 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7379 {
7380 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7381 {
7382  j6valid[iij6]=false; _ij6[1] = iij6; break;
7383 }
7384 }
7385 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7386 {
7387 IkReal evalcond[8];
7388 IkReal x324=IKcos(j6);
7389 IkReal x325=IKsin(j6);
7390 IkReal x326=((1.0)*sj4);
7391 IkReal x327=(sj4*x325);
7392 IkReal x328=((1.0)*x324);
7393 IkReal x329=(sj4*x324);
7394 IkReal x330=(cj4*x325);
7395 IkReal x331=(cj4*x328);
7396 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x325);
7397 evalcond[1]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x328)));
7398 evalcond[2]=(x330+x329+new_r01);
7399 evalcond[3]=(x327+(((-1.0)*x331))+new_r00);
7400 evalcond[4]=(x327+(((-1.0)*x331))+new_r11);
7401 evalcond[5]=(((cj4*new_r10))+(((-1.0)*x325))+(((-1.0)*new_r00*x326)));
7402 evalcond[6]=(((cj4*new_r11))+(((-1.0)*x328))+(((-1.0)*new_r01*x326)));
7403 evalcond[7]=((((-1.0)*x330))+new_r10+(((-1.0)*x324*x326)));
7404 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7405 {
7406 continue;
7407 }
7408 }
7409 
7410 {
7411 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7412 vinfos[0].jointtype = 17;
7413 vinfos[0].foffset = j0;
7414 vinfos[0].indices[0] = _ij0[0];
7415 vinfos[0].indices[1] = _ij0[1];
7416 vinfos[0].maxsolutions = _nj0;
7417 vinfos[1].jointtype = 1;
7418 vinfos[1].foffset = j1;
7419 vinfos[1].indices[0] = _ij1[0];
7420 vinfos[1].indices[1] = _ij1[1];
7421 vinfos[1].maxsolutions = _nj1;
7422 vinfos[2].jointtype = 1;
7423 vinfos[2].foffset = j2;
7424 vinfos[2].indices[0] = _ij2[0];
7425 vinfos[2].indices[1] = _ij2[1];
7426 vinfos[2].maxsolutions = _nj2;
7427 vinfos[3].jointtype = 1;
7428 vinfos[3].foffset = j3;
7429 vinfos[3].indices[0] = _ij3[0];
7430 vinfos[3].indices[1] = _ij3[1];
7431 vinfos[3].maxsolutions = _nj3;
7432 vinfos[4].jointtype = 1;
7433 vinfos[4].foffset = j4;
7434 vinfos[4].indices[0] = _ij4[0];
7435 vinfos[4].indices[1] = _ij4[1];
7436 vinfos[4].maxsolutions = _nj4;
7437 vinfos[5].jointtype = 1;
7438 vinfos[5].foffset = j5;
7439 vinfos[5].indices[0] = _ij5[0];
7440 vinfos[5].indices[1] = _ij5[1];
7441 vinfos[5].maxsolutions = _nj5;
7442 vinfos[6].jointtype = 1;
7443 vinfos[6].foffset = j6;
7444 vinfos[6].indices[0] = _ij6[0];
7445 vinfos[6].indices[1] = _ij6[1];
7446 vinfos[6].maxsolutions = _nj6;
7447 std::vector<int> vfree(0);
7448 solutions.AddSolution(vinfos,vfree);
7449 }
7450 }
7451 }
7452 
7453 }
7454 } while(0);
7455 if( bgotonextstatement )
7456 {
7457 bool bgotonextstatement = true;
7458 do
7459 {
7460 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
7461 evalcond[1]=new_r12;
7462 evalcond[2]=new_r02;
7463 evalcond[3]=new_r20;
7464 evalcond[4]=new_r21;
7465 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
7466 {
7467 bgotonextstatement=false;
7468 {
7469 IkReal j6array[1], cj6array[1], sj6array[1];
7470 bool j6valid[1]={false};
7471 _nj6 = 1;
7472 IkReal x332=((1.0)*new_r11);
7473 if( IKabs(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x332)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj4))+(((-1.0)*cj4*x332)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x332))))+IKsqr((((new_r01*sj4))+(((-1.0)*cj4*x332))))-1) <= IKFAST_SINCOS_THRESH )
7474  continue;
7475 j6array[0]=IKatan2(((((-1.0)*cj4*new_r01))+(((-1.0)*sj4*x332))), (((new_r01*sj4))+(((-1.0)*cj4*x332))));
7476 sj6array[0]=IKsin(j6array[0]);
7477 cj6array[0]=IKcos(j6array[0]);
7478 if( j6array[0] > IKPI )
7479 {
7480  j6array[0]-=IK2PI;
7481 }
7482 else if( j6array[0] < -IKPI )
7483 { j6array[0]+=IK2PI;
7484 }
7485 j6valid[0] = true;
7486 for(int ij6 = 0; ij6 < 1; ++ij6)
7487 {
7488 if( !j6valid[ij6] )
7489 {
7490  continue;
7491 }
7492 _ij6[0] = ij6; _ij6[1] = -1;
7493 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7494 {
7495 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7496 {
7497  j6valid[iij6]=false; _ij6[1] = iij6; break;
7498 }
7499 }
7500 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7501 {
7502 IkReal evalcond[8];
7503 IkReal x333=IKsin(j6);
7504 IkReal x334=IKcos(j6);
7505 IkReal x335=((1.0)*sj4);
7506 IkReal x336=(cj4*x333);
7507 IkReal x337=((1.0)*x334);
7508 IkReal x338=(x334*x335);
7509 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x333);
7510 evalcond[1]=(((cj4*new_r10))+x333+(((-1.0)*new_r00*x335)));
7511 evalcond[2]=(((cj4*new_r11))+x334+(((-1.0)*new_r01*x335)));
7512 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x337)));
7513 evalcond[4]=(((sj4*x333))+((cj4*x334))+new_r11);
7514 evalcond[5]=(x336+(((-1.0)*x338))+new_r01);
7515 evalcond[6]=(x336+(((-1.0)*x338))+new_r10);
7516 evalcond[7]=((((-1.0)*x333*x335))+(((-1.0)*cj4*x337))+new_r00);
7517 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7518 {
7519 continue;
7520 }
7521 }
7522 
7523 {
7524 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7525 vinfos[0].jointtype = 17;
7526 vinfos[0].foffset = j0;
7527 vinfos[0].indices[0] = _ij0[0];
7528 vinfos[0].indices[1] = _ij0[1];
7529 vinfos[0].maxsolutions = _nj0;
7530 vinfos[1].jointtype = 1;
7531 vinfos[1].foffset = j1;
7532 vinfos[1].indices[0] = _ij1[0];
7533 vinfos[1].indices[1] = _ij1[1];
7534 vinfos[1].maxsolutions = _nj1;
7535 vinfos[2].jointtype = 1;
7536 vinfos[2].foffset = j2;
7537 vinfos[2].indices[0] = _ij2[0];
7538 vinfos[2].indices[1] = _ij2[1];
7539 vinfos[2].maxsolutions = _nj2;
7540 vinfos[3].jointtype = 1;
7541 vinfos[3].foffset = j3;
7542 vinfos[3].indices[0] = _ij3[0];
7543 vinfos[3].indices[1] = _ij3[1];
7544 vinfos[3].maxsolutions = _nj3;
7545 vinfos[4].jointtype = 1;
7546 vinfos[4].foffset = j4;
7547 vinfos[4].indices[0] = _ij4[0];
7548 vinfos[4].indices[1] = _ij4[1];
7549 vinfos[4].maxsolutions = _nj4;
7550 vinfos[5].jointtype = 1;
7551 vinfos[5].foffset = j5;
7552 vinfos[5].indices[0] = _ij5[0];
7553 vinfos[5].indices[1] = _ij5[1];
7554 vinfos[5].maxsolutions = _nj5;
7555 vinfos[6].jointtype = 1;
7556 vinfos[6].foffset = j6;
7557 vinfos[6].indices[0] = _ij6[0];
7558 vinfos[6].indices[1] = _ij6[1];
7559 vinfos[6].maxsolutions = _nj6;
7560 std::vector<int> vfree(0);
7561 solutions.AddSolution(vinfos,vfree);
7562 }
7563 }
7564 }
7565 
7566 }
7567 } while(0);
7568 if( bgotonextstatement )
7569 {
7570 bool bgotonextstatement = true;
7571 do
7572 {
7573 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
7574 evalcond[1]=new_r22;
7575 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7576 {
7577 bgotonextstatement=false;
7578 {
7579 IkReal j6array[1], cj6array[1], sj6array[1];
7580 bool j6valid[1]={false};
7581 _nj6 = 1;
7582 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
7583  continue;
7584 j6array[0]=IKatan2(new_r20, new_r21);
7585 sj6array[0]=IKsin(j6array[0]);
7586 cj6array[0]=IKcos(j6array[0]);
7587 if( j6array[0] > IKPI )
7588 {
7589  j6array[0]-=IK2PI;
7590 }
7591 else if( j6array[0] < -IKPI )
7592 { j6array[0]+=IK2PI;
7593 }
7594 j6valid[0] = true;
7595 for(int ij6 = 0; ij6 < 1; ++ij6)
7596 {
7597 if( !j6valid[ij6] )
7598 {
7599  continue;
7600 }
7601 _ij6[0] = ij6; _ij6[1] = -1;
7602 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7603 {
7604 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7605 {
7606  j6valid[iij6]=false; _ij6[1] = iij6; break;
7607 }
7608 }
7609 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7610 {
7611 IkReal evalcond[8];
7612 IkReal x339=IKsin(j6);
7613 IkReal x340=IKcos(j6);
7614 IkReal x341=((1.0)*x340);
7615 evalcond[0]=((((-1.0)*x339))+new_r20);
7616 evalcond[1]=(new_r21+(((-1.0)*x341)));
7617 evalcond[2]=(((cj4*x339))+new_r01);
7618 evalcond[3]=(((sj4*x339))+new_r11);
7619 evalcond[4]=((((-1.0)*cj4*x341))+new_r00);
7620 evalcond[5]=((((-1.0)*new_r02*x341))+new_r10);
7621 evalcond[6]=(((cj4*new_r01))+((new_r11*sj4))+x339);
7622 evalcond[7]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x341)));
7623 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7624 {
7625 continue;
7626 }
7627 }
7628 
7629 {
7630 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7631 vinfos[0].jointtype = 17;
7632 vinfos[0].foffset = j0;
7633 vinfos[0].indices[0] = _ij0[0];
7634 vinfos[0].indices[1] = _ij0[1];
7635 vinfos[0].maxsolutions = _nj0;
7636 vinfos[1].jointtype = 1;
7637 vinfos[1].foffset = j1;
7638 vinfos[1].indices[0] = _ij1[0];
7639 vinfos[1].indices[1] = _ij1[1];
7640 vinfos[1].maxsolutions = _nj1;
7641 vinfos[2].jointtype = 1;
7642 vinfos[2].foffset = j2;
7643 vinfos[2].indices[0] = _ij2[0];
7644 vinfos[2].indices[1] = _ij2[1];
7645 vinfos[2].maxsolutions = _nj2;
7646 vinfos[3].jointtype = 1;
7647 vinfos[3].foffset = j3;
7648 vinfos[3].indices[0] = _ij3[0];
7649 vinfos[3].indices[1] = _ij3[1];
7650 vinfos[3].maxsolutions = _nj3;
7651 vinfos[4].jointtype = 1;
7652 vinfos[4].foffset = j4;
7653 vinfos[4].indices[0] = _ij4[0];
7654 vinfos[4].indices[1] = _ij4[1];
7655 vinfos[4].maxsolutions = _nj4;
7656 vinfos[5].jointtype = 1;
7657 vinfos[5].foffset = j5;
7658 vinfos[5].indices[0] = _ij5[0];
7659 vinfos[5].indices[1] = _ij5[1];
7660 vinfos[5].maxsolutions = _nj5;
7661 vinfos[6].jointtype = 1;
7662 vinfos[6].foffset = j6;
7663 vinfos[6].indices[0] = _ij6[0];
7664 vinfos[6].indices[1] = _ij6[1];
7665 vinfos[6].maxsolutions = _nj6;
7666 std::vector<int> vfree(0);
7667 solutions.AddSolution(vinfos,vfree);
7668 }
7669 }
7670 }
7671 
7672 }
7673 } while(0);
7674 if( bgotonextstatement )
7675 {
7676 bool bgotonextstatement = true;
7677 do
7678 {
7679 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
7680 evalcond[1]=new_r22;
7681 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7682 {
7683 bgotonextstatement=false;
7684 {
7685 IkReal j6array[1], cj6array[1], sj6array[1];
7686 bool j6valid[1]={false};
7687 _nj6 = 1;
7688 if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
7689  continue;
7690 j6array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21));
7691 sj6array[0]=IKsin(j6array[0]);
7692 cj6array[0]=IKcos(j6array[0]);
7693 if( j6array[0] > IKPI )
7694 {
7695  j6array[0]-=IK2PI;
7696 }
7697 else if( j6array[0] < -IKPI )
7698 { j6array[0]+=IK2PI;
7699 }
7700 j6valid[0] = true;
7701 for(int ij6 = 0; ij6 < 1; ++ij6)
7702 {
7703 if( !j6valid[ij6] )
7704 {
7705  continue;
7706 }
7707 _ij6[0] = ij6; _ij6[1] = -1;
7708 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7709 {
7710 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7711 {
7712  j6valid[iij6]=false; _ij6[1] = iij6; break;
7713 }
7714 }
7715 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7716 {
7717 IkReal evalcond[8];
7718 IkReal x342=IKsin(j6);
7719 IkReal x343=IKcos(j6);
7720 IkReal x344=((1.0)*x343);
7721 evalcond[0]=(x342+new_r20);
7722 evalcond[1]=(x343+new_r21);
7723 evalcond[2]=(((cj4*x342))+new_r01);
7724 evalcond[3]=(((sj4*x342))+new_r11);
7725 evalcond[4]=(((new_r02*x343))+new_r10);
7726 evalcond[5]=((((-1.0)*cj4*x344))+new_r00);
7727 evalcond[6]=(((cj4*new_r01))+((new_r11*sj4))+x342);
7728 evalcond[7]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x344)));
7729 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7730 {
7731 continue;
7732 }
7733 }
7734 
7735 {
7736 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7737 vinfos[0].jointtype = 17;
7738 vinfos[0].foffset = j0;
7739 vinfos[0].indices[0] = _ij0[0];
7740 vinfos[0].indices[1] = _ij0[1];
7741 vinfos[0].maxsolutions = _nj0;
7742 vinfos[1].jointtype = 1;
7743 vinfos[1].foffset = j1;
7744 vinfos[1].indices[0] = _ij1[0];
7745 vinfos[1].indices[1] = _ij1[1];
7746 vinfos[1].maxsolutions = _nj1;
7747 vinfos[2].jointtype = 1;
7748 vinfos[2].foffset = j2;
7749 vinfos[2].indices[0] = _ij2[0];
7750 vinfos[2].indices[1] = _ij2[1];
7751 vinfos[2].maxsolutions = _nj2;
7752 vinfos[3].jointtype = 1;
7753 vinfos[3].foffset = j3;
7754 vinfos[3].indices[0] = _ij3[0];
7755 vinfos[3].indices[1] = _ij3[1];
7756 vinfos[3].maxsolutions = _nj3;
7757 vinfos[4].jointtype = 1;
7758 vinfos[4].foffset = j4;
7759 vinfos[4].indices[0] = _ij4[0];
7760 vinfos[4].indices[1] = _ij4[1];
7761 vinfos[4].maxsolutions = _nj4;
7762 vinfos[5].jointtype = 1;
7763 vinfos[5].foffset = j5;
7764 vinfos[5].indices[0] = _ij5[0];
7765 vinfos[5].indices[1] = _ij5[1];
7766 vinfos[5].maxsolutions = _nj5;
7767 vinfos[6].jointtype = 1;
7768 vinfos[6].foffset = j6;
7769 vinfos[6].indices[0] = _ij6[0];
7770 vinfos[6].indices[1] = _ij6[1];
7771 vinfos[6].maxsolutions = _nj6;
7772 std::vector<int> vfree(0);
7773 solutions.AddSolution(vinfos,vfree);
7774 }
7775 }
7776 }
7777 
7778 }
7779 } while(0);
7780 if( bgotonextstatement )
7781 {
7782 bool bgotonextstatement = true;
7783 do
7784 {
7785 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
7786 evalcond[1]=new_r02;
7787 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7788 {
7789 bgotonextstatement=false;
7790 {
7791 IkReal j6array[1], cj6array[1], sj6array[1];
7792 bool j6valid[1]={false};
7793 _nj6 = 1;
7794 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
7795  continue;
7796 j6array[0]=IKatan2(((-1.0)*new_r01), new_r00);
7797 sj6array[0]=IKsin(j6array[0]);
7798 cj6array[0]=IKcos(j6array[0]);
7799 if( j6array[0] > IKPI )
7800 {
7801  j6array[0]-=IK2PI;
7802 }
7803 else if( j6array[0] < -IKPI )
7804 { j6array[0]+=IK2PI;
7805 }
7806 j6valid[0] = true;
7807 for(int ij6 = 0; ij6 < 1; ++ij6)
7808 {
7809 if( !j6valid[ij6] )
7810 {
7811  continue;
7812 }
7813 _ij6[0] = ij6; _ij6[1] = -1;
7814 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7815 {
7816 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7817 {
7818  j6valid[iij6]=false; _ij6[1] = iij6; break;
7819 }
7820 }
7821 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7822 {
7823 IkReal evalcond[8];
7824 IkReal x345=IKsin(j6);
7825 IkReal x346=IKcos(j6);
7826 IkReal x347=((1.0)*x346);
7827 IkReal x348=((1.0)*x345);
7828 evalcond[0]=(x345+new_r01);
7829 evalcond[1]=(((new_r12*x345))+new_r20);
7830 evalcond[2]=(((new_r12*x346))+new_r21);
7831 evalcond[3]=(new_r00+(((-1.0)*x347)));
7832 evalcond[4]=((((-1.0)*cj5*x347))+new_r11);
7833 evalcond[5]=((((-1.0)*cj5*x348))+new_r10);
7834 evalcond[6]=(((new_r20*sj5))+((cj5*new_r10))+(((-1.0)*x348)));
7835 evalcond[7]=(((cj5*new_r11))+((new_r21*sj5))+(((-1.0)*x347)));
7836 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7837 {
7838 continue;
7839 }
7840 }
7841 
7842 {
7843 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7844 vinfos[0].jointtype = 17;
7845 vinfos[0].foffset = j0;
7846 vinfos[0].indices[0] = _ij0[0];
7847 vinfos[0].indices[1] = _ij0[1];
7848 vinfos[0].maxsolutions = _nj0;
7849 vinfos[1].jointtype = 1;
7850 vinfos[1].foffset = j1;
7851 vinfos[1].indices[0] = _ij1[0];
7852 vinfos[1].indices[1] = _ij1[1];
7853 vinfos[1].maxsolutions = _nj1;
7854 vinfos[2].jointtype = 1;
7855 vinfos[2].foffset = j2;
7856 vinfos[2].indices[0] = _ij2[0];
7857 vinfos[2].indices[1] = _ij2[1];
7858 vinfos[2].maxsolutions = _nj2;
7859 vinfos[3].jointtype = 1;
7860 vinfos[3].foffset = j3;
7861 vinfos[3].indices[0] = _ij3[0];
7862 vinfos[3].indices[1] = _ij3[1];
7863 vinfos[3].maxsolutions = _nj3;
7864 vinfos[4].jointtype = 1;
7865 vinfos[4].foffset = j4;
7866 vinfos[4].indices[0] = _ij4[0];
7867 vinfos[4].indices[1] = _ij4[1];
7868 vinfos[4].maxsolutions = _nj4;
7869 vinfos[5].jointtype = 1;
7870 vinfos[5].foffset = j5;
7871 vinfos[5].indices[0] = _ij5[0];
7872 vinfos[5].indices[1] = _ij5[1];
7873 vinfos[5].maxsolutions = _nj5;
7874 vinfos[6].jointtype = 1;
7875 vinfos[6].foffset = j6;
7876 vinfos[6].indices[0] = _ij6[0];
7877 vinfos[6].indices[1] = _ij6[1];
7878 vinfos[6].maxsolutions = _nj6;
7879 std::vector<int> vfree(0);
7880 solutions.AddSolution(vinfos,vfree);
7881 }
7882 }
7883 }
7884 
7885 }
7886 } while(0);
7887 if( bgotonextstatement )
7888 {
7889 bool bgotonextstatement = true;
7890 do
7891 {
7892 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
7893 evalcond[1]=new_r02;
7894 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7895 {
7896 bgotonextstatement=false;
7897 {
7898 IkReal j6array[1], cj6array[1], sj6array[1];
7899 bool j6valid[1]={false};
7900 _nj6 = 1;
7901 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
7902  continue;
7903 j6array[0]=IKatan2(new_r01, ((-1.0)*new_r00));
7904 sj6array[0]=IKsin(j6array[0]);
7905 cj6array[0]=IKcos(j6array[0]);
7906 if( j6array[0] > IKPI )
7907 {
7908  j6array[0]-=IK2PI;
7909 }
7910 else if( j6array[0] < -IKPI )
7911 { j6array[0]+=IK2PI;
7912 }
7913 j6valid[0] = true;
7914 for(int ij6 = 0; ij6 < 1; ++ij6)
7915 {
7916 if( !j6valid[ij6] )
7917 {
7918  continue;
7919 }
7920 _ij6[0] = ij6; _ij6[1] = -1;
7921 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
7922 {
7923 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
7924 {
7925  j6valid[iij6]=false; _ij6[1] = iij6; break;
7926 }
7927 }
7928 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7929 {
7930 IkReal evalcond[8];
7931 IkReal x349=IKsin(j6);
7932 IkReal x350=IKcos(j6);
7933 IkReal x351=((1.0)*cj5);
7934 IkReal x352=((1.0)*x350);
7935 IkReal x353=((1.0)*x349);
7936 evalcond[0]=(x349+(((-1.0)*new_r01)));
7937 evalcond[1]=(((cj5*x350))+new_r11);
7938 evalcond[2]=((((-1.0)*new_r12*x353))+new_r20);
7939 evalcond[3]=((((-1.0)*new_r12*x352))+new_r21);
7940 evalcond[4]=((((-1.0)*x352))+(((-1.0)*new_r00)));
7941 evalcond[5]=((((-1.0)*x349*x351))+(((-1.0)*new_r10)));
7942 evalcond[6]=(((new_r20*sj5))+(((-1.0)*x353))+(((-1.0)*new_r10*x351)));
7943 evalcond[7]=((((-1.0)*x352))+(((-1.0)*new_r11*x351))+((new_r21*sj5)));
7944 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7945 {
7946 continue;
7947 }
7948 }
7949 
7950 {
7951 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7952 vinfos[0].jointtype = 17;
7953 vinfos[0].foffset = j0;
7954 vinfos[0].indices[0] = _ij0[0];
7955 vinfos[0].indices[1] = _ij0[1];
7956 vinfos[0].maxsolutions = _nj0;
7957 vinfos[1].jointtype = 1;
7958 vinfos[1].foffset = j1;
7959 vinfos[1].indices[0] = _ij1[0];
7960 vinfos[1].indices[1] = _ij1[1];
7961 vinfos[1].maxsolutions = _nj1;
7962 vinfos[2].jointtype = 1;
7963 vinfos[2].foffset = j2;
7964 vinfos[2].indices[0] = _ij2[0];
7965 vinfos[2].indices[1] = _ij2[1];
7966 vinfos[2].maxsolutions = _nj2;
7967 vinfos[3].jointtype = 1;
7968 vinfos[3].foffset = j3;
7969 vinfos[3].indices[0] = _ij3[0];
7970 vinfos[3].indices[1] = _ij3[1];
7971 vinfos[3].maxsolutions = _nj3;
7972 vinfos[4].jointtype = 1;
7973 vinfos[4].foffset = j4;
7974 vinfos[4].indices[0] = _ij4[0];
7975 vinfos[4].indices[1] = _ij4[1];
7976 vinfos[4].maxsolutions = _nj4;
7977 vinfos[5].jointtype = 1;
7978 vinfos[5].foffset = j5;
7979 vinfos[5].indices[0] = _ij5[0];
7980 vinfos[5].indices[1] = _ij5[1];
7981 vinfos[5].maxsolutions = _nj5;
7982 vinfos[6].jointtype = 1;
7983 vinfos[6].foffset = j6;
7984 vinfos[6].indices[0] = _ij6[0];
7985 vinfos[6].indices[1] = _ij6[1];
7986 vinfos[6].maxsolutions = _nj6;
7987 std::vector<int> vfree(0);
7988 solutions.AddSolution(vinfos,vfree);
7989 }
7990 }
7991 }
7992 
7993 }
7994 } while(0);
7995 if( bgotonextstatement )
7996 {
7997 bool bgotonextstatement = true;
7998 do
7999 {
8000 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
8001 evalcond[1]=new_r12;
8002 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
8003 {
8004 bgotonextstatement=false;
8005 {
8006 IkReal j6array[1], cj6array[1], sj6array[1];
8007 bool j6valid[1]={false};
8008 _nj6 = 1;
8009 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
8010  continue;
8011 j6array[0]=IKatan2(((-1.0)*new_r11), new_r10);
8012 sj6array[0]=IKsin(j6array[0]);
8013 cj6array[0]=IKcos(j6array[0]);
8014 if( j6array[0] > IKPI )
8015 {
8016  j6array[0]-=IK2PI;
8017 }
8018 else if( j6array[0] < -IKPI )
8019 { j6array[0]+=IK2PI;
8020 }
8021 j6valid[0] = true;
8022 for(int ij6 = 0; ij6 < 1; ++ij6)
8023 {
8024 if( !j6valid[ij6] )
8025 {
8026  continue;
8027 }
8028 _ij6[0] = ij6; _ij6[1] = -1;
8029 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8030 {
8031 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8032 {
8033  j6valid[iij6]=false; _ij6[1] = iij6; break;
8034 }
8035 }
8036 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8037 {
8038 IkReal evalcond[8];
8039 IkReal x354=IKcos(j6);
8040 IkReal x355=IKsin(j6);
8041 IkReal x356=((1.0)*cj5);
8042 IkReal x357=((1.0)*x354);
8043 IkReal x358=((1.0)*x355);
8044 evalcond[0]=(x355+new_r11);
8045 evalcond[1]=((((-1.0)*x357))+new_r10);
8046 evalcond[2]=(((cj5*x354))+new_r01);
8047 evalcond[3]=(((cj5*x355))+new_r00);
8048 evalcond[4]=((((-1.0)*new_r02*x358))+new_r20);
8049 evalcond[5]=((((-1.0)*new_r02*x357))+new_r21);
8050 evalcond[6]=(((new_r20*sj5))+(((-1.0)*x358))+(((-1.0)*new_r00*x356)));
8051 evalcond[7]=((((-1.0)*x357))+(((-1.0)*new_r01*x356))+((new_r21*sj5)));
8052 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8053 {
8054 continue;
8055 }
8056 }
8057 
8058 {
8059 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8060 vinfos[0].jointtype = 17;
8061 vinfos[0].foffset = j0;
8062 vinfos[0].indices[0] = _ij0[0];
8063 vinfos[0].indices[1] = _ij0[1];
8064 vinfos[0].maxsolutions = _nj0;
8065 vinfos[1].jointtype = 1;
8066 vinfos[1].foffset = j1;
8067 vinfos[1].indices[0] = _ij1[0];
8068 vinfos[1].indices[1] = _ij1[1];
8069 vinfos[1].maxsolutions = _nj1;
8070 vinfos[2].jointtype = 1;
8071 vinfos[2].foffset = j2;
8072 vinfos[2].indices[0] = _ij2[0];
8073 vinfos[2].indices[1] = _ij2[1];
8074 vinfos[2].maxsolutions = _nj2;
8075 vinfos[3].jointtype = 1;
8076 vinfos[3].foffset = j3;
8077 vinfos[3].indices[0] = _ij3[0];
8078 vinfos[3].indices[1] = _ij3[1];
8079 vinfos[3].maxsolutions = _nj3;
8080 vinfos[4].jointtype = 1;
8081 vinfos[4].foffset = j4;
8082 vinfos[4].indices[0] = _ij4[0];
8083 vinfos[4].indices[1] = _ij4[1];
8084 vinfos[4].maxsolutions = _nj4;
8085 vinfos[5].jointtype = 1;
8086 vinfos[5].foffset = j5;
8087 vinfos[5].indices[0] = _ij5[0];
8088 vinfos[5].indices[1] = _ij5[1];
8089 vinfos[5].maxsolutions = _nj5;
8090 vinfos[6].jointtype = 1;
8091 vinfos[6].foffset = j6;
8092 vinfos[6].indices[0] = _ij6[0];
8093 vinfos[6].indices[1] = _ij6[1];
8094 vinfos[6].maxsolutions = _nj6;
8095 std::vector<int> vfree(0);
8096 solutions.AddSolution(vinfos,vfree);
8097 }
8098 }
8099 }
8100 
8101 }
8102 } while(0);
8103 if( bgotonextstatement )
8104 {
8105 bool bgotonextstatement = true;
8106 do
8107 {
8108 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
8109 evalcond[1]=new_r12;
8110 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
8111 {
8112 bgotonextstatement=false;
8113 {
8114 IkReal j6eval[3];
8115 sj4=-1.0;
8116 cj4=0;
8117 j4=-1.5707963267949;
8118 j6eval[0]=new_r02;
8119 j6eval[1]=IKsign(new_r02);
8120 j6eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
8121 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 || IKabs(j6eval[2]) < 0.0000010000000000 )
8122 {
8123 {
8124 IkReal j6eval[1];
8125 sj4=-1.0;
8126 cj4=0;
8127 j4=-1.5707963267949;
8128 j6eval[0]=new_r02;
8129 if( IKabs(j6eval[0]) < 0.0000010000000000 )
8130 {
8131 {
8132 IkReal j6eval[2];
8133 sj4=-1.0;
8134 cj4=0;
8135 j4=-1.5707963267949;
8136 j6eval[0]=new_r02;
8137 j6eval[1]=cj5;
8138 if( IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 )
8139 {
8140 {
8141 IkReal evalcond[4];
8142 bool bgotonextstatement = true;
8143 do
8144 {
8145 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
8146 evalcond[1]=new_r22;
8147 evalcond[2]=new_r01;
8148 evalcond[3]=new_r00;
8149 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
8150 {
8151 bgotonextstatement=false;
8152 {
8153 IkReal j6array[1], cj6array[1], sj6array[1];
8154 bool j6valid[1]={false};
8155 _nj6 = 1;
8156 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
8157  continue;
8158 j6array[0]=IKatan2(new_r20, new_r21);
8159 sj6array[0]=IKsin(j6array[0]);
8160 cj6array[0]=IKcos(j6array[0]);
8161 if( j6array[0] > IKPI )
8162 {
8163  j6array[0]-=IK2PI;
8164 }
8165 else if( j6array[0] < -IKPI )
8166 { j6array[0]+=IK2PI;
8167 }
8168 j6valid[0] = true;
8169 for(int ij6 = 0; ij6 < 1; ++ij6)
8170 {
8171 if( !j6valid[ij6] )
8172 {
8173  continue;
8174 }
8175 _ij6[0] = ij6; _ij6[1] = -1;
8176 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8177 {
8178 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8179 {
8180  j6valid[iij6]=false; _ij6[1] = iij6; break;
8181 }
8182 }
8183 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8184 {
8185 IkReal evalcond[4];
8186 IkReal x359=IKsin(j6);
8187 IkReal x360=((1.0)*(IKcos(j6)));
8188 evalcond[0]=((((-1.0)*x359))+new_r20);
8189 evalcond[1]=((((-1.0)*x360))+new_r21);
8190 evalcond[2]=(x359+(((-1.0)*new_r11)));
8191 evalcond[3]=((((-1.0)*x360))+(((-1.0)*new_r10)));
8192 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
8193 {
8194 continue;
8195 }
8196 }
8197 
8198 {
8199 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8200 vinfos[0].jointtype = 17;
8201 vinfos[0].foffset = j0;
8202 vinfos[0].indices[0] = _ij0[0];
8203 vinfos[0].indices[1] = _ij0[1];
8204 vinfos[0].maxsolutions = _nj0;
8205 vinfos[1].jointtype = 1;
8206 vinfos[1].foffset = j1;
8207 vinfos[1].indices[0] = _ij1[0];
8208 vinfos[1].indices[1] = _ij1[1];
8209 vinfos[1].maxsolutions = _nj1;
8210 vinfos[2].jointtype = 1;
8211 vinfos[2].foffset = j2;
8212 vinfos[2].indices[0] = _ij2[0];
8213 vinfos[2].indices[1] = _ij2[1];
8214 vinfos[2].maxsolutions = _nj2;
8215 vinfos[3].jointtype = 1;
8216 vinfos[3].foffset = j3;
8217 vinfos[3].indices[0] = _ij3[0];
8218 vinfos[3].indices[1] = _ij3[1];
8219 vinfos[3].maxsolutions = _nj3;
8220 vinfos[4].jointtype = 1;
8221 vinfos[4].foffset = j4;
8222 vinfos[4].indices[0] = _ij4[0];
8223 vinfos[4].indices[1] = _ij4[1];
8224 vinfos[4].maxsolutions = _nj4;
8225 vinfos[5].jointtype = 1;
8226 vinfos[5].foffset = j5;
8227 vinfos[5].indices[0] = _ij5[0];
8228 vinfos[5].indices[1] = _ij5[1];
8229 vinfos[5].maxsolutions = _nj5;
8230 vinfos[6].jointtype = 1;
8231 vinfos[6].foffset = j6;
8232 vinfos[6].indices[0] = _ij6[0];
8233 vinfos[6].indices[1] = _ij6[1];
8234 vinfos[6].maxsolutions = _nj6;
8235 std::vector<int> vfree(0);
8236 solutions.AddSolution(vinfos,vfree);
8237 }
8238 }
8239 }
8240 
8241 }
8242 } while(0);
8243 if( bgotonextstatement )
8244 {
8245 bool bgotonextstatement = true;
8246 do
8247 {
8248 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
8249 evalcond[1]=new_r22;
8250 evalcond[2]=new_r01;
8251 evalcond[3]=new_r00;
8252 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
8253 {
8254 bgotonextstatement=false;
8255 {
8256 IkReal j6array[1], cj6array[1], sj6array[1];
8257 bool j6valid[1]={false};
8258 _nj6 = 1;
8259 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
8260  continue;
8261 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r21));
8262 sj6array[0]=IKsin(j6array[0]);
8263 cj6array[0]=IKcos(j6array[0]);
8264 if( j6array[0] > IKPI )
8265 {
8266  j6array[0]-=IK2PI;
8267 }
8268 else if( j6array[0] < -IKPI )
8269 { j6array[0]+=IK2PI;
8270 }
8271 j6valid[0] = true;
8272 for(int ij6 = 0; ij6 < 1; ++ij6)
8273 {
8274 if( !j6valid[ij6] )
8275 {
8276  continue;
8277 }
8278 _ij6[0] = ij6; _ij6[1] = -1;
8279 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8280 {
8281 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8282 {
8283  j6valid[iij6]=false; _ij6[1] = iij6; break;
8284 }
8285 }
8286 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8287 {
8288 IkReal evalcond[4];
8289 IkReal x361=IKsin(j6);
8290 IkReal x362=IKcos(j6);
8291 evalcond[0]=(x361+new_r20);
8292 evalcond[1]=(x362+new_r21);
8293 evalcond[2]=(x361+(((-1.0)*new_r11)));
8294 evalcond[3]=((((-1.0)*x362))+(((-1.0)*new_r10)));
8295 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
8296 {
8297 continue;
8298 }
8299 }
8300 
8301 {
8302 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8303 vinfos[0].jointtype = 17;
8304 vinfos[0].foffset = j0;
8305 vinfos[0].indices[0] = _ij0[0];
8306 vinfos[0].indices[1] = _ij0[1];
8307 vinfos[0].maxsolutions = _nj0;
8308 vinfos[1].jointtype = 1;
8309 vinfos[1].foffset = j1;
8310 vinfos[1].indices[0] = _ij1[0];
8311 vinfos[1].indices[1] = _ij1[1];
8312 vinfos[1].maxsolutions = _nj1;
8313 vinfos[2].jointtype = 1;
8314 vinfos[2].foffset = j2;
8315 vinfos[2].indices[0] = _ij2[0];
8316 vinfos[2].indices[1] = _ij2[1];
8317 vinfos[2].maxsolutions = _nj2;
8318 vinfos[3].jointtype = 1;
8319 vinfos[3].foffset = j3;
8320 vinfos[3].indices[0] = _ij3[0];
8321 vinfos[3].indices[1] = _ij3[1];
8322 vinfos[3].maxsolutions = _nj3;
8323 vinfos[4].jointtype = 1;
8324 vinfos[4].foffset = j4;
8325 vinfos[4].indices[0] = _ij4[0];
8326 vinfos[4].indices[1] = _ij4[1];
8327 vinfos[4].maxsolutions = _nj4;
8328 vinfos[5].jointtype = 1;
8329 vinfos[5].foffset = j5;
8330 vinfos[5].indices[0] = _ij5[0];
8331 vinfos[5].indices[1] = _ij5[1];
8332 vinfos[5].maxsolutions = _nj5;
8333 vinfos[6].jointtype = 1;
8334 vinfos[6].foffset = j6;
8335 vinfos[6].indices[0] = _ij6[0];
8336 vinfos[6].indices[1] = _ij6[1];
8337 vinfos[6].maxsolutions = _nj6;
8338 std::vector<int> vfree(0);
8339 solutions.AddSolution(vinfos,vfree);
8340 }
8341 }
8342 }
8343 
8344 }
8345 } while(0);
8346 if( bgotonextstatement )
8347 {
8348 bool bgotonextstatement = true;
8349 do
8350 {
8351 evalcond[0]=IKabs(new_r02);
8352 evalcond[1]=new_r20;
8353 evalcond[2]=new_r21;
8354 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
8355 {
8356 bgotonextstatement=false;
8357 {
8358 IkReal j6array[1], cj6array[1], sj6array[1];
8359 bool j6valid[1]={false};
8360 _nj6 = 1;
8361 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
8362  continue;
8363 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
8364 sj6array[0]=IKsin(j6array[0]);
8365 cj6array[0]=IKcos(j6array[0]);
8366 if( j6array[0] > IKPI )
8367 {
8368  j6array[0]-=IK2PI;
8369 }
8370 else if( j6array[0] < -IKPI )
8371 { j6array[0]+=IK2PI;
8372 }
8373 j6valid[0] = true;
8374 for(int ij6 = 0; ij6 < 1; ++ij6)
8375 {
8376 if( !j6valid[ij6] )
8377 {
8378  continue;
8379 }
8380 _ij6[0] = ij6; _ij6[1] = -1;
8381 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8382 {
8383 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8384 {
8385  j6valid[iij6]=false; _ij6[1] = iij6; break;
8386 }
8387 }
8388 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8389 {
8390 IkReal evalcond[6];
8391 IkReal x363=IKsin(j6);
8392 IkReal x364=IKcos(j6);
8393 IkReal x365=((1.0)*x364);
8394 IkReal x366=((1.0)*x363);
8395 evalcond[0]=(x363+(((-1.0)*new_r11)));
8396 evalcond[1]=((((-1.0)*cj5*x365))+new_r01);
8397 evalcond[2]=((((-1.0)*cj5*x366))+new_r00);
8398 evalcond[3]=((((-1.0)*x365))+(((-1.0)*new_r10)));
8399 evalcond[4]=((((-1.0)*x366))+((cj5*new_r00)));
8400 evalcond[5]=((((-1.0)*x365))+((cj5*new_r01)));
8401 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8402 {
8403 continue;
8404 }
8405 }
8406 
8407 {
8408 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8409 vinfos[0].jointtype = 17;
8410 vinfos[0].foffset = j0;
8411 vinfos[0].indices[0] = _ij0[0];
8412 vinfos[0].indices[1] = _ij0[1];
8413 vinfos[0].maxsolutions = _nj0;
8414 vinfos[1].jointtype = 1;
8415 vinfos[1].foffset = j1;
8416 vinfos[1].indices[0] = _ij1[0];
8417 vinfos[1].indices[1] = _ij1[1];
8418 vinfos[1].maxsolutions = _nj1;
8419 vinfos[2].jointtype = 1;
8420 vinfos[2].foffset = j2;
8421 vinfos[2].indices[0] = _ij2[0];
8422 vinfos[2].indices[1] = _ij2[1];
8423 vinfos[2].maxsolutions = _nj2;
8424 vinfos[3].jointtype = 1;
8425 vinfos[3].foffset = j3;
8426 vinfos[3].indices[0] = _ij3[0];
8427 vinfos[3].indices[1] = _ij3[1];
8428 vinfos[3].maxsolutions = _nj3;
8429 vinfos[4].jointtype = 1;
8430 vinfos[4].foffset = j4;
8431 vinfos[4].indices[0] = _ij4[0];
8432 vinfos[4].indices[1] = _ij4[1];
8433 vinfos[4].maxsolutions = _nj4;
8434 vinfos[5].jointtype = 1;
8435 vinfos[5].foffset = j5;
8436 vinfos[5].indices[0] = _ij5[0];
8437 vinfos[5].indices[1] = _ij5[1];
8438 vinfos[5].maxsolutions = _nj5;
8439 vinfos[6].jointtype = 1;
8440 vinfos[6].foffset = j6;
8441 vinfos[6].indices[0] = _ij6[0];
8442 vinfos[6].indices[1] = _ij6[1];
8443 vinfos[6].maxsolutions = _nj6;
8444 std::vector<int> vfree(0);
8445 solutions.AddSolution(vinfos,vfree);
8446 }
8447 }
8448 }
8449 
8450 }
8451 } while(0);
8452 if( bgotonextstatement )
8453 {
8454 bool bgotonextstatement = true;
8455 do
8456 {
8457 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
8458 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8459 {
8460 bgotonextstatement=false;
8461 {
8462 IkReal j6array[1], cj6array[1], sj6array[1];
8463 bool j6valid[1]={false};
8464 _nj6 = 1;
8465 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
8466  continue;
8467 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
8468 sj6array[0]=IKsin(j6array[0]);
8469 cj6array[0]=IKcos(j6array[0]);
8470 if( j6array[0] > IKPI )
8471 {
8472  j6array[0]-=IK2PI;
8473 }
8474 else if( j6array[0] < -IKPI )
8475 { j6array[0]+=IK2PI;
8476 }
8477 j6valid[0] = true;
8478 for(int ij6 = 0; ij6 < 1; ++ij6)
8479 {
8480 if( !j6valid[ij6] )
8481 {
8482  continue;
8483 }
8484 _ij6[0] = ij6; _ij6[1] = -1;
8485 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8486 {
8487 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8488 {
8489  j6valid[iij6]=false; _ij6[1] = iij6; break;
8490 }
8491 }
8492 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8493 {
8494 IkReal evalcond[6];
8495 IkReal x367=IKcos(j6);
8496 IkReal x368=IKsin(j6);
8497 IkReal x369=((-1.0)*x367);
8498 IkReal x370=((-1.0)*x368);
8499 evalcond[0]=x370;
8500 evalcond[1]=x369;
8501 evalcond[2]=(new_r22*x369);
8502 evalcond[3]=(new_r22*x370);
8503 evalcond[4]=(x368+(((-1.0)*new_r11)));
8504 evalcond[5]=((((-1.0)*x367))+(((-1.0)*new_r10)));
8505 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8506 {
8507 continue;
8508 }
8509 }
8510 
8511 {
8512 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8513 vinfos[0].jointtype = 17;
8514 vinfos[0].foffset = j0;
8515 vinfos[0].indices[0] = _ij0[0];
8516 vinfos[0].indices[1] = _ij0[1];
8517 vinfos[0].maxsolutions = _nj0;
8518 vinfos[1].jointtype = 1;
8519 vinfos[1].foffset = j1;
8520 vinfos[1].indices[0] = _ij1[0];
8521 vinfos[1].indices[1] = _ij1[1];
8522 vinfos[1].maxsolutions = _nj1;
8523 vinfos[2].jointtype = 1;
8524 vinfos[2].foffset = j2;
8525 vinfos[2].indices[0] = _ij2[0];
8526 vinfos[2].indices[1] = _ij2[1];
8527 vinfos[2].maxsolutions = _nj2;
8528 vinfos[3].jointtype = 1;
8529 vinfos[3].foffset = j3;
8530 vinfos[3].indices[0] = _ij3[0];
8531 vinfos[3].indices[1] = _ij3[1];
8532 vinfos[3].maxsolutions = _nj3;
8533 vinfos[4].jointtype = 1;
8534 vinfos[4].foffset = j4;
8535 vinfos[4].indices[0] = _ij4[0];
8536 vinfos[4].indices[1] = _ij4[1];
8537 vinfos[4].maxsolutions = _nj4;
8538 vinfos[5].jointtype = 1;
8539 vinfos[5].foffset = j5;
8540 vinfos[5].indices[0] = _ij5[0];
8541 vinfos[5].indices[1] = _ij5[1];
8542 vinfos[5].maxsolutions = _nj5;
8543 vinfos[6].jointtype = 1;
8544 vinfos[6].foffset = j6;
8545 vinfos[6].indices[0] = _ij6[0];
8546 vinfos[6].indices[1] = _ij6[1];
8547 vinfos[6].maxsolutions = _nj6;
8548 std::vector<int> vfree(0);
8549 solutions.AddSolution(vinfos,vfree);
8550 }
8551 }
8552 }
8553 
8554 }
8555 } while(0);
8556 if( bgotonextstatement )
8557 {
8558 bool bgotonextstatement = true;
8559 do
8560 {
8561 if( 1 )
8562 {
8563 bgotonextstatement=false;
8564 continue; // branch miss [j6]
8565 
8566 }
8567 } while(0);
8568 if( bgotonextstatement )
8569 {
8570 }
8571 }
8572 }
8573 }
8574 }
8575 }
8576 
8577 } else
8578 {
8579 {
8580 IkReal j6array[1], cj6array[1], sj6array[1];
8581 bool j6valid[1]={false};
8582 _nj6 = 1;
8583 CheckValue<IkReal> x371=IKPowWithIntegerCheck(new_r02,-1);
8584 if(!x371.valid){
8585 continue;
8586 }
8587 CheckValue<IkReal> x372=IKPowWithIntegerCheck(cj5,-1);
8588 if(!x372.valid){
8589 continue;
8590 }
8591 if( IKabs(((-1.0)*new_r20*(x371.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x372.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x371.value)))+IKsqr((new_r01*(x372.value)))-1) <= IKFAST_SINCOS_THRESH )
8592  continue;
8593 j6array[0]=IKatan2(((-1.0)*new_r20*(x371.value)), (new_r01*(x372.value)));
8594 sj6array[0]=IKsin(j6array[0]);
8595 cj6array[0]=IKcos(j6array[0]);
8596 if( j6array[0] > IKPI )
8597 {
8598  j6array[0]-=IK2PI;
8599 }
8600 else if( j6array[0] < -IKPI )
8601 { j6array[0]+=IK2PI;
8602 }
8603 j6valid[0] = true;
8604 for(int ij6 = 0; ij6 < 1; ++ij6)
8605 {
8606 if( !j6valid[ij6] )
8607 {
8608  continue;
8609 }
8610 _ij6[0] = ij6; _ij6[1] = -1;
8611 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8612 {
8613 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8614 {
8615  j6valid[iij6]=false; _ij6[1] = iij6; break;
8616 }
8617 }
8618 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8619 {
8620 IkReal evalcond[8];
8621 IkReal x373=IKsin(j6);
8622 IkReal x374=IKcos(j6);
8623 IkReal x375=((1.0)*x374);
8624 IkReal x376=((1.0)*x373);
8625 evalcond[0]=(new_r20+((new_r02*x373)));
8626 evalcond[1]=(new_r21+((new_r02*x374)));
8627 evalcond[2]=(x373+(((-1.0)*new_r11)));
8628 evalcond[3]=((((-1.0)*cj5*x375))+new_r01);
8629 evalcond[4]=((((-1.0)*cj5*x376))+new_r00);
8630 evalcond[5]=((((-1.0)*x375))+(((-1.0)*new_r10)));
8631 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x376)));
8632 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x375))+((new_r21*sj5)));
8633 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8634 {
8635 continue;
8636 }
8637 }
8638 
8639 {
8640 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8641 vinfos[0].jointtype = 17;
8642 vinfos[0].foffset = j0;
8643 vinfos[0].indices[0] = _ij0[0];
8644 vinfos[0].indices[1] = _ij0[1];
8645 vinfos[0].maxsolutions = _nj0;
8646 vinfos[1].jointtype = 1;
8647 vinfos[1].foffset = j1;
8648 vinfos[1].indices[0] = _ij1[0];
8649 vinfos[1].indices[1] = _ij1[1];
8650 vinfos[1].maxsolutions = _nj1;
8651 vinfos[2].jointtype = 1;
8652 vinfos[2].foffset = j2;
8653 vinfos[2].indices[0] = _ij2[0];
8654 vinfos[2].indices[1] = _ij2[1];
8655 vinfos[2].maxsolutions = _nj2;
8656 vinfos[3].jointtype = 1;
8657 vinfos[3].foffset = j3;
8658 vinfos[3].indices[0] = _ij3[0];
8659 vinfos[3].indices[1] = _ij3[1];
8660 vinfos[3].maxsolutions = _nj3;
8661 vinfos[4].jointtype = 1;
8662 vinfos[4].foffset = j4;
8663 vinfos[4].indices[0] = _ij4[0];
8664 vinfos[4].indices[1] = _ij4[1];
8665 vinfos[4].maxsolutions = _nj4;
8666 vinfos[5].jointtype = 1;
8667 vinfos[5].foffset = j5;
8668 vinfos[5].indices[0] = _ij5[0];
8669 vinfos[5].indices[1] = _ij5[1];
8670 vinfos[5].maxsolutions = _nj5;
8671 vinfos[6].jointtype = 1;
8672 vinfos[6].foffset = j6;
8673 vinfos[6].indices[0] = _ij6[0];
8674 vinfos[6].indices[1] = _ij6[1];
8675 vinfos[6].maxsolutions = _nj6;
8676 std::vector<int> vfree(0);
8677 solutions.AddSolution(vinfos,vfree);
8678 }
8679 }
8680 }
8681 
8682 }
8683 
8684 }
8685 
8686 } else
8687 {
8688 {
8689 IkReal j6array[1], cj6array[1], sj6array[1];
8690 bool j6valid[1]={false};
8691 _nj6 = 1;
8692 CheckValue<IkReal> x377=IKPowWithIntegerCheck(new_r02,-1);
8693 if(!x377.valid){
8694 continue;
8695 }
8696 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x377.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x377.value)))-1) <= IKFAST_SINCOS_THRESH )
8697  continue;
8698 j6array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x377.value)));
8699 sj6array[0]=IKsin(j6array[0]);
8700 cj6array[0]=IKcos(j6array[0]);
8701 if( j6array[0] > IKPI )
8702 {
8703  j6array[0]-=IK2PI;
8704 }
8705 else if( j6array[0] < -IKPI )
8706 { j6array[0]+=IK2PI;
8707 }
8708 j6valid[0] = true;
8709 for(int ij6 = 0; ij6 < 1; ++ij6)
8710 {
8711 if( !j6valid[ij6] )
8712 {
8713  continue;
8714 }
8715 _ij6[0] = ij6; _ij6[1] = -1;
8716 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8717 {
8718 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8719 {
8720  j6valid[iij6]=false; _ij6[1] = iij6; break;
8721 }
8722 }
8723 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8724 {
8725 IkReal evalcond[8];
8726 IkReal x378=IKsin(j6);
8727 IkReal x379=IKcos(j6);
8728 IkReal x380=((1.0)*x379);
8729 IkReal x381=((1.0)*x378);
8730 evalcond[0]=(new_r20+((new_r02*x378)));
8731 evalcond[1]=(new_r21+((new_r02*x379)));
8732 evalcond[2]=(x378+(((-1.0)*new_r11)));
8733 evalcond[3]=((((-1.0)*cj5*x380))+new_r01);
8734 evalcond[4]=((((-1.0)*cj5*x381))+new_r00);
8735 evalcond[5]=((((-1.0)*x380))+(((-1.0)*new_r10)));
8736 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x381)));
8737 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x380))+((new_r21*sj5)));
8738 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8739 {
8740 continue;
8741 }
8742 }
8743 
8744 {
8745 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8746 vinfos[0].jointtype = 17;
8747 vinfos[0].foffset = j0;
8748 vinfos[0].indices[0] = _ij0[0];
8749 vinfos[0].indices[1] = _ij0[1];
8750 vinfos[0].maxsolutions = _nj0;
8751 vinfos[1].jointtype = 1;
8752 vinfos[1].foffset = j1;
8753 vinfos[1].indices[0] = _ij1[0];
8754 vinfos[1].indices[1] = _ij1[1];
8755 vinfos[1].maxsolutions = _nj1;
8756 vinfos[2].jointtype = 1;
8757 vinfos[2].foffset = j2;
8758 vinfos[2].indices[0] = _ij2[0];
8759 vinfos[2].indices[1] = _ij2[1];
8760 vinfos[2].maxsolutions = _nj2;
8761 vinfos[3].jointtype = 1;
8762 vinfos[3].foffset = j3;
8763 vinfos[3].indices[0] = _ij3[0];
8764 vinfos[3].indices[1] = _ij3[1];
8765 vinfos[3].maxsolutions = _nj3;
8766 vinfos[4].jointtype = 1;
8767 vinfos[4].foffset = j4;
8768 vinfos[4].indices[0] = _ij4[0];
8769 vinfos[4].indices[1] = _ij4[1];
8770 vinfos[4].maxsolutions = _nj4;
8771 vinfos[5].jointtype = 1;
8772 vinfos[5].foffset = j5;
8773 vinfos[5].indices[0] = _ij5[0];
8774 vinfos[5].indices[1] = _ij5[1];
8775 vinfos[5].maxsolutions = _nj5;
8776 vinfos[6].jointtype = 1;
8777 vinfos[6].foffset = j6;
8778 vinfos[6].indices[0] = _ij6[0];
8779 vinfos[6].indices[1] = _ij6[1];
8780 vinfos[6].maxsolutions = _nj6;
8781 std::vector<int> vfree(0);
8782 solutions.AddSolution(vinfos,vfree);
8783 }
8784 }
8785 }
8786 
8787 }
8788 
8789 }
8790 
8791 } else
8792 {
8793 {
8794 IkReal j6array[1], cj6array[1], sj6array[1];
8795 bool j6valid[1]={false};
8796 _nj6 = 1;
8797 CheckValue<IkReal> x382=IKPowWithIntegerCheck(IKsign(new_r02),-1);
8798 if(!x382.valid){
8799 continue;
8800 }
8801 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH);
8802 if(!x383.valid){
8803 continue;
8804 }
8805 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x382.value)))+(x383.value));
8806 sj6array[0]=IKsin(j6array[0]);
8807 cj6array[0]=IKcos(j6array[0]);
8808 if( j6array[0] > IKPI )
8809 {
8810  j6array[0]-=IK2PI;
8811 }
8812 else if( j6array[0] < -IKPI )
8813 { j6array[0]+=IK2PI;
8814 }
8815 j6valid[0] = true;
8816 for(int ij6 = 0; ij6 < 1; ++ij6)
8817 {
8818 if( !j6valid[ij6] )
8819 {
8820  continue;
8821 }
8822 _ij6[0] = ij6; _ij6[1] = -1;
8823 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
8824 {
8825 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8826 {
8827  j6valid[iij6]=false; _ij6[1] = iij6; break;
8828 }
8829 }
8830 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8831 {
8832 IkReal evalcond[8];
8833 IkReal x384=IKsin(j6);
8834 IkReal x385=IKcos(j6);
8835 IkReal x386=((1.0)*x385);
8836 IkReal x387=((1.0)*x384);
8837 evalcond[0]=(((new_r02*x384))+new_r20);
8838 evalcond[1]=(((new_r02*x385))+new_r21);
8839 evalcond[2]=(x384+(((-1.0)*new_r11)));
8840 evalcond[3]=((((-1.0)*cj5*x386))+new_r01);
8841 evalcond[4]=((((-1.0)*cj5*x387))+new_r00);
8842 evalcond[5]=((((-1.0)*x386))+(((-1.0)*new_r10)));
8843 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+(((-1.0)*x387)));
8844 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x386))+((new_r21*sj5)));
8845 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8846 {
8847 continue;
8848 }
8849 }
8850 
8851 {
8852 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8853 vinfos[0].jointtype = 17;
8854 vinfos[0].foffset = j0;
8855 vinfos[0].indices[0] = _ij0[0];
8856 vinfos[0].indices[1] = _ij0[1];
8857 vinfos[0].maxsolutions = _nj0;
8858 vinfos[1].jointtype = 1;
8859 vinfos[1].foffset = j1;
8860 vinfos[1].indices[0] = _ij1[0];
8861 vinfos[1].indices[1] = _ij1[1];
8862 vinfos[1].maxsolutions = _nj1;
8863 vinfos[2].jointtype = 1;
8864 vinfos[2].foffset = j2;
8865 vinfos[2].indices[0] = _ij2[0];
8866 vinfos[2].indices[1] = _ij2[1];
8867 vinfos[2].maxsolutions = _nj2;
8868 vinfos[3].jointtype = 1;
8869 vinfos[3].foffset = j3;
8870 vinfos[3].indices[0] = _ij3[0];
8871 vinfos[3].indices[1] = _ij3[1];
8872 vinfos[3].maxsolutions = _nj3;
8873 vinfos[4].jointtype = 1;
8874 vinfos[4].foffset = j4;
8875 vinfos[4].indices[0] = _ij4[0];
8876 vinfos[4].indices[1] = _ij4[1];
8877 vinfos[4].maxsolutions = _nj4;
8878 vinfos[5].jointtype = 1;
8879 vinfos[5].foffset = j5;
8880 vinfos[5].indices[0] = _ij5[0];
8881 vinfos[5].indices[1] = _ij5[1];
8882 vinfos[5].maxsolutions = _nj5;
8883 vinfos[6].jointtype = 1;
8884 vinfos[6].foffset = j6;
8885 vinfos[6].indices[0] = _ij6[0];
8886 vinfos[6].indices[1] = _ij6[1];
8887 vinfos[6].maxsolutions = _nj6;
8888 std::vector<int> vfree(0);
8889 solutions.AddSolution(vinfos,vfree);
8890 }
8891 }
8892 }
8893 
8894 }
8895 
8896 }
8897 
8898 }
8899 } while(0);
8900 if( bgotonextstatement )
8901 {
8902 bool bgotonextstatement = true;
8903 do
8904 {
8905 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
8906 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8907 {
8908 bgotonextstatement=false;
8909 {
8910 IkReal j6eval[1];
8911 new_r21=0;
8912 new_r20=0;
8913 new_r02=0;
8914 new_r12=0;
8915 j6eval[0]=1.0;
8916 if( IKabs(j6eval[0]) < 0.0000000100000000 )
8917 {
8918 continue; // no branches [j6]
8919 
8920 } else
8921 {
8922 IkReal op[2+1], zeror[2];
8923 int numroots;
8924 op[0]=1.0;
8925 op[1]=0;
8926 op[2]=-1.0;
8927 polyroots2(op,zeror,numroots);
8928 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
8929 int numsolutions = 0;
8930 for(int ij6 = 0; ij6 < numroots; ++ij6)
8931 {
8932 IkReal htj6 = zeror[ij6];
8933 tempj6array[0]=((2.0)*(atan(htj6)));
8934 for(int kj6 = 0; kj6 < 1; ++kj6)
8935 {
8936 j6array[numsolutions] = tempj6array[kj6];
8937 if( j6array[numsolutions] > IKPI )
8938 {
8939  j6array[numsolutions]-=IK2PI;
8940 }
8941 else if( j6array[numsolutions] < -IKPI )
8942 {
8943  j6array[numsolutions]+=IK2PI;
8944 }
8945 sj6array[numsolutions] = IKsin(j6array[numsolutions]);
8946 cj6array[numsolutions] = IKcos(j6array[numsolutions]);
8947 numsolutions++;
8948 }
8949 }
8950 bool j6valid[2]={true,true};
8951 _nj6 = 2;
8952 for(int ij6 = 0; ij6 < numsolutions; ++ij6)
8953  {
8954 if( !j6valid[ij6] )
8955 {
8956  continue;
8957 }
8958  j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8959 htj6 = IKtan(j6/2);
8960 
8961 _ij6[0] = ij6; _ij6[1] = -1;
8962 for(int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
8963 {
8964 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
8965 {
8966  j6valid[iij6]=false; _ij6[1] = iij6; break;
8967 }
8968 }
8969 {
8970 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8971 vinfos[0].jointtype = 17;
8972 vinfos[0].foffset = j0;
8973 vinfos[0].indices[0] = _ij0[0];
8974 vinfos[0].indices[1] = _ij0[1];
8975 vinfos[0].maxsolutions = _nj0;
8976 vinfos[1].jointtype = 1;
8977 vinfos[1].foffset = j1;
8978 vinfos[1].indices[0] = _ij1[0];
8979 vinfos[1].indices[1] = _ij1[1];
8980 vinfos[1].maxsolutions = _nj1;
8981 vinfos[2].jointtype = 1;
8982 vinfos[2].foffset = j2;
8983 vinfos[2].indices[0] = _ij2[0];
8984 vinfos[2].indices[1] = _ij2[1];
8985 vinfos[2].maxsolutions = _nj2;
8986 vinfos[3].jointtype = 1;
8987 vinfos[3].foffset = j3;
8988 vinfos[3].indices[0] = _ij3[0];
8989 vinfos[3].indices[1] = _ij3[1];
8990 vinfos[3].maxsolutions = _nj3;
8991 vinfos[4].jointtype = 1;
8992 vinfos[4].foffset = j4;
8993 vinfos[4].indices[0] = _ij4[0];
8994 vinfos[4].indices[1] = _ij4[1];
8995 vinfos[4].maxsolutions = _nj4;
8996 vinfos[5].jointtype = 1;
8997 vinfos[5].foffset = j5;
8998 vinfos[5].indices[0] = _ij5[0];
8999 vinfos[5].indices[1] = _ij5[1];
9000 vinfos[5].maxsolutions = _nj5;
9001 vinfos[6].jointtype = 1;
9002 vinfos[6].foffset = j6;
9003 vinfos[6].indices[0] = _ij6[0];
9004 vinfos[6].indices[1] = _ij6[1];
9005 vinfos[6].maxsolutions = _nj6;
9006 std::vector<int> vfree(0);
9007 solutions.AddSolution(vinfos,vfree);
9008 }
9009  }
9010 
9011 }
9012 
9013 }
9014 
9015 }
9016 } while(0);
9017 if( bgotonextstatement )
9018 {
9019 bool bgotonextstatement = true;
9020 do
9021 {
9022 if( 1 )
9023 {
9024 bgotonextstatement=false;
9025 continue; // branch miss [j6]
9026 
9027 }
9028 } while(0);
9029 if( bgotonextstatement )
9030 {
9031 }
9032 }
9033 }
9034 }
9035 }
9036 }
9037 }
9038 }
9039 }
9040 }
9041 }
9042 
9043 } else
9044 {
9045 {
9046 IkReal j6array[1], cj6array[1], sj6array[1];
9047 bool j6valid[1]={false};
9048 _nj6 = 1;
9049 CheckValue<IkReal> x389=IKPowWithIntegerCheck(sj5,-1);
9050 if(!x389.valid){
9051 continue;
9052 }
9053 IkReal x388=x389.value;
9054 CheckValue<IkReal> x390=IKPowWithIntegerCheck(cj5,-1);
9055 if(!x390.valid){
9056 continue;
9057 }
9058 CheckValue<IkReal> x391=IKPowWithIntegerCheck(sj4,-1);
9059 if(!x391.valid){
9060 continue;
9061 }
9062 if( IKabs((new_r20*x388)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x388*(x390.value)*(x391.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x388))+IKsqr((x388*(x390.value)*(x391.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20))))))-1) <= IKFAST_SINCOS_THRESH )
9063  continue;
9064 j6array[0]=IKatan2((new_r20*x388), (x388*(x390.value)*(x391.value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj4*new_r20))))));
9065 sj6array[0]=IKsin(j6array[0]);
9066 cj6array[0]=IKcos(j6array[0]);
9067 if( j6array[0] > IKPI )
9068 {
9069  j6array[0]-=IK2PI;
9070 }
9071 else if( j6array[0] < -IKPI )
9072 { j6array[0]+=IK2PI;
9073 }
9074 j6valid[0] = true;
9075 for(int ij6 = 0; ij6 < 1; ++ij6)
9076 {
9077 if( !j6valid[ij6] )
9078 {
9079  continue;
9080 }
9081 _ij6[0] = ij6; _ij6[1] = -1;
9082 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
9083 {
9084 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
9085 {
9086  j6valid[iij6]=false; _ij6[1] = iij6; break;
9087 }
9088 }
9089 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9090 {
9091 IkReal evalcond[12];
9092 IkReal x392=IKsin(j6);
9093 IkReal x393=IKcos(j6);
9094 IkReal x394=((1.0)*sj4);
9095 IkReal x395=(cj4*cj5);
9096 IkReal x396=((1.0)*x393);
9097 IkReal x397=((1.0)*x392);
9098 IkReal x398=(sj4*x392);
9099 evalcond[0]=((((-1.0)*sj5*x397))+new_r20);
9100 evalcond[1]=((((-1.0)*sj5*x396))+new_r21);
9101 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x392);
9102 evalcond[3]=((((-1.0)*x396))+((cj4*new_r00))+((new_r10*sj4)));
9103 evalcond[4]=(((cj5*sj4*x393))+((cj4*x392))+new_r01);
9104 evalcond[5]=((((-1.0)*cj4*x396))+((cj5*x398))+new_r00);
9105 evalcond[6]=((((-1.0)*x395*x396))+x398+new_r11);
9106 evalcond[7]=((((-1.0)*cj5*x397))+((cj4*new_r10))+(((-1.0)*new_r00*x394)));
9107 evalcond[8]=((((-1.0)*new_r01*x394))+(((-1.0)*cj5*x396))+((cj4*new_r11)));
9108 evalcond[9]=((((-1.0)*x395*x397))+new_r10+(((-1.0)*x393*x394)));
9109 evalcond[10]=((((-1.0)*x397))+((new_r20*sj5))+(((-1.0)*cj5*new_r00*x394))+((new_r10*x395)));
9110 evalcond[11]=(((new_r11*x395))+(((-1.0)*x396))+(((-1.0)*cj5*new_r01*x394))+((new_r21*sj5)));
9111 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
9112 {
9113 continue;
9114 }
9115 }
9116 
9117 {
9118 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9119 vinfos[0].jointtype = 17;
9120 vinfos[0].foffset = j0;
9121 vinfos[0].indices[0] = _ij0[0];
9122 vinfos[0].indices[1] = _ij0[1];
9123 vinfos[0].maxsolutions = _nj0;
9124 vinfos[1].jointtype = 1;
9125 vinfos[1].foffset = j1;
9126 vinfos[1].indices[0] = _ij1[0];
9127 vinfos[1].indices[1] = _ij1[1];
9128 vinfos[1].maxsolutions = _nj1;
9129 vinfos[2].jointtype = 1;
9130 vinfos[2].foffset = j2;
9131 vinfos[2].indices[0] = _ij2[0];
9132 vinfos[2].indices[1] = _ij2[1];
9133 vinfos[2].maxsolutions = _nj2;
9134 vinfos[3].jointtype = 1;
9135 vinfos[3].foffset = j3;
9136 vinfos[3].indices[0] = _ij3[0];
9137 vinfos[3].indices[1] = _ij3[1];
9138 vinfos[3].maxsolutions = _nj3;
9139 vinfos[4].jointtype = 1;
9140 vinfos[4].foffset = j4;
9141 vinfos[4].indices[0] = _ij4[0];
9142 vinfos[4].indices[1] = _ij4[1];
9143 vinfos[4].maxsolutions = _nj4;
9144 vinfos[5].jointtype = 1;
9145 vinfos[5].foffset = j5;
9146 vinfos[5].indices[0] = _ij5[0];
9147 vinfos[5].indices[1] = _ij5[1];
9148 vinfos[5].maxsolutions = _nj5;
9149 vinfos[6].jointtype = 1;
9150 vinfos[6].foffset = j6;
9151 vinfos[6].indices[0] = _ij6[0];
9152 vinfos[6].indices[1] = _ij6[1];
9153 vinfos[6].maxsolutions = _nj6;
9154 std::vector<int> vfree(0);
9155 solutions.AddSolution(vinfos,vfree);
9156 }
9157 }
9158 }
9159 
9160 }
9161 
9162 }
9163 
9164 } else
9165 {
9166 {
9167 IkReal j6array[1], cj6array[1], sj6array[1];
9168 bool j6valid[1]={false};
9169 _nj6 = 1;
9170 CheckValue<IkReal> x400=IKPowWithIntegerCheck(sj5,-1);
9171 if(!x400.valid){
9172 continue;
9173 }
9174 IkReal x399=x400.value;
9175 CheckValue<IkReal> x401=IKPowWithIntegerCheck(cj4,-1);
9176 if(!x401.valid){
9177 continue;
9178 }
9179 if( IKabs((new_r20*x399)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x399*(x401.value)*((((new_r00*sj5))+((cj5*new_r20*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x399))+IKsqr((x399*(x401.value)*((((new_r00*sj5))+((cj5*new_r20*sj4))))))-1) <= IKFAST_SINCOS_THRESH )
9180  continue;
9181 j6array[0]=IKatan2((new_r20*x399), (x399*(x401.value)*((((new_r00*sj5))+((cj5*new_r20*sj4))))));
9182 sj6array[0]=IKsin(j6array[0]);
9183 cj6array[0]=IKcos(j6array[0]);
9184 if( j6array[0] > IKPI )
9185 {
9186  j6array[0]-=IK2PI;
9187 }
9188 else if( j6array[0] < -IKPI )
9189 { j6array[0]+=IK2PI;
9190 }
9191 j6valid[0] = true;
9192 for(int ij6 = 0; ij6 < 1; ++ij6)
9193 {
9194 if( !j6valid[ij6] )
9195 {
9196  continue;
9197 }
9198 _ij6[0] = ij6; _ij6[1] = -1;
9199 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
9200 {
9201 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
9202 {
9203  j6valid[iij6]=false; _ij6[1] = iij6; break;
9204 }
9205 }
9206 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9207 {
9208 IkReal evalcond[12];
9209 IkReal x402=IKsin(j6);
9210 IkReal x403=IKcos(j6);
9211 IkReal x404=((1.0)*sj4);
9212 IkReal x405=(cj4*cj5);
9213 IkReal x406=((1.0)*x403);
9214 IkReal x407=((1.0)*x402);
9215 IkReal x408=(sj4*x402);
9216 evalcond[0]=((((-1.0)*sj5*x407))+new_r20);
9217 evalcond[1]=((((-1.0)*sj5*x406))+new_r21);
9218 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x402);
9219 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x406)));
9220 evalcond[4]=(((cj5*sj4*x403))+((cj4*x402))+new_r01);
9221 evalcond[5]=(((cj5*x408))+new_r00+(((-1.0)*cj4*x406)));
9222 evalcond[6]=(x408+new_r11+(((-1.0)*x405*x406)));
9223 evalcond[7]=(((cj4*new_r10))+(((-1.0)*cj5*x407))+(((-1.0)*new_r00*x404)));
9224 evalcond[8]=(((cj4*new_r11))+(((-1.0)*cj5*x406))+(((-1.0)*new_r01*x404)));
9225 evalcond[9]=((((-1.0)*x403*x404))+new_r10+(((-1.0)*x405*x407)));
9226 evalcond[10]=((((-1.0)*cj5*new_r00*x404))+((new_r20*sj5))+((new_r10*x405))+(((-1.0)*x407)));
9227 evalcond[11]=((((-1.0)*cj5*new_r01*x404))+((new_r11*x405))+((new_r21*sj5))+(((-1.0)*x406)));
9228 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
9229 {
9230 continue;
9231 }
9232 }
9233 
9234 {
9235 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9236 vinfos[0].jointtype = 17;
9237 vinfos[0].foffset = j0;
9238 vinfos[0].indices[0] = _ij0[0];
9239 vinfos[0].indices[1] = _ij0[1];
9240 vinfos[0].maxsolutions = _nj0;
9241 vinfos[1].jointtype = 1;
9242 vinfos[1].foffset = j1;
9243 vinfos[1].indices[0] = _ij1[0];
9244 vinfos[1].indices[1] = _ij1[1];
9245 vinfos[1].maxsolutions = _nj1;
9246 vinfos[2].jointtype = 1;
9247 vinfos[2].foffset = j2;
9248 vinfos[2].indices[0] = _ij2[0];
9249 vinfos[2].indices[1] = _ij2[1];
9250 vinfos[2].maxsolutions = _nj2;
9251 vinfos[3].jointtype = 1;
9252 vinfos[3].foffset = j3;
9253 vinfos[3].indices[0] = _ij3[0];
9254 vinfos[3].indices[1] = _ij3[1];
9255 vinfos[3].maxsolutions = _nj3;
9256 vinfos[4].jointtype = 1;
9257 vinfos[4].foffset = j4;
9258 vinfos[4].indices[0] = _ij4[0];
9259 vinfos[4].indices[1] = _ij4[1];
9260 vinfos[4].maxsolutions = _nj4;
9261 vinfos[5].jointtype = 1;
9262 vinfos[5].foffset = j5;
9263 vinfos[5].indices[0] = _ij5[0];
9264 vinfos[5].indices[1] = _ij5[1];
9265 vinfos[5].maxsolutions = _nj5;
9266 vinfos[6].jointtype = 1;
9267 vinfos[6].foffset = j6;
9268 vinfos[6].indices[0] = _ij6[0];
9269 vinfos[6].indices[1] = _ij6[1];
9270 vinfos[6].maxsolutions = _nj6;
9271 std::vector<int> vfree(0);
9272 solutions.AddSolution(vinfos,vfree);
9273 }
9274 }
9275 }
9276 
9277 }
9278 
9279 }
9280 
9281 } else
9282 {
9283 {
9284 IkReal j6array[1], cj6array[1], sj6array[1];
9285 bool j6valid[1]={false};
9286 _nj6 = 1;
9287 CheckValue<IkReal> x409=IKPowWithIntegerCheck(IKsign(sj5),-1);
9288 if(!x409.valid){
9289 continue;
9290 }
9291 CheckValue<IkReal> x410 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
9292 if(!x410.valid){
9293 continue;
9294 }
9295 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x409.value)))+(x410.value));
9296 sj6array[0]=IKsin(j6array[0]);
9297 cj6array[0]=IKcos(j6array[0]);
9298 if( j6array[0] > IKPI )
9299 {
9300  j6array[0]-=IK2PI;
9301 }
9302 else if( j6array[0] < -IKPI )
9303 { j6array[0]+=IK2PI;
9304 }
9305 j6valid[0] = true;
9306 for(int ij6 = 0; ij6 < 1; ++ij6)
9307 {
9308 if( !j6valid[ij6] )
9309 {
9310  continue;
9311 }
9312 _ij6[0] = ij6; _ij6[1] = -1;
9313 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
9314 {
9315 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
9316 {
9317  j6valid[iij6]=false; _ij6[1] = iij6; break;
9318 }
9319 }
9320 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9321 {
9322 IkReal evalcond[12];
9323 IkReal x411=IKsin(j6);
9324 IkReal x412=IKcos(j6);
9325 IkReal x413=((1.0)*sj4);
9326 IkReal x414=(cj4*cj5);
9327 IkReal x415=((1.0)*x412);
9328 IkReal x416=((1.0)*x411);
9329 IkReal x417=(sj4*x411);
9330 evalcond[0]=((((-1.0)*sj5*x416))+new_r20);
9331 evalcond[1]=((((-1.0)*sj5*x415))+new_r21);
9332 evalcond[2]=(((cj4*new_r01))+((new_r11*sj4))+x411);
9333 evalcond[3]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x415)));
9334 evalcond[4]=(((cj5*sj4*x412))+((cj4*x411))+new_r01);
9335 evalcond[5]=(((cj5*x417))+(((-1.0)*cj4*x415))+new_r00);
9336 evalcond[6]=(x417+new_r11+(((-1.0)*x414*x415)));
9337 evalcond[7]=(((cj4*new_r10))+(((-1.0)*cj5*x416))+(((-1.0)*new_r00*x413)));
9338 evalcond[8]=(((cj4*new_r11))+(((-1.0)*cj5*x415))+(((-1.0)*new_r01*x413)));
9339 evalcond[9]=((((-1.0)*x412*x413))+new_r10+(((-1.0)*x414*x416)));
9340 evalcond[10]=(((new_r20*sj5))+((new_r10*x414))+(((-1.0)*x416))+(((-1.0)*cj5*new_r00*x413)));
9341 evalcond[11]=((((-1.0)*cj5*new_r01*x413))+((new_r11*x414))+((new_r21*sj5))+(((-1.0)*x415)));
9342 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
9343 {
9344 continue;
9345 }
9346 }
9347 
9348 {
9349 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9350 vinfos[0].jointtype = 17;
9351 vinfos[0].foffset = j0;
9352 vinfos[0].indices[0] = _ij0[0];
9353 vinfos[0].indices[1] = _ij0[1];
9354 vinfos[0].maxsolutions = _nj0;
9355 vinfos[1].jointtype = 1;
9356 vinfos[1].foffset = j1;
9357 vinfos[1].indices[0] = _ij1[0];
9358 vinfos[1].indices[1] = _ij1[1];
9359 vinfos[1].maxsolutions = _nj1;
9360 vinfos[2].jointtype = 1;
9361 vinfos[2].foffset = j2;
9362 vinfos[2].indices[0] = _ij2[0];
9363 vinfos[2].indices[1] = _ij2[1];
9364 vinfos[2].maxsolutions = _nj2;
9365 vinfos[3].jointtype = 1;
9366 vinfos[3].foffset = j3;
9367 vinfos[3].indices[0] = _ij3[0];
9368 vinfos[3].indices[1] = _ij3[1];
9369 vinfos[3].maxsolutions = _nj3;
9370 vinfos[4].jointtype = 1;
9371 vinfos[4].foffset = j4;
9372 vinfos[4].indices[0] = _ij4[0];
9373 vinfos[4].indices[1] = _ij4[1];
9374 vinfos[4].maxsolutions = _nj4;
9375 vinfos[5].jointtype = 1;
9376 vinfos[5].foffset = j5;
9377 vinfos[5].indices[0] = _ij5[0];
9378 vinfos[5].indices[1] = _ij5[1];
9379 vinfos[5].maxsolutions = _nj5;
9380 vinfos[6].jointtype = 1;
9381 vinfos[6].foffset = j6;
9382 vinfos[6].indices[0] = _ij6[0];
9383 vinfos[6].indices[1] = _ij6[1];
9384 vinfos[6].maxsolutions = _nj6;
9385 std::vector<int> vfree(0);
9386 solutions.AddSolution(vinfos,vfree);
9387 }
9388 }
9389 }
9390 
9391 }
9392 
9393 }
9394 }
9395 }
9396 
9397 }
9398 
9399 }
9400 
9401 } else
9402 {
9403 {
9404 IkReal j6array[1], cj6array[1], sj6array[1];
9405 bool j6valid[1]={false};
9406 _nj6 = 1;
9407 CheckValue<IkReal> x418=IKPowWithIntegerCheck(IKsign(sj5),-1);
9408 if(!x418.valid){
9409 continue;
9410 }
9411 CheckValue<IkReal> x419 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
9412 if(!x419.valid){
9413 continue;
9414 }
9415 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x418.value)))+(x419.value));
9416 sj6array[0]=IKsin(j6array[0]);
9417 cj6array[0]=IKcos(j6array[0]);
9418 if( j6array[0] > IKPI )
9419 {
9420  j6array[0]-=IK2PI;
9421 }
9422 else if( j6array[0] < -IKPI )
9423 { j6array[0]+=IK2PI;
9424 }
9425 j6valid[0] = true;
9426 for(int ij6 = 0; ij6 < 1; ++ij6)
9427 {
9428 if( !j6valid[ij6] )
9429 {
9430  continue;
9431 }
9432 _ij6[0] = ij6; _ij6[1] = -1;
9433 for(int iij6 = ij6+1; iij6 < 1; ++iij6)
9434 {
9435 if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH )
9436 {
9437  j6valid[iij6]=false; _ij6[1] = iij6; break;
9438 }
9439 }
9440 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9441 {
9442 IkReal evalcond[2];
9443 IkReal x420=((1.0)*sj5);
9444 evalcond[0]=(new_r20+(((-1.0)*x420*(IKsin(j6)))));
9445 evalcond[1]=((((-1.0)*x420*(IKcos(j6))))+new_r21);
9446 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
9447 {
9448 continue;
9449 }
9450 }
9451 
9452 {
9453 IkReal j4eval[3];
9454 j4eval[0]=sj5;
9455 j4eval[1]=((IKabs(new_r12))+(IKabs(new_r02)));
9456 j4eval[2]=IKsign(sj5);
9457 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
9458 {
9459 {
9460 IkReal j4eval[2];
9461 j4eval[0]=new_r11;
9462 j4eval[1]=sj5;
9463 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 )
9464 {
9465 {
9466 IkReal evalcond[5];
9467 bool bgotonextstatement = true;
9468 do
9469 {
9470 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
9471 evalcond[1]=new_r12;
9472 evalcond[2]=new_r02;
9473 evalcond[3]=new_r20;
9474 evalcond[4]=new_r21;
9475 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
9476 {
9477 bgotonextstatement=false;
9478 {
9479 IkReal j4array[1], cj4array[1], sj4array[1];
9480 bool j4valid[1]={false};
9481 _nj4 = 1;
9482 IkReal x421=((1.0)*sj6);
9483 if( IKabs(((((-1.0)*cj6*new_r01))+(((-1.0)*new_r00*x421)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj6*new_r00))+(((-1.0)*new_r01*x421)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj6*new_r01))+(((-1.0)*new_r00*x421))))+IKsqr((((cj6*new_r00))+(((-1.0)*new_r01*x421))))-1) <= IKFAST_SINCOS_THRESH )
9484  continue;
9485 j4array[0]=IKatan2(((((-1.0)*cj6*new_r01))+(((-1.0)*new_r00*x421))), (((cj6*new_r00))+(((-1.0)*new_r01*x421))));
9486 sj4array[0]=IKsin(j4array[0]);
9487 cj4array[0]=IKcos(j4array[0]);
9488 if( j4array[0] > IKPI )
9489 {
9490  j4array[0]-=IK2PI;
9491 }
9492 else if( j4array[0] < -IKPI )
9493 { j4array[0]+=IK2PI;
9494 }
9495 j4valid[0] = true;
9496 for(int ij4 = 0; ij4 < 1; ++ij4)
9497 {
9498 if( !j4valid[ij4] )
9499 {
9500  continue;
9501 }
9502 _ij4[0] = ij4; _ij4[1] = -1;
9503 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
9504 {
9505 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
9506 {
9507  j4valid[iij4]=false; _ij4[1] = iij4; break;
9508 }
9509 }
9510 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9511 {
9512 IkReal evalcond[8];
9513 IkReal x422=IKcos(j4);
9514 IkReal x423=IKsin(j4);
9515 IkReal x424=((1.0)*cj6);
9516 IkReal x425=(sj6*x423);
9517 IkReal x426=(cj6*x423);
9518 IkReal x427=(sj6*x422);
9519 IkReal x428=((1.0)*x423);
9520 IkReal x429=(x422*x424);
9521 evalcond[0]=(sj6+((new_r11*x423))+((new_r01*x422)));
9522 evalcond[1]=(x427+x426+new_r01);
9523 evalcond[2]=((((-1.0)*x429))+x425+new_r00);
9524 evalcond[3]=((((-1.0)*x429))+x425+new_r11);
9525 evalcond[4]=((((-1.0)*x424))+((new_r10*x423))+((new_r00*x422)));
9526 evalcond[5]=((((-1.0)*x427))+new_r10+(((-1.0)*x423*x424)));
9527 evalcond[6]=((((-1.0)*sj6))+(((-1.0)*new_r00*x428))+((new_r10*x422)));
9528 evalcond[7]=((((-1.0)*x424))+((new_r11*x422))+(((-1.0)*new_r01*x428)));
9529 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9530 {
9531 continue;
9532 }
9533 }
9534 
9535 {
9536 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9537 vinfos[0].jointtype = 17;
9538 vinfos[0].foffset = j0;
9539 vinfos[0].indices[0] = _ij0[0];
9540 vinfos[0].indices[1] = _ij0[1];
9541 vinfos[0].maxsolutions = _nj0;
9542 vinfos[1].jointtype = 1;
9543 vinfos[1].foffset = j1;
9544 vinfos[1].indices[0] = _ij1[0];
9545 vinfos[1].indices[1] = _ij1[1];
9546 vinfos[1].maxsolutions = _nj1;
9547 vinfos[2].jointtype = 1;
9548 vinfos[2].foffset = j2;
9549 vinfos[2].indices[0] = _ij2[0];
9550 vinfos[2].indices[1] = _ij2[1];
9551 vinfos[2].maxsolutions = _nj2;
9552 vinfos[3].jointtype = 1;
9553 vinfos[3].foffset = j3;
9554 vinfos[3].indices[0] = _ij3[0];
9555 vinfos[3].indices[1] = _ij3[1];
9556 vinfos[3].maxsolutions = _nj3;
9557 vinfos[4].jointtype = 1;
9558 vinfos[4].foffset = j4;
9559 vinfos[4].indices[0] = _ij4[0];
9560 vinfos[4].indices[1] = _ij4[1];
9561 vinfos[4].maxsolutions = _nj4;
9562 vinfos[5].jointtype = 1;
9563 vinfos[5].foffset = j5;
9564 vinfos[5].indices[0] = _ij5[0];
9565 vinfos[5].indices[1] = _ij5[1];
9566 vinfos[5].maxsolutions = _nj5;
9567 vinfos[6].jointtype = 1;
9568 vinfos[6].foffset = j6;
9569 vinfos[6].indices[0] = _ij6[0];
9570 vinfos[6].indices[1] = _ij6[1];
9571 vinfos[6].maxsolutions = _nj6;
9572 std::vector<int> vfree(0);
9573 solutions.AddSolution(vinfos,vfree);
9574 }
9575 }
9576 }
9577 
9578 }
9579 } while(0);
9580 if( bgotonextstatement )
9581 {
9582 bool bgotonextstatement = true;
9583 do
9584 {
9585 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
9586 evalcond[1]=new_r12;
9587 evalcond[2]=new_r02;
9588 evalcond[3]=new_r20;
9589 evalcond[4]=new_r21;
9590 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
9591 {
9592 bgotonextstatement=false;
9593 {
9594 IkReal j4eval[3];
9595 sj5=0;
9596 cj5=-1.0;
9597 j5=3.14159265358979;
9598 IkReal x430=(((new_r11*sj6))+((cj6*new_r01)));
9599 j4eval[0]=x430;
9600 j4eval[1]=((IKabs(((-1.0)+(new_r01*new_r01)+(cj6*cj6))))+(IKabs(((((-1.0)*new_r01*new_r11))+(((-1.0)*cj6*sj6))))));
9601 j4eval[2]=IKsign(x430);
9602 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
9603 {
9604 {
9605 IkReal j4eval[3];
9606 sj5=0;
9607 cj5=-1.0;
9608 j5=3.14159265358979;
9609 IkReal x431=((1.0)*sj6);
9610 IkReal x432=(((new_r10*new_r11))+((new_r00*new_r01)));
9611 j4eval[0]=x432;
9612 j4eval[1]=IKsign(x432);
9613 j4eval[2]=((IKabs((((new_r01*sj6))+(((-1.0)*new_r10*x431)))))+(IKabs(((((-1.0)*new_r00*x431))+(((-1.0)*new_r11*x431))))));
9614 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
9615 {
9616 {
9617 IkReal j4eval[3];
9618 sj5=0;
9619 cj5=-1.0;
9620 j5=3.14159265358979;
9621 IkReal x433=((1.0)*new_r11);
9622 IkReal x434=((new_r01*new_r01)+(new_r11*new_r11));
9623 j4eval[0]=x434;
9624 j4eval[1]=((IKabs((((cj6*new_r01))+(((-1.0)*sj6*x433)))))+(IKabs(((((-1.0)*new_r01*sj6))+(((-1.0)*cj6*x433))))));
9625 j4eval[2]=IKsign(x434);
9626 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
9627 {
9628 {
9629 IkReal evalcond[1];
9630 bool bgotonextstatement = true;
9631 do
9632 {
9633 evalcond[0]=((new_r01*new_r01)+(new_r11*new_r11));
9634 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9635 {
9636 bgotonextstatement=false;
9637 {
9638 IkReal j4eval[1];
9639 sj5=0;
9640 cj5=-1.0;
9641 j5=3.14159265358979;
9642 new_r01=0;
9643 new_r11=0;
9644 j4eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9645 if( IKabs(j4eval[0]) < 0.0000010000000000 )
9646 {
9647 continue; // 3 cases reached
9648 
9649 } else
9650 {
9651 {
9652 IkReal j4array[2], cj4array[2], sj4array[2];
9653 bool j4valid[2]={false};
9654 _nj4 = 2;
9655 CheckValue<IkReal> x436 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9656 if(!x436.valid){
9657 continue;
9658 }
9659 IkReal x435=x436.value;
9660 j4array[0]=((-1.0)*x435);
9661 sj4array[0]=IKsin(j4array[0]);
9662 cj4array[0]=IKcos(j4array[0]);
9663 j4array[1]=((3.14159265358979)+(((-1.0)*x435)));
9664 sj4array[1]=IKsin(j4array[1]);
9665 cj4array[1]=IKcos(j4array[1]);
9666 if( j4array[0] > IKPI )
9667 {
9668  j4array[0]-=IK2PI;
9669 }
9670 else if( j4array[0] < -IKPI )
9671 { j4array[0]+=IK2PI;
9672 }
9673 j4valid[0] = true;
9674 if( j4array[1] > IKPI )
9675 {
9676  j4array[1]-=IK2PI;
9677 }
9678 else if( j4array[1] < -IKPI )
9679 { j4array[1]+=IK2PI;
9680 }
9681 j4valid[1] = true;
9682 for(int ij4 = 0; ij4 < 2; ++ij4)
9683 {
9684 if( !j4valid[ij4] )
9685 {
9686  continue;
9687 }
9688 _ij4[0] = ij4; _ij4[1] = -1;
9689 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
9690 {
9691 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
9692 {
9693  j4valid[iij4]=false; _ij4[1] = iij4; break;
9694 }
9695 }
9696 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9697 {
9698 IkReal evalcond[1];
9699 evalcond[0]=(((new_r10*(IKcos(j4))))+(((-1.0)*new_r00*(IKsin(j4)))));
9700 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9701 {
9702 continue;
9703 }
9704 }
9705 
9706 {
9707 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9708 vinfos[0].jointtype = 17;
9709 vinfos[0].foffset = j0;
9710 vinfos[0].indices[0] = _ij0[0];
9711 vinfos[0].indices[1] = _ij0[1];
9712 vinfos[0].maxsolutions = _nj0;
9713 vinfos[1].jointtype = 1;
9714 vinfos[1].foffset = j1;
9715 vinfos[1].indices[0] = _ij1[0];
9716 vinfos[1].indices[1] = _ij1[1];
9717 vinfos[1].maxsolutions = _nj1;
9718 vinfos[2].jointtype = 1;
9719 vinfos[2].foffset = j2;
9720 vinfos[2].indices[0] = _ij2[0];
9721 vinfos[2].indices[1] = _ij2[1];
9722 vinfos[2].maxsolutions = _nj2;
9723 vinfos[3].jointtype = 1;
9724 vinfos[3].foffset = j3;
9725 vinfos[3].indices[0] = _ij3[0];
9726 vinfos[3].indices[1] = _ij3[1];
9727 vinfos[3].maxsolutions = _nj3;
9728 vinfos[4].jointtype = 1;
9729 vinfos[4].foffset = j4;
9730 vinfos[4].indices[0] = _ij4[0];
9731 vinfos[4].indices[1] = _ij4[1];
9732 vinfos[4].maxsolutions = _nj4;
9733 vinfos[5].jointtype = 1;
9734 vinfos[5].foffset = j5;
9735 vinfos[5].indices[0] = _ij5[0];
9736 vinfos[5].indices[1] = _ij5[1];
9737 vinfos[5].maxsolutions = _nj5;
9738 vinfos[6].jointtype = 1;
9739 vinfos[6].foffset = j6;
9740 vinfos[6].indices[0] = _ij6[0];
9741 vinfos[6].indices[1] = _ij6[1];
9742 vinfos[6].maxsolutions = _nj6;
9743 std::vector<int> vfree(0);
9744 solutions.AddSolution(vinfos,vfree);
9745 }
9746 }
9747 }
9748 
9749 }
9750 
9751 }
9752 
9753 }
9754 } while(0);
9755 if( bgotonextstatement )
9756 {
9757 bool bgotonextstatement = true;
9758 do
9759 {
9760 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j6))), 6.28318530717959)));
9761 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9762 {
9763 bgotonextstatement=false;
9764 {
9765 IkReal j4array[1], cj4array[1], sj4array[1];
9766 bool j4valid[1]={false};
9767 _nj4 = 1;
9768 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
9769  continue;
9770 j4array[0]=IKatan2(new_r01, ((-1.0)*new_r11));
9771 sj4array[0]=IKsin(j4array[0]);
9772 cj4array[0]=IKcos(j4array[0]);
9773 if( j4array[0] > IKPI )
9774 {
9775  j4array[0]-=IK2PI;
9776 }
9777 else if( j4array[0] < -IKPI )
9778 { j4array[0]+=IK2PI;
9779 }
9780 j4valid[0] = true;
9781 for(int ij4 = 0; ij4 < 1; ++ij4)
9782 {
9783 if( !j4valid[ij4] )
9784 {
9785  continue;
9786 }
9787 _ij4[0] = ij4; _ij4[1] = -1;
9788 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
9789 {
9790 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
9791 {
9792  j4valid[iij4]=false; _ij4[1] = iij4; break;
9793 }
9794 }
9795 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9796 {
9797 IkReal evalcond[8];
9798 IkReal x437=IKcos(j4);
9799 IkReal x438=IKsin(j4);
9800 IkReal x439=((1.0)*x438);
9801 evalcond[0]=(x437+new_r11);
9802 evalcond[1]=((((-1.0)*x439))+new_r01);
9803 evalcond[2]=((((-1.0)*x437))+new_r00);
9804 evalcond[3]=((((-1.0)*x439))+new_r10);
9805 evalcond[4]=(((new_r01*x437))+((new_r11*x438)));
9806 evalcond[5]=((-1.0)+((new_r00*x437))+((new_r10*x438)));
9807 evalcond[6]=((((-1.0)*new_r00*x439))+((new_r10*x437)));
9808 evalcond[7]=((1.0)+(((-1.0)*new_r01*x439))+((new_r11*x437)));
9809 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9810 {
9811 continue;
9812 }
9813 }
9814 
9815 {
9816 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9817 vinfos[0].jointtype = 17;
9818 vinfos[0].foffset = j0;
9819 vinfos[0].indices[0] = _ij0[0];
9820 vinfos[0].indices[1] = _ij0[1];
9821 vinfos[0].maxsolutions = _nj0;
9822 vinfos[1].jointtype = 1;
9823 vinfos[1].foffset = j1;
9824 vinfos[1].indices[0] = _ij1[0];
9825 vinfos[1].indices[1] = _ij1[1];
9826 vinfos[1].maxsolutions = _nj1;
9827 vinfos[2].jointtype = 1;
9828 vinfos[2].foffset = j2;
9829 vinfos[2].indices[0] = _ij2[0];
9830 vinfos[2].indices[1] = _ij2[1];
9831 vinfos[2].maxsolutions = _nj2;
9832 vinfos[3].jointtype = 1;
9833 vinfos[3].foffset = j3;
9834 vinfos[3].indices[0] = _ij3[0];
9835 vinfos[3].indices[1] = _ij3[1];
9836 vinfos[3].maxsolutions = _nj3;
9837 vinfos[4].jointtype = 1;
9838 vinfos[4].foffset = j4;
9839 vinfos[4].indices[0] = _ij4[0];
9840 vinfos[4].indices[1] = _ij4[1];
9841 vinfos[4].maxsolutions = _nj4;
9842 vinfos[5].jointtype = 1;
9843 vinfos[5].foffset = j5;
9844 vinfos[5].indices[0] = _ij5[0];
9845 vinfos[5].indices[1] = _ij5[1];
9846 vinfos[5].maxsolutions = _nj5;
9847 vinfos[6].jointtype = 1;
9848 vinfos[6].foffset = j6;
9849 vinfos[6].indices[0] = _ij6[0];
9850 vinfos[6].indices[1] = _ij6[1];
9851 vinfos[6].maxsolutions = _nj6;
9852 std::vector<int> vfree(0);
9853 solutions.AddSolution(vinfos,vfree);
9854 }
9855 }
9856 }
9857 
9858 }
9859 } while(0);
9860 if( bgotonextstatement )
9861 {
9862 bool bgotonextstatement = true;
9863 do
9864 {
9865 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j6)))), 6.28318530717959)));
9866 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9867 {
9868 bgotonextstatement=false;
9869 {
9870 IkReal j4array[1], cj4array[1], sj4array[1];
9871 bool j4valid[1]={false};
9872 _nj4 = 1;
9873 if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
9874  continue;
9875 j4array[0]=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
9876 sj4array[0]=IKsin(j4array[0]);
9877 cj4array[0]=IKcos(j4array[0]);
9878 if( j4array[0] > IKPI )
9879 {
9880  j4array[0]-=IK2PI;
9881 }
9882 else if( j4array[0] < -IKPI )
9883 { j4array[0]+=IK2PI;
9884 }
9885 j4valid[0] = true;
9886 for(int ij4 = 0; ij4 < 1; ++ij4)
9887 {
9888 if( !j4valid[ij4] )
9889 {
9890  continue;
9891 }
9892 _ij4[0] = ij4; _ij4[1] = -1;
9893 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
9894 {
9895 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
9896 {
9897  j4valid[iij4]=false; _ij4[1] = iij4; break;
9898 }
9899 }
9900 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9901 {
9902 IkReal evalcond[8];
9903 IkReal x440=IKsin(j4);
9904 IkReal x441=IKcos(j4);
9905 IkReal x442=((1.0)*x440);
9906 evalcond[0]=(x440+new_r01);
9907 evalcond[1]=(x441+new_r00);
9908 evalcond[2]=(x440+new_r10);
9909 evalcond[3]=((((-1.0)*x441))+new_r11);
9910 evalcond[4]=(((new_r01*x441))+((new_r11*x440)));
9911 evalcond[5]=((1.0)+((new_r00*x441))+((new_r10*x440)));
9912 evalcond[6]=((((-1.0)*new_r00*x442))+((new_r10*x441)));
9913 evalcond[7]=((-1.0)+(((-1.0)*new_r01*x442))+((new_r11*x441)));
9914 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9915 {
9916 continue;
9917 }
9918 }
9919 
9920 {
9921 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9922 vinfos[0].jointtype = 17;
9923 vinfos[0].foffset = j0;
9924 vinfos[0].indices[0] = _ij0[0];
9925 vinfos[0].indices[1] = _ij0[1];
9926 vinfos[0].maxsolutions = _nj0;
9927 vinfos[1].jointtype = 1;
9928 vinfos[1].foffset = j1;
9929 vinfos[1].indices[0] = _ij1[0];
9930 vinfos[1].indices[1] = _ij1[1];
9931 vinfos[1].maxsolutions = _nj1;
9932 vinfos[2].jointtype = 1;
9933 vinfos[2].foffset = j2;
9934 vinfos[2].indices[0] = _ij2[0];
9935 vinfos[2].indices[1] = _ij2[1];
9936 vinfos[2].maxsolutions = _nj2;
9937 vinfos[3].jointtype = 1;
9938 vinfos[3].foffset = j3;
9939 vinfos[3].indices[0] = _ij3[0];
9940 vinfos[3].indices[1] = _ij3[1];
9941 vinfos[3].maxsolutions = _nj3;
9942 vinfos[4].jointtype = 1;
9943 vinfos[4].foffset = j4;
9944 vinfos[4].indices[0] = _ij4[0];
9945 vinfos[4].indices[1] = _ij4[1];
9946 vinfos[4].maxsolutions = _nj4;
9947 vinfos[5].jointtype = 1;
9948 vinfos[5].foffset = j5;
9949 vinfos[5].indices[0] = _ij5[0];
9950 vinfos[5].indices[1] = _ij5[1];
9951 vinfos[5].maxsolutions = _nj5;
9952 vinfos[6].jointtype = 1;
9953 vinfos[6].foffset = j6;
9954 vinfos[6].indices[0] = _ij6[0];
9955 vinfos[6].indices[1] = _ij6[1];
9956 vinfos[6].maxsolutions = _nj6;
9957 std::vector<int> vfree(0);
9958 solutions.AddSolution(vinfos,vfree);
9959 }
9960 }
9961 }
9962 
9963 }
9964 } while(0);
9965 if( bgotonextstatement )
9966 {
9967 bool bgotonextstatement = true;
9968 do
9969 {
9970 IkReal x444 = ((new_r01*new_r01)+(new_r11*new_r11));
9971 if(IKabs(x444)==0){
9972 continue;
9973 }
9974 IkReal x443=pow(x444,-0.5);
9975 CheckValue<IkReal> x445 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9976 if(!x445.valid){
9977 continue;
9978 }
9979 IkReal gconst24=((-1.0)*(x445.value));
9980 IkReal gconst25=((-1.0)*new_r01*x443);
9981 IkReal gconst26=(new_r11*x443);
9982 CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9983 if(!x446.valid){
9984 continue;
9985 }
9986 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x446.value)+j6)))), 6.28318530717959)));
9987 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9988 {
9989 bgotonextstatement=false;
9990 {
9991 IkReal j4eval[3];
9992 CheckValue<IkReal> x449 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9993 if(!x449.valid){
9994 continue;
9995 }
9996 IkReal x447=((-1.0)*(x449.value));
9997 IkReal x448=x443;
9998 sj5=0;
9999 cj5=-1.0;
10000 j5=3.14159265358979;
10001 sj6=gconst25;
10002 cj6=gconst26;
10003 j6=x447;
10004 IkReal gconst24=x447;
10005 IkReal gconst25=((-1.0)*new_r01*x448);
10006 IkReal gconst26=(new_r11*x448);
10007 IkReal x450=new_r01*new_r01;
10008 IkReal x451=(new_r00*new_r01);
10009 IkReal x452=(((new_r10*new_r11))+x451);
10010 IkReal x453=x443;
10011 IkReal x454=(new_r01*x453);
10012 j4eval[0]=x452;
10013 j4eval[1]=IKsign(x452);
10014 j4eval[2]=((IKabs((((new_r11*x454))+((x451*x453)))))+(IKabs((((new_r10*x454))+(((-1.0)*x450*x453))))));
10015 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
10016 {
10017 {
10018 IkReal j4eval[2];
10019 CheckValue<IkReal> x457 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10020 if(!x457.valid){
10021 continue;
10022 }
10023 IkReal x455=((-1.0)*(x457.value));
10024 IkReal x456=x443;
10025 sj5=0;
10026 cj5=-1.0;
10027 j5=3.14159265358979;
10028 sj6=gconst25;
10029 cj6=gconst26;
10030 j6=x455;
10031 IkReal gconst24=x455;
10032 IkReal gconst25=((-1.0)*new_r01*x456);
10033 IkReal gconst26=(new_r11*x456);
10034 IkReal x458=((new_r01*new_r01)+(new_r11*new_r11));
10035 j4eval[0]=x458;
10036 j4eval[1]=IKsign(x458);
10037 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 )
10038 {
10039 {
10040 IkReal j4eval[1];
10041 CheckValue<IkReal> x461 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10042 if(!x461.valid){
10043 continue;
10044 }
10045 IkReal x459=((-1.0)*(x461.value));
10046 IkReal x460=x443;
10047 sj5=0;
10048 cj5=-1.0;
10049 j5=3.14159265358979;
10050 sj6=gconst25;
10051 cj6=gconst26;
10052 j6=x459;
10053 IkReal gconst24=x459;
10054 IkReal gconst25=((-1.0)*new_r01*x460);
10055 IkReal gconst26=(new_r11*x460);
10056 j4eval[0]=((new_r01*new_r01)+(new_r11*new_r11));
10057 if( IKabs(j4eval[0]) < 0.0000010000000000 )
10058 {
10059 {
10060 IkReal evalcond[5];
10061 bool bgotonextstatement = true;
10062 do
10063 {
10064 evalcond[0]=((gconst26*gconst26)+(gconst25*gconst25));
10065 evalcond[1]=new_r01;
10066 evalcond[2]=new_r00;
10067 evalcond[3]=new_r11;
10068 evalcond[4]=new_r10;
10069 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
10070 {
10071 bgotonextstatement=false;
10072 {
10073 IkReal j4array[4], cj4array[4], sj4array[4];
10074 bool j4valid[4]={false};
10075 _nj4 = 4;
10076 j4array[0]=0;
10077 sj4array[0]=IKsin(j4array[0]);
10078 cj4array[0]=IKcos(j4array[0]);
10079 j4array[1]=1.5707963267949;
10080 sj4array[1]=IKsin(j4array[1]);
10081 cj4array[1]=IKcos(j4array[1]);
10082 j4array[2]=3.14159265358979;
10083 sj4array[2]=IKsin(j4array[2]);
10084 cj4array[2]=IKcos(j4array[2]);
10085 j4array[3]=-1.5707963267949;
10086 sj4array[3]=IKsin(j4array[3]);
10087 cj4array[3]=IKcos(j4array[3]);
10088 if( j4array[0] > IKPI )
10089 {
10090  j4array[0]-=IK2PI;
10091 }
10092 else if( j4array[0] < -IKPI )
10093 { j4array[0]+=IK2PI;
10094 }
10095 j4valid[0] = true;
10096 if( j4array[1] > IKPI )
10097 {
10098  j4array[1]-=IK2PI;
10099 }
10100 else if( j4array[1] < -IKPI )
10101 { j4array[1]+=IK2PI;
10102 }
10103 j4valid[1] = true;
10104 if( j4array[2] > IKPI )
10105 {
10106  j4array[2]-=IK2PI;
10107 }
10108 else if( j4array[2] < -IKPI )
10109 { j4array[2]+=IK2PI;
10110 }
10111 j4valid[2] = true;
10112 if( j4array[3] > IKPI )
10113 {
10114  j4array[3]-=IK2PI;
10115 }
10116 else if( j4array[3] < -IKPI )
10117 { j4array[3]+=IK2PI;
10118 }
10119 j4valid[3] = true;
10120 for(int ij4 = 0; ij4 < 4; ++ij4)
10121 {
10122 if( !j4valid[ij4] )
10123 {
10124  continue;
10125 }
10126 _ij4[0] = ij4; _ij4[1] = -1;
10127 for(int iij4 = ij4+1; iij4 < 4; ++iij4)
10128 {
10129 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10130 {
10131  j4valid[iij4]=false; _ij4[1] = iij4; break;
10132 }
10133 }
10134 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10135 
10136 {
10137 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10138 vinfos[0].jointtype = 17;
10139 vinfos[0].foffset = j0;
10140 vinfos[0].indices[0] = _ij0[0];
10141 vinfos[0].indices[1] = _ij0[1];
10142 vinfos[0].maxsolutions = _nj0;
10143 vinfos[1].jointtype = 1;
10144 vinfos[1].foffset = j1;
10145 vinfos[1].indices[0] = _ij1[0];
10146 vinfos[1].indices[1] = _ij1[1];
10147 vinfos[1].maxsolutions = _nj1;
10148 vinfos[2].jointtype = 1;
10149 vinfos[2].foffset = j2;
10150 vinfos[2].indices[0] = _ij2[0];
10151 vinfos[2].indices[1] = _ij2[1];
10152 vinfos[2].maxsolutions = _nj2;
10153 vinfos[3].jointtype = 1;
10154 vinfos[3].foffset = j3;
10155 vinfos[3].indices[0] = _ij3[0];
10156 vinfos[3].indices[1] = _ij3[1];
10157 vinfos[3].maxsolutions = _nj3;
10158 vinfos[4].jointtype = 1;
10159 vinfos[4].foffset = j4;
10160 vinfos[4].indices[0] = _ij4[0];
10161 vinfos[4].indices[1] = _ij4[1];
10162 vinfos[4].maxsolutions = _nj4;
10163 vinfos[5].jointtype = 1;
10164 vinfos[5].foffset = j5;
10165 vinfos[5].indices[0] = _ij5[0];
10166 vinfos[5].indices[1] = _ij5[1];
10167 vinfos[5].maxsolutions = _nj5;
10168 vinfos[6].jointtype = 1;
10169 vinfos[6].foffset = j6;
10170 vinfos[6].indices[0] = _ij6[0];
10171 vinfos[6].indices[1] = _ij6[1];
10172 vinfos[6].maxsolutions = _nj6;
10173 std::vector<int> vfree(0);
10174 solutions.AddSolution(vinfos,vfree);
10175 }
10176 }
10177 }
10178 
10179 }
10180 } while(0);
10181 if( bgotonextstatement )
10182 {
10183 bool bgotonextstatement = true;
10184 do
10185 {
10186 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10187 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10188 {
10189 bgotonextstatement=false;
10190 {
10191 IkReal j4eval[1];
10192 CheckValue<IkReal> x463 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10193 if(!x463.valid){
10194 continue;
10195 }
10196 IkReal x462=((-1.0)*(x463.value));
10197 sj5=0;
10198 cj5=-1.0;
10199 j5=3.14159265358979;
10200 sj6=gconst25;
10201 cj6=gconst26;
10202 j6=x462;
10203 new_r11=0;
10204 new_r00=0;
10205 IkReal gconst24=x462;
10206 IkReal x464 = new_r01*new_r01;
10207 if(IKabs(x464)==0){
10208 continue;
10209 }
10210 IkReal gconst25=((-1.0)*new_r01*(pow(x464,-0.5)));
10211 IkReal gconst26=0;
10212 j4eval[0]=new_r01;
10213 if( IKabs(j4eval[0]) < 0.0000010000000000 )
10214 {
10215 {
10216 IkReal j4array[2], cj4array[2], sj4array[2];
10217 bool j4valid[2]={false};
10218 _nj4 = 2;
10219 CheckValue<IkReal> x465=IKPowWithIntegerCheck(gconst25,-1);
10220 if(!x465.valid){
10221 continue;
10222 }
10223 cj4array[0]=((-1.0)*new_r01*(x465.value));
10224 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
10225 {
10226  j4valid[0] = j4valid[1] = true;
10227  j4array[0] = IKacos(cj4array[0]);
10228  sj4array[0] = IKsin(j4array[0]);
10229  cj4array[1] = cj4array[0];
10230  j4array[1] = -j4array[0];
10231  sj4array[1] = -sj4array[0];
10232 }
10233 else if( isnan(cj4array[0]) )
10234 {
10235  // probably any value will work
10236  j4valid[0] = true;
10237  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
10238 }
10239 for(int ij4 = 0; ij4 < 2; ++ij4)
10240 {
10241 if( !j4valid[ij4] )
10242 {
10243  continue;
10244 }
10245 _ij4[0] = ij4; _ij4[1] = -1;
10246 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
10247 {
10248 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10249 {
10250  j4valid[iij4]=false; _ij4[1] = iij4; break;
10251 }
10252 }
10253 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10254 {
10255 IkReal evalcond[6];
10256 IkReal x466=IKsin(j4);
10257 IkReal x467=IKcos(j4);
10258 IkReal x468=((-1.0)*x466);
10259 evalcond[0]=(new_r10*x466);
10260 evalcond[1]=(gconst25*x468);
10261 evalcond[2]=(new_r01*x468);
10262 evalcond[3]=(((new_r01*x467))+gconst25);
10263 evalcond[4]=(((new_r10*x467))+gconst25);
10264 evalcond[5]=(new_r10+((gconst25*x467)));
10265 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10266 {
10267 continue;
10268 }
10269 }
10270 
10271 {
10272 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10273 vinfos[0].jointtype = 17;
10274 vinfos[0].foffset = j0;
10275 vinfos[0].indices[0] = _ij0[0];
10276 vinfos[0].indices[1] = _ij0[1];
10277 vinfos[0].maxsolutions = _nj0;
10278 vinfos[1].jointtype = 1;
10279 vinfos[1].foffset = j1;
10280 vinfos[1].indices[0] = _ij1[0];
10281 vinfos[1].indices[1] = _ij1[1];
10282 vinfos[1].maxsolutions = _nj1;
10283 vinfos[2].jointtype = 1;
10284 vinfos[2].foffset = j2;
10285 vinfos[2].indices[0] = _ij2[0];
10286 vinfos[2].indices[1] = _ij2[1];
10287 vinfos[2].maxsolutions = _nj2;
10288 vinfos[3].jointtype = 1;
10289 vinfos[3].foffset = j3;
10290 vinfos[3].indices[0] = _ij3[0];
10291 vinfos[3].indices[1] = _ij3[1];
10292 vinfos[3].maxsolutions = _nj3;
10293 vinfos[4].jointtype = 1;
10294 vinfos[4].foffset = j4;
10295 vinfos[4].indices[0] = _ij4[0];
10296 vinfos[4].indices[1] = _ij4[1];
10297 vinfos[4].maxsolutions = _nj4;
10298 vinfos[5].jointtype = 1;
10299 vinfos[5].foffset = j5;
10300 vinfos[5].indices[0] = _ij5[0];
10301 vinfos[5].indices[1] = _ij5[1];
10302 vinfos[5].maxsolutions = _nj5;
10303 vinfos[6].jointtype = 1;
10304 vinfos[6].foffset = j6;
10305 vinfos[6].indices[0] = _ij6[0];
10306 vinfos[6].indices[1] = _ij6[1];
10307 vinfos[6].maxsolutions = _nj6;
10308 std::vector<int> vfree(0);
10309 solutions.AddSolution(vinfos,vfree);
10310 }
10311 }
10312 }
10313 
10314 } else
10315 {
10316 {
10317 IkReal j4array[2], cj4array[2], sj4array[2];
10318 bool j4valid[2]={false};
10319 _nj4 = 2;
10320 CheckValue<IkReal> x469=IKPowWithIntegerCheck(new_r01,-1);
10321 if(!x469.valid){
10322 continue;
10323 }
10324 cj4array[0]=((-1.0)*gconst25*(x469.value));
10325 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
10326 {
10327  j4valid[0] = j4valid[1] = true;
10328  j4array[0] = IKacos(cj4array[0]);
10329  sj4array[0] = IKsin(j4array[0]);
10330  cj4array[1] = cj4array[0];
10331  j4array[1] = -j4array[0];
10332  sj4array[1] = -sj4array[0];
10333 }
10334 else if( isnan(cj4array[0]) )
10335 {
10336  // probably any value will work
10337  j4valid[0] = true;
10338  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
10339 }
10340 for(int ij4 = 0; ij4 < 2; ++ij4)
10341 {
10342 if( !j4valid[ij4] )
10343 {
10344  continue;
10345 }
10346 _ij4[0] = ij4; _ij4[1] = -1;
10347 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
10348 {
10349 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10350 {
10351  j4valid[iij4]=false; _ij4[1] = iij4; break;
10352 }
10353 }
10354 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10355 {
10356 IkReal evalcond[6];
10357 IkReal x470=IKsin(j4);
10358 IkReal x471=IKcos(j4);
10359 IkReal x472=(gconst25*x471);
10360 IkReal x473=((-1.0)*x470);
10361 evalcond[0]=(new_r10*x470);
10362 evalcond[1]=(gconst25*x473);
10363 evalcond[2]=(new_r01*x473);
10364 evalcond[3]=(x472+new_r01);
10365 evalcond[4]=(((new_r10*x471))+gconst25);
10366 evalcond[5]=(x472+new_r10);
10367 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10368 {
10369 continue;
10370 }
10371 }
10372 
10373 {
10374 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10375 vinfos[0].jointtype = 17;
10376 vinfos[0].foffset = j0;
10377 vinfos[0].indices[0] = _ij0[0];
10378 vinfos[0].indices[1] = _ij0[1];
10379 vinfos[0].maxsolutions = _nj0;
10380 vinfos[1].jointtype = 1;
10381 vinfos[1].foffset = j1;
10382 vinfos[1].indices[0] = _ij1[0];
10383 vinfos[1].indices[1] = _ij1[1];
10384 vinfos[1].maxsolutions = _nj1;
10385 vinfos[2].jointtype = 1;
10386 vinfos[2].foffset = j2;
10387 vinfos[2].indices[0] = _ij2[0];
10388 vinfos[2].indices[1] = _ij2[1];
10389 vinfos[2].maxsolutions = _nj2;
10390 vinfos[3].jointtype = 1;
10391 vinfos[3].foffset = j3;
10392 vinfos[3].indices[0] = _ij3[0];
10393 vinfos[3].indices[1] = _ij3[1];
10394 vinfos[3].maxsolutions = _nj3;
10395 vinfos[4].jointtype = 1;
10396 vinfos[4].foffset = j4;
10397 vinfos[4].indices[0] = _ij4[0];
10398 vinfos[4].indices[1] = _ij4[1];
10399 vinfos[4].maxsolutions = _nj4;
10400 vinfos[5].jointtype = 1;
10401 vinfos[5].foffset = j5;
10402 vinfos[5].indices[0] = _ij5[0];
10403 vinfos[5].indices[1] = _ij5[1];
10404 vinfos[5].maxsolutions = _nj5;
10405 vinfos[6].jointtype = 1;
10406 vinfos[6].foffset = j6;
10407 vinfos[6].indices[0] = _ij6[0];
10408 vinfos[6].indices[1] = _ij6[1];
10409 vinfos[6].maxsolutions = _nj6;
10410 std::vector<int> vfree(0);
10411 solutions.AddSolution(vinfos,vfree);
10412 }
10413 }
10414 }
10415 
10416 }
10417 
10418 }
10419 
10420 }
10421 } while(0);
10422 if( bgotonextstatement )
10423 {
10424 bool bgotonextstatement = true;
10425 do
10426 {
10427 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
10428 evalcond[1]=gconst25;
10429 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
10430 {
10431 bgotonextstatement=false;
10432 {
10433 IkReal j4eval[3];
10434 CheckValue<IkReal> x475 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10435 if(!x475.valid){
10436 continue;
10437 }
10438 IkReal x474=((-1.0)*(x475.value));
10439 sj5=0;
10440 cj5=-1.0;
10441 j5=3.14159265358979;
10442 sj6=gconst25;
10443 cj6=gconst26;
10444 j6=x474;
10445 new_r00=0;
10446 new_r10=0;
10447 new_r21=0;
10448 new_r22=0;
10449 IkReal gconst24=x474;
10450 IkReal gconst25=((-1.0)*new_r01);
10451 IkReal gconst26=new_r11;
10452 j4eval[0]=-1.0;
10453 j4eval[1]=-1.0;
10454 j4eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
10455 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
10456 {
10457 {
10458 IkReal j4eval[3];
10459 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10460 if(!x477.valid){
10461 continue;
10462 }
10463 IkReal x476=((-1.0)*(x477.value));
10464 sj5=0;
10465 cj5=-1.0;
10466 j5=3.14159265358979;
10467 sj6=gconst25;
10468 cj6=gconst26;
10469 j6=x476;
10470 new_r00=0;
10471 new_r10=0;
10472 new_r21=0;
10473 new_r22=0;
10474 IkReal gconst24=x476;
10475 IkReal gconst25=((-1.0)*new_r01);
10476 IkReal gconst26=new_r11;
10477 j4eval[0]=-1.0;
10478 j4eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
10479 j4eval[2]=-1.0;
10480 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
10481 {
10482 {
10483 IkReal j4eval[3];
10484 CheckValue<IkReal> x479 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10485 if(!x479.valid){
10486 continue;
10487 }
10488 IkReal x478=((-1.0)*(x479.value));
10489 sj5=0;
10490 cj5=-1.0;
10491 j5=3.14159265358979;
10492 sj6=gconst25;
10493 cj6=gconst26;
10494 j6=x478;
10495 new_r00=0;
10496 new_r10=0;
10497 new_r21=0;
10498 new_r22=0;
10499 IkReal gconst24=x478;
10500 IkReal gconst25=((-1.0)*new_r01);
10501 IkReal gconst26=new_r11;
10502 j4eval[0]=1.0;
10503 j4eval[1]=1.0;
10504 j4eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
10505 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
10506 {
10507 continue; // 3 cases reached
10508 
10509 } else
10510 {
10511 {
10512 IkReal j4array[1], cj4array[1], sj4array[1];
10513 bool j4valid[1]={false};
10514 _nj4 = 1;
10515 IkReal x480=((1.0)*gconst25);
10516 CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal((((gconst26*new_r01))+(((-1.0)*new_r11*x480)))),IkReal(((((-1.0)*new_r01*x480))+(((-1.0)*gconst26*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10517 if(!x481.valid){
10518 continue;
10519 }
10520 CheckValue<IkReal> x482=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
10521 if(!x482.valid){
10522 continue;
10523 }
10524 j4array[0]=((-1.5707963267949)+(x481.value)+(((1.5707963267949)*(x482.value))));
10525 sj4array[0]=IKsin(j4array[0]);
10526 cj4array[0]=IKcos(j4array[0]);
10527 if( j4array[0] > IKPI )
10528 {
10529  j4array[0]-=IK2PI;
10530 }
10531 else if( j4array[0] < -IKPI )
10532 { j4array[0]+=IK2PI;
10533 }
10534 j4valid[0] = true;
10535 for(int ij4 = 0; ij4 < 1; ++ij4)
10536 {
10537 if( !j4valid[ij4] )
10538 {
10539  continue;
10540 }
10541 _ij4[0] = ij4; _ij4[1] = -1;
10542 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10543 {
10544 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10545 {
10546  j4valid[iij4]=false; _ij4[1] = iij4; break;
10547 }
10548 }
10549 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10550 {
10551 IkReal evalcond[6];
10552 IkReal x483=IKcos(j4);
10553 IkReal x484=IKsin(j4);
10554 IkReal x485=(gconst25*x483);
10555 IkReal x486=((1.0)*x484);
10556 IkReal x487=(gconst26*x483);
10557 IkReal x488=(gconst26*x486);
10558 evalcond[0]=(((new_r11*x484))+gconst25+((new_r01*x483)));
10559 evalcond[1]=(((gconst25*x484))+x487+new_r11);
10560 evalcond[2]=((((-1.0)*x488))+x485);
10561 evalcond[3]=((((-1.0)*x488))+x485+new_r01);
10562 evalcond[4]=((((-1.0)*x487))+(((-1.0)*gconst25*x486)));
10563 evalcond[5]=((((-1.0)*new_r01*x486))+((new_r11*x483))+gconst26);
10564 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10565 {
10566 continue;
10567 }
10568 }
10569 
10570 {
10571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10572 vinfos[0].jointtype = 17;
10573 vinfos[0].foffset = j0;
10574 vinfos[0].indices[0] = _ij0[0];
10575 vinfos[0].indices[1] = _ij0[1];
10576 vinfos[0].maxsolutions = _nj0;
10577 vinfos[1].jointtype = 1;
10578 vinfos[1].foffset = j1;
10579 vinfos[1].indices[0] = _ij1[0];
10580 vinfos[1].indices[1] = _ij1[1];
10581 vinfos[1].maxsolutions = _nj1;
10582 vinfos[2].jointtype = 1;
10583 vinfos[2].foffset = j2;
10584 vinfos[2].indices[0] = _ij2[0];
10585 vinfos[2].indices[1] = _ij2[1];
10586 vinfos[2].maxsolutions = _nj2;
10587 vinfos[3].jointtype = 1;
10588 vinfos[3].foffset = j3;
10589 vinfos[3].indices[0] = _ij3[0];
10590 vinfos[3].indices[1] = _ij3[1];
10591 vinfos[3].maxsolutions = _nj3;
10592 vinfos[4].jointtype = 1;
10593 vinfos[4].foffset = j4;
10594 vinfos[4].indices[0] = _ij4[0];
10595 vinfos[4].indices[1] = _ij4[1];
10596 vinfos[4].maxsolutions = _nj4;
10597 vinfos[5].jointtype = 1;
10598 vinfos[5].foffset = j5;
10599 vinfos[5].indices[0] = _ij5[0];
10600 vinfos[5].indices[1] = _ij5[1];
10601 vinfos[5].maxsolutions = _nj5;
10602 vinfos[6].jointtype = 1;
10603 vinfos[6].foffset = j6;
10604 vinfos[6].indices[0] = _ij6[0];
10605 vinfos[6].indices[1] = _ij6[1];
10606 vinfos[6].maxsolutions = _nj6;
10607 std::vector<int> vfree(0);
10608 solutions.AddSolution(vinfos,vfree);
10609 }
10610 }
10611 }
10612 
10613 }
10614 
10615 }
10616 
10617 } else
10618 {
10619 {
10620 IkReal j4array[1], cj4array[1], sj4array[1];
10621 bool j4valid[1]={false};
10622 _nj4 = 1;
10623 CheckValue<IkReal> x489 = IKatan2WithCheck(IkReal((gconst25*new_r11)),IkReal((gconst26*new_r11)),IKFAST_ATAN2_MAGTHRESH);
10624 if(!x489.valid){
10625 continue;
10626 }
10627 CheckValue<IkReal> x490=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst25*gconst25)))+(((-1.0)*(gconst26*gconst26))))),-1);
10628 if(!x490.valid){
10629 continue;
10630 }
10631 j4array[0]=((-1.5707963267949)+(x489.value)+(((1.5707963267949)*(x490.value))));
10632 sj4array[0]=IKsin(j4array[0]);
10633 cj4array[0]=IKcos(j4array[0]);
10634 if( j4array[0] > IKPI )
10635 {
10636  j4array[0]-=IK2PI;
10637 }
10638 else if( j4array[0] < -IKPI )
10639 { j4array[0]+=IK2PI;
10640 }
10641 j4valid[0] = true;
10642 for(int ij4 = 0; ij4 < 1; ++ij4)
10643 {
10644 if( !j4valid[ij4] )
10645 {
10646  continue;
10647 }
10648 _ij4[0] = ij4; _ij4[1] = -1;
10649 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10650 {
10651 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10652 {
10653  j4valid[iij4]=false; _ij4[1] = iij4; break;
10654 }
10655 }
10656 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10657 {
10658 IkReal evalcond[6];
10659 IkReal x491=IKcos(j4);
10660 IkReal x492=IKsin(j4);
10661 IkReal x493=(gconst25*x491);
10662 IkReal x494=((1.0)*x492);
10663 IkReal x495=(gconst26*x491);
10664 IkReal x496=(gconst26*x494);
10665 evalcond[0]=(gconst25+((new_r11*x492))+((new_r01*x491)));
10666 evalcond[1]=(((gconst25*x492))+x495+new_r11);
10667 evalcond[2]=((((-1.0)*x496))+x493);
10668 evalcond[3]=((((-1.0)*x496))+x493+new_r01);
10669 evalcond[4]=((((-1.0)*x495))+(((-1.0)*gconst25*x494)));
10670 evalcond[5]=((((-1.0)*new_r01*x494))+gconst26+((new_r11*x491)));
10671 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10672 {
10673 continue;
10674 }
10675 }
10676 
10677 {
10678 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10679 vinfos[0].jointtype = 17;
10680 vinfos[0].foffset = j0;
10681 vinfos[0].indices[0] = _ij0[0];
10682 vinfos[0].indices[1] = _ij0[1];
10683 vinfos[0].maxsolutions = _nj0;
10684 vinfos[1].jointtype = 1;
10685 vinfos[1].foffset = j1;
10686 vinfos[1].indices[0] = _ij1[0];
10687 vinfos[1].indices[1] = _ij1[1];
10688 vinfos[1].maxsolutions = _nj1;
10689 vinfos[2].jointtype = 1;
10690 vinfos[2].foffset = j2;
10691 vinfos[2].indices[0] = _ij2[0];
10692 vinfos[2].indices[1] = _ij2[1];
10693 vinfos[2].maxsolutions = _nj2;
10694 vinfos[3].jointtype = 1;
10695 vinfos[3].foffset = j3;
10696 vinfos[3].indices[0] = _ij3[0];
10697 vinfos[3].indices[1] = _ij3[1];
10698 vinfos[3].maxsolutions = _nj3;
10699 vinfos[4].jointtype = 1;
10700 vinfos[4].foffset = j4;
10701 vinfos[4].indices[0] = _ij4[0];
10702 vinfos[4].indices[1] = _ij4[1];
10703 vinfos[4].maxsolutions = _nj4;
10704 vinfos[5].jointtype = 1;
10705 vinfos[5].foffset = j5;
10706 vinfos[5].indices[0] = _ij5[0];
10707 vinfos[5].indices[1] = _ij5[1];
10708 vinfos[5].maxsolutions = _nj5;
10709 vinfos[6].jointtype = 1;
10710 vinfos[6].foffset = j6;
10711 vinfos[6].indices[0] = _ij6[0];
10712 vinfos[6].indices[1] = _ij6[1];
10713 vinfos[6].maxsolutions = _nj6;
10714 std::vector<int> vfree(0);
10715 solutions.AddSolution(vinfos,vfree);
10716 }
10717 }
10718 }
10719 
10720 }
10721 
10722 }
10723 
10724 } else
10725 {
10726 {
10727 IkReal j4array[1], cj4array[1], sj4array[1];
10728 bool j4valid[1]={false};
10729 _nj4 = 1;
10730 CheckValue<IkReal> x497=IKPowWithIntegerCheck(IKsign((((gconst25*new_r01))+(((-1.0)*gconst26*new_r11)))),-1);
10731 if(!x497.valid){
10732 continue;
10733 }
10734 CheckValue<IkReal> x498 = IKatan2WithCheck(IkReal((gconst25*gconst26)),IkReal(((-1.0)*(gconst25*gconst25))),IKFAST_ATAN2_MAGTHRESH);
10735 if(!x498.valid){
10736 continue;
10737 }
10738 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x497.value)))+(x498.value));
10739 sj4array[0]=IKsin(j4array[0]);
10740 cj4array[0]=IKcos(j4array[0]);
10741 if( j4array[0] > IKPI )
10742 {
10743  j4array[0]-=IK2PI;
10744 }
10745 else if( j4array[0] < -IKPI )
10746 { j4array[0]+=IK2PI;
10747 }
10748 j4valid[0] = true;
10749 for(int ij4 = 0; ij4 < 1; ++ij4)
10750 {
10751 if( !j4valid[ij4] )
10752 {
10753  continue;
10754 }
10755 _ij4[0] = ij4; _ij4[1] = -1;
10756 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
10757 {
10758 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10759 {
10760  j4valid[iij4]=false; _ij4[1] = iij4; break;
10761 }
10762 }
10763 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10764 {
10765 IkReal evalcond[6];
10766 IkReal x499=IKcos(j4);
10767 IkReal x500=IKsin(j4);
10768 IkReal x501=(gconst25*x499);
10769 IkReal x502=((1.0)*x500);
10770 IkReal x503=(gconst26*x499);
10771 IkReal x504=(gconst26*x502);
10772 evalcond[0]=(gconst25+((new_r11*x500))+((new_r01*x499)));
10773 evalcond[1]=(((gconst25*x500))+x503+new_r11);
10774 evalcond[2]=((((-1.0)*x504))+x501);
10775 evalcond[3]=((((-1.0)*x504))+x501+new_r01);
10776 evalcond[4]=((((-1.0)*gconst25*x502))+(((-1.0)*x503)));
10777 evalcond[5]=((((-1.0)*new_r01*x502))+gconst26+((new_r11*x499)));
10778 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10779 {
10780 continue;
10781 }
10782 }
10783 
10784 {
10785 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10786 vinfos[0].jointtype = 17;
10787 vinfos[0].foffset = j0;
10788 vinfos[0].indices[0] = _ij0[0];
10789 vinfos[0].indices[1] = _ij0[1];
10790 vinfos[0].maxsolutions = _nj0;
10791 vinfos[1].jointtype = 1;
10792 vinfos[1].foffset = j1;
10793 vinfos[1].indices[0] = _ij1[0];
10794 vinfos[1].indices[1] = _ij1[1];
10795 vinfos[1].maxsolutions = _nj1;
10796 vinfos[2].jointtype = 1;
10797 vinfos[2].foffset = j2;
10798 vinfos[2].indices[0] = _ij2[0];
10799 vinfos[2].indices[1] = _ij2[1];
10800 vinfos[2].maxsolutions = _nj2;
10801 vinfos[3].jointtype = 1;
10802 vinfos[3].foffset = j3;
10803 vinfos[3].indices[0] = _ij3[0];
10804 vinfos[3].indices[1] = _ij3[1];
10805 vinfos[3].maxsolutions = _nj3;
10806 vinfos[4].jointtype = 1;
10807 vinfos[4].foffset = j4;
10808 vinfos[4].indices[0] = _ij4[0];
10809 vinfos[4].indices[1] = _ij4[1];
10810 vinfos[4].maxsolutions = _nj4;
10811 vinfos[5].jointtype = 1;
10812 vinfos[5].foffset = j5;
10813 vinfos[5].indices[0] = _ij5[0];
10814 vinfos[5].indices[1] = _ij5[1];
10815 vinfos[5].maxsolutions = _nj5;
10816 vinfos[6].jointtype = 1;
10817 vinfos[6].foffset = j6;
10818 vinfos[6].indices[0] = _ij6[0];
10819 vinfos[6].indices[1] = _ij6[1];
10820 vinfos[6].maxsolutions = _nj6;
10821 std::vector<int> vfree(0);
10822 solutions.AddSolution(vinfos,vfree);
10823 }
10824 }
10825 }
10826 
10827 }
10828 
10829 }
10830 
10831 }
10832 } while(0);
10833 if( bgotonextstatement )
10834 {
10835 bool bgotonextstatement = true;
10836 do
10837 {
10838 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
10839 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10840 {
10841 bgotonextstatement=false;
10842 {
10843 IkReal j4array[2], cj4array[2], sj4array[2];
10844 bool j4valid[2]={false};
10845 _nj4 = 2;
10846 CheckValue<IkReal> x505=IKPowWithIntegerCheck(gconst26,-1);
10847 if(!x505.valid){
10848 continue;
10849 }
10850 cj4array[0]=(new_r00*(x505.value));
10851 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
10852 {
10853  j4valid[0] = j4valid[1] = true;
10854  j4array[0] = IKacos(cj4array[0]);
10855  sj4array[0] = IKsin(j4array[0]);
10856  cj4array[1] = cj4array[0];
10857  j4array[1] = -j4array[0];
10858  sj4array[1] = -sj4array[0];
10859 }
10860 else if( isnan(cj4array[0]) )
10861 {
10862  // probably any value will work
10863  j4valid[0] = true;
10864  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
10865 }
10866 for(int ij4 = 0; ij4 < 2; ++ij4)
10867 {
10868 if( !j4valid[ij4] )
10869 {
10870  continue;
10871 }
10872 _ij4[0] = ij4; _ij4[1] = -1;
10873 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
10874 {
10875 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
10876 {
10877  j4valid[iij4]=false; _ij4[1] = iij4; break;
10878 }
10879 }
10880 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
10881 {
10882 IkReal evalcond[6];
10883 IkReal x506=IKsin(j4);
10884 IkReal x507=IKcos(j4);
10885 IkReal x508=((-1.0)*x506);
10886 evalcond[0]=(new_r11*x506);
10887 evalcond[1]=(gconst26*x508);
10888 evalcond[2]=(new_r00*x508);
10889 evalcond[3]=(((gconst26*x507))+new_r11);
10890 evalcond[4]=(gconst26+((new_r11*x507)));
10891 evalcond[5]=(((new_r00*x507))+(((-1.0)*gconst26)));
10892 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10893 {
10894 continue;
10895 }
10896 }
10897 
10898 {
10899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10900 vinfos[0].jointtype = 17;
10901 vinfos[0].foffset = j0;
10902 vinfos[0].indices[0] = _ij0[0];
10903 vinfos[0].indices[1] = _ij0[1];
10904 vinfos[0].maxsolutions = _nj0;
10905 vinfos[1].jointtype = 1;
10906 vinfos[1].foffset = j1;
10907 vinfos[1].indices[0] = _ij1[0];
10908 vinfos[1].indices[1] = _ij1[1];
10909 vinfos[1].maxsolutions = _nj1;
10910 vinfos[2].jointtype = 1;
10911 vinfos[2].foffset = j2;
10912 vinfos[2].indices[0] = _ij2[0];
10913 vinfos[2].indices[1] = _ij2[1];
10914 vinfos[2].maxsolutions = _nj2;
10915 vinfos[3].jointtype = 1;
10916 vinfos[3].foffset = j3;
10917 vinfos[3].indices[0] = _ij3[0];
10918 vinfos[3].indices[1] = _ij3[1];
10919 vinfos[3].maxsolutions = _nj3;
10920 vinfos[4].jointtype = 1;
10921 vinfos[4].foffset = j4;
10922 vinfos[4].indices[0] = _ij4[0];
10923 vinfos[4].indices[1] = _ij4[1];
10924 vinfos[4].maxsolutions = _nj4;
10925 vinfos[5].jointtype = 1;
10926 vinfos[5].foffset = j5;
10927 vinfos[5].indices[0] = _ij5[0];
10928 vinfos[5].indices[1] = _ij5[1];
10929 vinfos[5].maxsolutions = _nj5;
10930 vinfos[6].jointtype = 1;
10931 vinfos[6].foffset = j6;
10932 vinfos[6].indices[0] = _ij6[0];
10933 vinfos[6].indices[1] = _ij6[1];
10934 vinfos[6].maxsolutions = _nj6;
10935 std::vector<int> vfree(0);
10936 solutions.AddSolution(vinfos,vfree);
10937 }
10938 }
10939 }
10940 
10941 }
10942 } while(0);
10943 if( bgotonextstatement )
10944 {
10945 bool bgotonextstatement = true;
10946 do
10947 {
10948 evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01)));
10949 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10950 {
10951 bgotonextstatement=false;
10952 {
10953 IkReal j4eval[1];
10954 CheckValue<IkReal> x510 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10955 if(!x510.valid){
10956 continue;
10957 }
10958 IkReal x509=((-1.0)*(x510.value));
10959 sj5=0;
10960 cj5=-1.0;
10961 j5=3.14159265358979;
10962 sj6=gconst25;
10963 cj6=gconst26;
10964 j6=x509;
10965 new_r00=0;
10966 new_r01=0;
10967 new_r12=0;
10968 new_r22=0;
10969 IkReal gconst24=x509;
10970 IkReal gconst25=0;
10971 IkReal x511 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
10972 if(IKabs(x511)==0){
10973 continue;
10974 }
10975 IkReal gconst26=(new_r11*(pow(x511,-0.5)));
10976 j4eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
10977 if( IKabs(j4eval[0]) < 0.0000010000000000 )
10978 {
10979 {
10980 IkReal j4eval[1];
10981 CheckValue<IkReal> x513 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10982 if(!x513.valid){
10983 continue;
10984 }
10985 IkReal x512=((-1.0)*(x513.value));
10986 sj5=0;
10987 cj5=-1.0;
10988 j5=3.14159265358979;
10989 sj6=gconst25;
10990 cj6=gconst26;
10991 j6=x512;
10992 new_r00=0;
10993 new_r01=0;
10994 new_r12=0;
10995 new_r22=0;
10996 IkReal gconst24=x512;
10997 IkReal gconst25=0;
10998 IkReal x514 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
10999 if(IKabs(x514)==0){
11000 continue;
11001 }
11002 IkReal gconst26=(new_r11*(pow(x514,-0.5)));
11003 j4eval[0]=new_r11;
11004 if( IKabs(j4eval[0]) < 0.0000010000000000 )
11005 {
11006 {
11007 IkReal j4eval[1];
11008 CheckValue<IkReal> x516 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
11009 if(!x516.valid){
11010 continue;
11011 }
11012 IkReal x515=((-1.0)*(x516.value));
11013 sj5=0;
11014 cj5=-1.0;
11015 j5=3.14159265358979;
11016 sj6=gconst25;
11017 cj6=gconst26;
11018 j6=x515;
11019 new_r00=0;
11020 new_r01=0;
11021 new_r12=0;
11022 new_r22=0;
11023 IkReal gconst24=x515;
11024 IkReal gconst25=0;
11025 IkReal x517 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
11026 if(IKabs(x517)==0){
11027 continue;
11028 }
11029 IkReal gconst26=(new_r11*(pow(x517,-0.5)));
11030 j4eval[0]=new_r10;
11031 if( IKabs(j4eval[0]) < 0.0000010000000000 )
11032 {
11033 continue; // 3 cases reached
11034 
11035 } else
11036 {
11037 {
11038 IkReal j4array[1], cj4array[1], sj4array[1];
11039 bool j4valid[1]={false};
11040 _nj4 = 1;
11041 CheckValue<IkReal> x518=IKPowWithIntegerCheck(new_r10,-1);
11042 if(!x518.valid){
11043 continue;
11044 }
11045 CheckValue<IkReal> x519=IKPowWithIntegerCheck(gconst26,-1);
11046 if(!x519.valid){
11047 continue;
11048 }
11049 if( IKabs((gconst26*(x518.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x519.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst26*(x518.value)))+IKsqr(((-1.0)*new_r11*(x519.value)))-1) <= IKFAST_SINCOS_THRESH )
11050  continue;
11051 j4array[0]=IKatan2((gconst26*(x518.value)), ((-1.0)*new_r11*(x519.value)));
11052 sj4array[0]=IKsin(j4array[0]);
11053 cj4array[0]=IKcos(j4array[0]);
11054 if( j4array[0] > IKPI )
11055 {
11056  j4array[0]-=IK2PI;
11057 }
11058 else if( j4array[0] < -IKPI )
11059 { j4array[0]+=IK2PI;
11060 }
11061 j4valid[0] = true;
11062 for(int ij4 = 0; ij4 < 1; ++ij4)
11063 {
11064 if( !j4valid[ij4] )
11065 {
11066  continue;
11067 }
11068 _ij4[0] = ij4; _ij4[1] = -1;
11069 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11070 {
11071 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11072 {
11073  j4valid[iij4]=false; _ij4[1] = iij4; break;
11074 }
11075 }
11076 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11077 {
11078 IkReal evalcond[8];
11079 IkReal x520=IKsin(j4);
11080 IkReal x521=IKcos(j4);
11081 IkReal x522=((1.0)*gconst26);
11082 IkReal x523=((-1.0)*gconst26);
11083 evalcond[0]=(new_r11*x520);
11084 evalcond[1]=(new_r10*x521);
11085 evalcond[2]=(x520*x523);
11086 evalcond[3]=(x521*x523);
11087 evalcond[4]=(((gconst26*x521))+new_r11);
11088 evalcond[5]=(gconst26+((new_r11*x521)));
11089 evalcond[6]=((((-1.0)*x520*x522))+new_r10);
11090 evalcond[7]=((((-1.0)*x522))+((new_r10*x520)));
11091 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11092 {
11093 continue;
11094 }
11095 }
11096 
11097 {
11098 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11099 vinfos[0].jointtype = 17;
11100 vinfos[0].foffset = j0;
11101 vinfos[0].indices[0] = _ij0[0];
11102 vinfos[0].indices[1] = _ij0[1];
11103 vinfos[0].maxsolutions = _nj0;
11104 vinfos[1].jointtype = 1;
11105 vinfos[1].foffset = j1;
11106 vinfos[1].indices[0] = _ij1[0];
11107 vinfos[1].indices[1] = _ij1[1];
11108 vinfos[1].maxsolutions = _nj1;
11109 vinfos[2].jointtype = 1;
11110 vinfos[2].foffset = j2;
11111 vinfos[2].indices[0] = _ij2[0];
11112 vinfos[2].indices[1] = _ij2[1];
11113 vinfos[2].maxsolutions = _nj2;
11114 vinfos[3].jointtype = 1;
11115 vinfos[3].foffset = j3;
11116 vinfos[3].indices[0] = _ij3[0];
11117 vinfos[3].indices[1] = _ij3[1];
11118 vinfos[3].maxsolutions = _nj3;
11119 vinfos[4].jointtype = 1;
11120 vinfos[4].foffset = j4;
11121 vinfos[4].indices[0] = _ij4[0];
11122 vinfos[4].indices[1] = _ij4[1];
11123 vinfos[4].maxsolutions = _nj4;
11124 vinfos[5].jointtype = 1;
11125 vinfos[5].foffset = j5;
11126 vinfos[5].indices[0] = _ij5[0];
11127 vinfos[5].indices[1] = _ij5[1];
11128 vinfos[5].maxsolutions = _nj5;
11129 vinfos[6].jointtype = 1;
11130 vinfos[6].foffset = j6;
11131 vinfos[6].indices[0] = _ij6[0];
11132 vinfos[6].indices[1] = _ij6[1];
11133 vinfos[6].maxsolutions = _nj6;
11134 std::vector<int> vfree(0);
11135 solutions.AddSolution(vinfos,vfree);
11136 }
11137 }
11138 }
11139 
11140 }
11141 
11142 }
11143 
11144 } else
11145 {
11146 {
11147 IkReal j4array[1], cj4array[1], sj4array[1];
11148 bool j4valid[1]={false};
11149 _nj4 = 1;
11150 CheckValue<IkReal> x524=IKPowWithIntegerCheck(gconst26,-1);
11151 if(!x524.valid){
11152 continue;
11153 }
11154 CheckValue<IkReal> x525=IKPowWithIntegerCheck(new_r11,-1);
11155 if(!x525.valid){
11156 continue;
11157 }
11158 if( IKabs((new_r10*(x524.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst26*(x525.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x524.value)))+IKsqr(((-1.0)*gconst26*(x525.value)))-1) <= IKFAST_SINCOS_THRESH )
11159  continue;
11160 j4array[0]=IKatan2((new_r10*(x524.value)), ((-1.0)*gconst26*(x525.value)));
11161 sj4array[0]=IKsin(j4array[0]);
11162 cj4array[0]=IKcos(j4array[0]);
11163 if( j4array[0] > IKPI )
11164 {
11165  j4array[0]-=IK2PI;
11166 }
11167 else if( j4array[0] < -IKPI )
11168 { j4array[0]+=IK2PI;
11169 }
11170 j4valid[0] = true;
11171 for(int ij4 = 0; ij4 < 1; ++ij4)
11172 {
11173 if( !j4valid[ij4] )
11174 {
11175  continue;
11176 }
11177 _ij4[0] = ij4; _ij4[1] = -1;
11178 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11179 {
11180 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11181 {
11182  j4valid[iij4]=false; _ij4[1] = iij4; break;
11183 }
11184 }
11185 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11186 {
11187 IkReal evalcond[8];
11188 IkReal x526=IKsin(j4);
11189 IkReal x527=IKcos(j4);
11190 IkReal x528=((1.0)*gconst26);
11191 IkReal x529=((-1.0)*gconst26);
11192 evalcond[0]=(new_r11*x526);
11193 evalcond[1]=(new_r10*x527);
11194 evalcond[2]=(x526*x529);
11195 evalcond[3]=(x527*x529);
11196 evalcond[4]=(((gconst26*x527))+new_r11);
11197 evalcond[5]=(gconst26+((new_r11*x527)));
11198 evalcond[6]=((((-1.0)*x526*x528))+new_r10);
11199 evalcond[7]=((((-1.0)*x528))+((new_r10*x526)));
11200 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11201 {
11202 continue;
11203 }
11204 }
11205 
11206 {
11207 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11208 vinfos[0].jointtype = 17;
11209 vinfos[0].foffset = j0;
11210 vinfos[0].indices[0] = _ij0[0];
11211 vinfos[0].indices[1] = _ij0[1];
11212 vinfos[0].maxsolutions = _nj0;
11213 vinfos[1].jointtype = 1;
11214 vinfos[1].foffset = j1;
11215 vinfos[1].indices[0] = _ij1[0];
11216 vinfos[1].indices[1] = _ij1[1];
11217 vinfos[1].maxsolutions = _nj1;
11218 vinfos[2].jointtype = 1;
11219 vinfos[2].foffset = j2;
11220 vinfos[2].indices[0] = _ij2[0];
11221 vinfos[2].indices[1] = _ij2[1];
11222 vinfos[2].maxsolutions = _nj2;
11223 vinfos[3].jointtype = 1;
11224 vinfos[3].foffset = j3;
11225 vinfos[3].indices[0] = _ij3[0];
11226 vinfos[3].indices[1] = _ij3[1];
11227 vinfos[3].maxsolutions = _nj3;
11228 vinfos[4].jointtype = 1;
11229 vinfos[4].foffset = j4;
11230 vinfos[4].indices[0] = _ij4[0];
11231 vinfos[4].indices[1] = _ij4[1];
11232 vinfos[4].maxsolutions = _nj4;
11233 vinfos[5].jointtype = 1;
11234 vinfos[5].foffset = j5;
11235 vinfos[5].indices[0] = _ij5[0];
11236 vinfos[5].indices[1] = _ij5[1];
11237 vinfos[5].maxsolutions = _nj5;
11238 vinfos[6].jointtype = 1;
11239 vinfos[6].foffset = j6;
11240 vinfos[6].indices[0] = _ij6[0];
11241 vinfos[6].indices[1] = _ij6[1];
11242 vinfos[6].maxsolutions = _nj6;
11243 std::vector<int> vfree(0);
11244 solutions.AddSolution(vinfos,vfree);
11245 }
11246 }
11247 }
11248 
11249 }
11250 
11251 }
11252 
11253 } else
11254 {
11255 {
11256 IkReal j4array[1], cj4array[1], sj4array[1];
11257 bool j4valid[1]={false};
11258 _nj4 = 1;
11259 CheckValue<IkReal> x530 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
11260 if(!x530.valid){
11261 continue;
11262 }
11263 CheckValue<IkReal> x531=IKPowWithIntegerCheck(IKsign(gconst26),-1);
11264 if(!x531.valid){
11265 continue;
11266 }
11267 j4array[0]=((-1.5707963267949)+(x530.value)+(((1.5707963267949)*(x531.value))));
11268 sj4array[0]=IKsin(j4array[0]);
11269 cj4array[0]=IKcos(j4array[0]);
11270 if( j4array[0] > IKPI )
11271 {
11272  j4array[0]-=IK2PI;
11273 }
11274 else if( j4array[0] < -IKPI )
11275 { j4array[0]+=IK2PI;
11276 }
11277 j4valid[0] = true;
11278 for(int ij4 = 0; ij4 < 1; ++ij4)
11279 {
11280 if( !j4valid[ij4] )
11281 {
11282  continue;
11283 }
11284 _ij4[0] = ij4; _ij4[1] = -1;
11285 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11286 {
11287 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11288 {
11289  j4valid[iij4]=false; _ij4[1] = iij4; break;
11290 }
11291 }
11292 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11293 {
11294 IkReal evalcond[8];
11295 IkReal x532=IKsin(j4);
11296 IkReal x533=IKcos(j4);
11297 IkReal x534=((1.0)*gconst26);
11298 IkReal x535=((-1.0)*gconst26);
11299 evalcond[0]=(new_r11*x532);
11300 evalcond[1]=(new_r10*x533);
11301 evalcond[2]=(x532*x535);
11302 evalcond[3]=(x533*x535);
11303 evalcond[4]=(((gconst26*x533))+new_r11);
11304 evalcond[5]=(gconst26+((new_r11*x533)));
11305 evalcond[6]=((((-1.0)*x532*x534))+new_r10);
11306 evalcond[7]=((((-1.0)*x534))+((new_r10*x532)));
11307 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11308 {
11309 continue;
11310 }
11311 }
11312 
11313 {
11314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11315 vinfos[0].jointtype = 17;
11316 vinfos[0].foffset = j0;
11317 vinfos[0].indices[0] = _ij0[0];
11318 vinfos[0].indices[1] = _ij0[1];
11319 vinfos[0].maxsolutions = _nj0;
11320 vinfos[1].jointtype = 1;
11321 vinfos[1].foffset = j1;
11322 vinfos[1].indices[0] = _ij1[0];
11323 vinfos[1].indices[1] = _ij1[1];
11324 vinfos[1].maxsolutions = _nj1;
11325 vinfos[2].jointtype = 1;
11326 vinfos[2].foffset = j2;
11327 vinfos[2].indices[0] = _ij2[0];
11328 vinfos[2].indices[1] = _ij2[1];
11329 vinfos[2].maxsolutions = _nj2;
11330 vinfos[3].jointtype = 1;
11331 vinfos[3].foffset = j3;
11332 vinfos[3].indices[0] = _ij3[0];
11333 vinfos[3].indices[1] = _ij3[1];
11334 vinfos[3].maxsolutions = _nj3;
11335 vinfos[4].jointtype = 1;
11336 vinfos[4].foffset = j4;
11337 vinfos[4].indices[0] = _ij4[0];
11338 vinfos[4].indices[1] = _ij4[1];
11339 vinfos[4].maxsolutions = _nj4;
11340 vinfos[5].jointtype = 1;
11341 vinfos[5].foffset = j5;
11342 vinfos[5].indices[0] = _ij5[0];
11343 vinfos[5].indices[1] = _ij5[1];
11344 vinfos[5].maxsolutions = _nj5;
11345 vinfos[6].jointtype = 1;
11346 vinfos[6].foffset = j6;
11347 vinfos[6].indices[0] = _ij6[0];
11348 vinfos[6].indices[1] = _ij6[1];
11349 vinfos[6].maxsolutions = _nj6;
11350 std::vector<int> vfree(0);
11351 solutions.AddSolution(vinfos,vfree);
11352 }
11353 }
11354 }
11355 
11356 }
11357 
11358 }
11359 
11360 }
11361 } while(0);
11362 if( bgotonextstatement )
11363 {
11364 bool bgotonextstatement = true;
11365 do
11366 {
11367 evalcond[0]=IKabs(new_r01);
11368 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11369 {
11370 bgotonextstatement=false;
11371 {
11372 IkReal j4eval[1];
11373 CheckValue<IkReal> x537 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
11374 if(!x537.valid){
11375 continue;
11376 }
11377 IkReal x536=((-1.0)*(x537.value));
11378 sj5=0;
11379 cj5=-1.0;
11380 j5=3.14159265358979;
11381 sj6=gconst25;
11382 cj6=gconst26;
11383 j6=x536;
11384 new_r01=0;
11385 IkReal gconst24=x536;
11386 IkReal gconst25=0;
11387 IkReal x538 = new_r11*new_r11;
11388 if(IKabs(x538)==0){
11389 continue;
11390 }
11391 IkReal gconst26=(new_r11*(pow(x538,-0.5)));
11392 j4eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
11393 if( IKabs(j4eval[0]) < 0.0000010000000000 )
11394 {
11395 {
11396 IkReal j4eval[1];
11397 CheckValue<IkReal> x540 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
11398 if(!x540.valid){
11399 continue;
11400 }
11401 IkReal x539=((-1.0)*(x540.value));
11402 sj5=0;
11403 cj5=-1.0;
11404 j5=3.14159265358979;
11405 sj6=gconst25;
11406 cj6=gconst26;
11407 j6=x539;
11408 new_r01=0;
11409 IkReal gconst24=x539;
11410 IkReal gconst25=0;
11411 IkReal x541 = new_r11*new_r11;
11412 if(IKabs(x541)==0){
11413 continue;
11414 }
11415 IkReal gconst26=(new_r11*(pow(x541,-0.5)));
11416 j4eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11417 if( IKabs(j4eval[0]) < 0.0000010000000000 )
11418 {
11419 {
11420 IkReal j4eval[1];
11421 CheckValue<IkReal> x543 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
11422 if(!x543.valid){
11423 continue;
11424 }
11425 IkReal x542=((-1.0)*(x543.value));
11426 sj5=0;
11427 cj5=-1.0;
11428 j5=3.14159265358979;
11429 sj6=gconst25;
11430 cj6=gconst26;
11431 j6=x542;
11432 new_r01=0;
11433 IkReal gconst24=x542;
11434 IkReal gconst25=0;
11435 IkReal x544 = new_r11*new_r11;
11436 if(IKabs(x544)==0){
11437 continue;
11438 }
11439 IkReal gconst26=(new_r11*(pow(x544,-0.5)));
11440 j4eval[0]=new_r11;
11441 if( IKabs(j4eval[0]) < 0.0000010000000000 )
11442 {
11443 continue; // 3 cases reached
11444 
11445 } else
11446 {
11447 {
11448 IkReal j4array[1], cj4array[1], sj4array[1];
11449 bool j4valid[1]={false};
11450 _nj4 = 1;
11451 CheckValue<IkReal> x545=IKPowWithIntegerCheck(gconst26,-1);
11452 if(!x545.valid){
11453 continue;
11454 }
11455 CheckValue<IkReal> x546=IKPowWithIntegerCheck(new_r11,-1);
11456 if(!x546.valid){
11457 continue;
11458 }
11459 if( IKabs((new_r10*(x545.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst26*(x546.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x545.value)))+IKsqr(((-1.0)*gconst26*(x546.value)))-1) <= IKFAST_SINCOS_THRESH )
11460  continue;
11461 j4array[0]=IKatan2((new_r10*(x545.value)), ((-1.0)*gconst26*(x546.value)));
11462 sj4array[0]=IKsin(j4array[0]);
11463 cj4array[0]=IKcos(j4array[0]);
11464 if( j4array[0] > IKPI )
11465 {
11466  j4array[0]-=IK2PI;
11467 }
11468 else if( j4array[0] < -IKPI )
11469 { j4array[0]+=IK2PI;
11470 }
11471 j4valid[0] = true;
11472 for(int ij4 = 0; ij4 < 1; ++ij4)
11473 {
11474 if( !j4valid[ij4] )
11475 {
11476  continue;
11477 }
11478 _ij4[0] = ij4; _ij4[1] = -1;
11479 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11480 {
11481 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11482 {
11483  j4valid[iij4]=false; _ij4[1] = iij4; break;
11484 }
11485 }
11486 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11487 {
11488 IkReal evalcond[8];
11489 IkReal x547=IKsin(j4);
11490 IkReal x548=IKcos(j4);
11491 IkReal x549=((1.0)*gconst26);
11492 IkReal x550=(gconst26*x547);
11493 evalcond[0]=(new_r11*x547);
11494 evalcond[1]=((-1.0)*x550);
11495 evalcond[2]=(((gconst26*x548))+new_r11);
11496 evalcond[3]=(gconst26+((new_r11*x548)));
11497 evalcond[4]=((((-1.0)*x548*x549))+new_r00);
11498 evalcond[5]=((((-1.0)*x547*x549))+new_r10);
11499 evalcond[6]=(((new_r10*x548))+(((-1.0)*new_r00*x547)));
11500 evalcond[7]=(((new_r00*x548))+((new_r10*x547))+(((-1.0)*x549)));
11501 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11502 {
11503 continue;
11504 }
11505 }
11506 
11507 {
11508 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11509 vinfos[0].jointtype = 17;
11510 vinfos[0].foffset = j0;
11511 vinfos[0].indices[0] = _ij0[0];
11512 vinfos[0].indices[1] = _ij0[1];
11513 vinfos[0].maxsolutions = _nj0;
11514 vinfos[1].jointtype = 1;
11515 vinfos[1].foffset = j1;
11516 vinfos[1].indices[0] = _ij1[0];
11517 vinfos[1].indices[1] = _ij1[1];
11518 vinfos[1].maxsolutions = _nj1;
11519 vinfos[2].jointtype = 1;
11520 vinfos[2].foffset = j2;
11521 vinfos[2].indices[0] = _ij2[0];
11522 vinfos[2].indices[1] = _ij2[1];
11523 vinfos[2].maxsolutions = _nj2;
11524 vinfos[3].jointtype = 1;
11525 vinfos[3].foffset = j3;
11526 vinfos[3].indices[0] = _ij3[0];
11527 vinfos[3].indices[1] = _ij3[1];
11528 vinfos[3].maxsolutions = _nj3;
11529 vinfos[4].jointtype = 1;
11530 vinfos[4].foffset = j4;
11531 vinfos[4].indices[0] = _ij4[0];
11532 vinfos[4].indices[1] = _ij4[1];
11533 vinfos[4].maxsolutions = _nj4;
11534 vinfos[5].jointtype = 1;
11535 vinfos[5].foffset = j5;
11536 vinfos[5].indices[0] = _ij5[0];
11537 vinfos[5].indices[1] = _ij5[1];
11538 vinfos[5].maxsolutions = _nj5;
11539 vinfos[6].jointtype = 1;
11540 vinfos[6].foffset = j6;
11541 vinfos[6].indices[0] = _ij6[0];
11542 vinfos[6].indices[1] = _ij6[1];
11543 vinfos[6].maxsolutions = _nj6;
11544 std::vector<int> vfree(0);
11545 solutions.AddSolution(vinfos,vfree);
11546 }
11547 }
11548 }
11549 
11550 }
11551 
11552 }
11553 
11554 } else
11555 {
11556 {
11557 IkReal j4array[1], cj4array[1], sj4array[1];
11558 bool j4valid[1]={false};
11559 _nj4 = 1;
11560 CheckValue<IkReal> x551 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
11561 if(!x551.valid){
11562 continue;
11563 }
11564 CheckValue<IkReal> x552=IKPowWithIntegerCheck(IKsign(gconst26),-1);
11565 if(!x552.valid){
11566 continue;
11567 }
11568 j4array[0]=((-1.5707963267949)+(x551.value)+(((1.5707963267949)*(x552.value))));
11569 sj4array[0]=IKsin(j4array[0]);
11570 cj4array[0]=IKcos(j4array[0]);
11571 if( j4array[0] > IKPI )
11572 {
11573  j4array[0]-=IK2PI;
11574 }
11575 else if( j4array[0] < -IKPI )
11576 { j4array[0]+=IK2PI;
11577 }
11578 j4valid[0] = true;
11579 for(int ij4 = 0; ij4 < 1; ++ij4)
11580 {
11581 if( !j4valid[ij4] )
11582 {
11583  continue;
11584 }
11585 _ij4[0] = ij4; _ij4[1] = -1;
11586 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11587 {
11588 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11589 {
11590  j4valid[iij4]=false; _ij4[1] = iij4; break;
11591 }
11592 }
11593 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11594 {
11595 IkReal evalcond[8];
11596 IkReal x553=IKsin(j4);
11597 IkReal x554=IKcos(j4);
11598 IkReal x555=((1.0)*gconst26);
11599 IkReal x556=(gconst26*x553);
11600 evalcond[0]=(new_r11*x553);
11601 evalcond[1]=((-1.0)*x556);
11602 evalcond[2]=(((gconst26*x554))+new_r11);
11603 evalcond[3]=(((new_r11*x554))+gconst26);
11604 evalcond[4]=((((-1.0)*x554*x555))+new_r00);
11605 evalcond[5]=((((-1.0)*x553*x555))+new_r10);
11606 evalcond[6]=((((-1.0)*new_r00*x553))+((new_r10*x554)));
11607 evalcond[7]=(((new_r10*x553))+(((-1.0)*x555))+((new_r00*x554)));
11608 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11609 {
11610 continue;
11611 }
11612 }
11613 
11614 {
11615 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11616 vinfos[0].jointtype = 17;
11617 vinfos[0].foffset = j0;
11618 vinfos[0].indices[0] = _ij0[0];
11619 vinfos[0].indices[1] = _ij0[1];
11620 vinfos[0].maxsolutions = _nj0;
11621 vinfos[1].jointtype = 1;
11622 vinfos[1].foffset = j1;
11623 vinfos[1].indices[0] = _ij1[0];
11624 vinfos[1].indices[1] = _ij1[1];
11625 vinfos[1].maxsolutions = _nj1;
11626 vinfos[2].jointtype = 1;
11627 vinfos[2].foffset = j2;
11628 vinfos[2].indices[0] = _ij2[0];
11629 vinfos[2].indices[1] = _ij2[1];
11630 vinfos[2].maxsolutions = _nj2;
11631 vinfos[3].jointtype = 1;
11632 vinfos[3].foffset = j3;
11633 vinfos[3].indices[0] = _ij3[0];
11634 vinfos[3].indices[1] = _ij3[1];
11635 vinfos[3].maxsolutions = _nj3;
11636 vinfos[4].jointtype = 1;
11637 vinfos[4].foffset = j4;
11638 vinfos[4].indices[0] = _ij4[0];
11639 vinfos[4].indices[1] = _ij4[1];
11640 vinfos[4].maxsolutions = _nj4;
11641 vinfos[5].jointtype = 1;
11642 vinfos[5].foffset = j5;
11643 vinfos[5].indices[0] = _ij5[0];
11644 vinfos[5].indices[1] = _ij5[1];
11645 vinfos[5].maxsolutions = _nj5;
11646 vinfos[6].jointtype = 1;
11647 vinfos[6].foffset = j6;
11648 vinfos[6].indices[0] = _ij6[0];
11649 vinfos[6].indices[1] = _ij6[1];
11650 vinfos[6].maxsolutions = _nj6;
11651 std::vector<int> vfree(0);
11652 solutions.AddSolution(vinfos,vfree);
11653 }
11654 }
11655 }
11656 
11657 }
11658 
11659 }
11660 
11661 } else
11662 {
11663 {
11664 IkReal j4array[1], cj4array[1], sj4array[1];
11665 bool j4valid[1]={false};
11666 _nj4 = 1;
11667 CheckValue<IkReal> x557=IKPowWithIntegerCheck(IKsign(gconst26),-1);
11668 if(!x557.valid){
11669 continue;
11670 }
11671 CheckValue<IkReal> x558 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
11672 if(!x558.valid){
11673 continue;
11674 }
11675 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x557.value)))+(x558.value));
11676 sj4array[0]=IKsin(j4array[0]);
11677 cj4array[0]=IKcos(j4array[0]);
11678 if( j4array[0] > IKPI )
11679 {
11680  j4array[0]-=IK2PI;
11681 }
11682 else if( j4array[0] < -IKPI )
11683 { j4array[0]+=IK2PI;
11684 }
11685 j4valid[0] = true;
11686 for(int ij4 = 0; ij4 < 1; ++ij4)
11687 {
11688 if( !j4valid[ij4] )
11689 {
11690  continue;
11691 }
11692 _ij4[0] = ij4; _ij4[1] = -1;
11693 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11694 {
11695 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11696 {
11697  j4valid[iij4]=false; _ij4[1] = iij4; break;
11698 }
11699 }
11700 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11701 {
11702 IkReal evalcond[8];
11703 IkReal x559=IKsin(j4);
11704 IkReal x560=IKcos(j4);
11705 IkReal x561=((1.0)*gconst26);
11706 IkReal x562=(gconst26*x559);
11707 evalcond[0]=(new_r11*x559);
11708 evalcond[1]=((-1.0)*x562);
11709 evalcond[2]=(((gconst26*x560))+new_r11);
11710 evalcond[3]=(gconst26+((new_r11*x560)));
11711 evalcond[4]=((((-1.0)*x560*x561))+new_r00);
11712 evalcond[5]=((((-1.0)*x559*x561))+new_r10);
11713 evalcond[6]=((((-1.0)*new_r00*x559))+((new_r10*x560)));
11714 evalcond[7]=(((new_r10*x559))+((new_r00*x560))+(((-1.0)*x561)));
11715 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11716 {
11717 continue;
11718 }
11719 }
11720 
11721 {
11722 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11723 vinfos[0].jointtype = 17;
11724 vinfos[0].foffset = j0;
11725 vinfos[0].indices[0] = _ij0[0];
11726 vinfos[0].indices[1] = _ij0[1];
11727 vinfos[0].maxsolutions = _nj0;
11728 vinfos[1].jointtype = 1;
11729 vinfos[1].foffset = j1;
11730 vinfos[1].indices[0] = _ij1[0];
11731 vinfos[1].indices[1] = _ij1[1];
11732 vinfos[1].maxsolutions = _nj1;
11733 vinfos[2].jointtype = 1;
11734 vinfos[2].foffset = j2;
11735 vinfos[2].indices[0] = _ij2[0];
11736 vinfos[2].indices[1] = _ij2[1];
11737 vinfos[2].maxsolutions = _nj2;
11738 vinfos[3].jointtype = 1;
11739 vinfos[3].foffset = j3;
11740 vinfos[3].indices[0] = _ij3[0];
11741 vinfos[3].indices[1] = _ij3[1];
11742 vinfos[3].maxsolutions = _nj3;
11743 vinfos[4].jointtype = 1;
11744 vinfos[4].foffset = j4;
11745 vinfos[4].indices[0] = _ij4[0];
11746 vinfos[4].indices[1] = _ij4[1];
11747 vinfos[4].maxsolutions = _nj4;
11748 vinfos[5].jointtype = 1;
11749 vinfos[5].foffset = j5;
11750 vinfos[5].indices[0] = _ij5[0];
11751 vinfos[5].indices[1] = _ij5[1];
11752 vinfos[5].maxsolutions = _nj5;
11753 vinfos[6].jointtype = 1;
11754 vinfos[6].foffset = j6;
11755 vinfos[6].indices[0] = _ij6[0];
11756 vinfos[6].indices[1] = _ij6[1];
11757 vinfos[6].maxsolutions = _nj6;
11758 std::vector<int> vfree(0);
11759 solutions.AddSolution(vinfos,vfree);
11760 }
11761 }
11762 }
11763 
11764 }
11765 
11766 }
11767 
11768 }
11769 } while(0);
11770 if( bgotonextstatement )
11771 {
11772 bool bgotonextstatement = true;
11773 do
11774 {
11775 if( 1 )
11776 {
11777 bgotonextstatement=false;
11778 continue; // branch miss [j4]
11779 
11780 }
11781 } while(0);
11782 if( bgotonextstatement )
11783 {
11784 }
11785 }
11786 }
11787 }
11788 }
11789 }
11790 }
11791 }
11792 
11793 } else
11794 {
11795 {
11796 IkReal j4array[1], cj4array[1], sj4array[1];
11797 bool j4valid[1]={false};
11798 _nj4 = 1;
11799 IkReal x563=((1.0)*new_r11);
11800 CheckValue<IkReal> x564 = IKatan2WithCheck(IkReal((((gconst26*new_r01))+(((-1.0)*gconst25*x563)))),IkReal(((((-1.0)*gconst25*new_r01))+(((-1.0)*gconst26*x563)))),IKFAST_ATAN2_MAGTHRESH);
11801 if(!x564.valid){
11802 continue;
11803 }
11804 CheckValue<IkReal> x565=IKPowWithIntegerCheck(IKsign(((gconst26*gconst26)+(gconst25*gconst25))),-1);
11805 if(!x565.valid){
11806 continue;
11807 }
11808 j4array[0]=((-1.5707963267949)+(x564.value)+(((1.5707963267949)*(x565.value))));
11809 sj4array[0]=IKsin(j4array[0]);
11810 cj4array[0]=IKcos(j4array[0]);
11811 if( j4array[0] > IKPI )
11812 {
11813  j4array[0]-=IK2PI;
11814 }
11815 else if( j4array[0] < -IKPI )
11816 { j4array[0]+=IK2PI;
11817 }
11818 j4valid[0] = true;
11819 for(int ij4 = 0; ij4 < 1; ++ij4)
11820 {
11821 if( !j4valid[ij4] )
11822 {
11823  continue;
11824 }
11825 _ij4[0] = ij4; _ij4[1] = -1;
11826 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11827 {
11828 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11829 {
11830  j4valid[iij4]=false; _ij4[1] = iij4; break;
11831 }
11832 }
11833 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11834 {
11835 IkReal evalcond[8];
11836 IkReal x566=IKcos(j4);
11837 IkReal x567=IKsin(j4);
11838 IkReal x568=((1.0)*gconst26);
11839 IkReal x569=(gconst25*x566);
11840 IkReal x570=((1.0)*x567);
11841 IkReal x571=(x567*x568);
11842 evalcond[0]=(gconst25+((new_r01*x566))+((new_r11*x567)));
11843 evalcond[1]=(((gconst26*x566))+new_r11+((gconst25*x567)));
11844 evalcond[2]=((((-1.0)*x571))+x569+new_r01);
11845 evalcond[3]=(gconst25+(((-1.0)*new_r00*x570))+((new_r10*x566)));
11846 evalcond[4]=(gconst26+((new_r11*x566))+(((-1.0)*new_r01*x570)));
11847 evalcond[5]=((((-1.0)*x571))+x569+new_r10);
11848 evalcond[6]=(((new_r00*x566))+((new_r10*x567))+(((-1.0)*x568)));
11849 evalcond[7]=((((-1.0)*x566*x568))+(((-1.0)*gconst25*x570))+new_r00);
11850 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11851 {
11852 continue;
11853 }
11854 }
11855 
11856 {
11857 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11858 vinfos[0].jointtype = 17;
11859 vinfos[0].foffset = j0;
11860 vinfos[0].indices[0] = _ij0[0];
11861 vinfos[0].indices[1] = _ij0[1];
11862 vinfos[0].maxsolutions = _nj0;
11863 vinfos[1].jointtype = 1;
11864 vinfos[1].foffset = j1;
11865 vinfos[1].indices[0] = _ij1[0];
11866 vinfos[1].indices[1] = _ij1[1];
11867 vinfos[1].maxsolutions = _nj1;
11868 vinfos[2].jointtype = 1;
11869 vinfos[2].foffset = j2;
11870 vinfos[2].indices[0] = _ij2[0];
11871 vinfos[2].indices[1] = _ij2[1];
11872 vinfos[2].maxsolutions = _nj2;
11873 vinfos[3].jointtype = 1;
11874 vinfos[3].foffset = j3;
11875 vinfos[3].indices[0] = _ij3[0];
11876 vinfos[3].indices[1] = _ij3[1];
11877 vinfos[3].maxsolutions = _nj3;
11878 vinfos[4].jointtype = 1;
11879 vinfos[4].foffset = j4;
11880 vinfos[4].indices[0] = _ij4[0];
11881 vinfos[4].indices[1] = _ij4[1];
11882 vinfos[4].maxsolutions = _nj4;
11883 vinfos[5].jointtype = 1;
11884 vinfos[5].foffset = j5;
11885 vinfos[5].indices[0] = _ij5[0];
11886 vinfos[5].indices[1] = _ij5[1];
11887 vinfos[5].maxsolutions = _nj5;
11888 vinfos[6].jointtype = 1;
11889 vinfos[6].foffset = j6;
11890 vinfos[6].indices[0] = _ij6[0];
11891 vinfos[6].indices[1] = _ij6[1];
11892 vinfos[6].maxsolutions = _nj6;
11893 std::vector<int> vfree(0);
11894 solutions.AddSolution(vinfos,vfree);
11895 }
11896 }
11897 }
11898 
11899 }
11900 
11901 }
11902 
11903 } else
11904 {
11905 {
11906 IkReal j4array[1], cj4array[1], sj4array[1];
11907 bool j4valid[1]={false};
11908 _nj4 = 1;
11909 IkReal x572=((1.0)*gconst25);
11910 CheckValue<IkReal> x573 = IKatan2WithCheck(IkReal((((gconst26*new_r01))+(((-1.0)*new_r11*x572)))),IkReal(((((-1.0)*gconst26*new_r11))+(((-1.0)*new_r01*x572)))),IKFAST_ATAN2_MAGTHRESH);
11911 if(!x573.valid){
11912 continue;
11913 }
11914 CheckValue<IkReal> x574=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
11915 if(!x574.valid){
11916 continue;
11917 }
11918 j4array[0]=((-1.5707963267949)+(x573.value)+(((1.5707963267949)*(x574.value))));
11919 sj4array[0]=IKsin(j4array[0]);
11920 cj4array[0]=IKcos(j4array[0]);
11921 if( j4array[0] > IKPI )
11922 {
11923  j4array[0]-=IK2PI;
11924 }
11925 else if( j4array[0] < -IKPI )
11926 { j4array[0]+=IK2PI;
11927 }
11928 j4valid[0] = true;
11929 for(int ij4 = 0; ij4 < 1; ++ij4)
11930 {
11931 if( !j4valid[ij4] )
11932 {
11933  continue;
11934 }
11935 _ij4[0] = ij4; _ij4[1] = -1;
11936 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
11937 {
11938 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
11939 {
11940  j4valid[iij4]=false; _ij4[1] = iij4; break;
11941 }
11942 }
11943 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
11944 {
11945 IkReal evalcond[8];
11946 IkReal x575=IKcos(j4);
11947 IkReal x576=IKsin(j4);
11948 IkReal x577=((1.0)*gconst26);
11949 IkReal x578=(gconst25*x575);
11950 IkReal x579=((1.0)*x576);
11951 IkReal x580=(x576*x577);
11952 evalcond[0]=(((new_r11*x576))+((new_r01*x575))+gconst25);
11953 evalcond[1]=(((gconst25*x576))+((gconst26*x575))+new_r11);
11954 evalcond[2]=(x578+new_r01+(((-1.0)*x580)));
11955 evalcond[3]=(((new_r10*x575))+gconst25+(((-1.0)*new_r00*x579)));
11956 evalcond[4]=(((new_r11*x575))+gconst26+(((-1.0)*new_r01*x579)));
11957 evalcond[5]=(x578+new_r10+(((-1.0)*x580)));
11958 evalcond[6]=(((new_r00*x575))+((new_r10*x576))+(((-1.0)*x577)));
11959 evalcond[7]=((((-1.0)*gconst25*x579))+(((-1.0)*x575*x577))+new_r00);
11960 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11961 {
11962 continue;
11963 }
11964 }
11965 
11966 {
11967 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11968 vinfos[0].jointtype = 17;
11969 vinfos[0].foffset = j0;
11970 vinfos[0].indices[0] = _ij0[0];
11971 vinfos[0].indices[1] = _ij0[1];
11972 vinfos[0].maxsolutions = _nj0;
11973 vinfos[1].jointtype = 1;
11974 vinfos[1].foffset = j1;
11975 vinfos[1].indices[0] = _ij1[0];
11976 vinfos[1].indices[1] = _ij1[1];
11977 vinfos[1].maxsolutions = _nj1;
11978 vinfos[2].jointtype = 1;
11979 vinfos[2].foffset = j2;
11980 vinfos[2].indices[0] = _ij2[0];
11981 vinfos[2].indices[1] = _ij2[1];
11982 vinfos[2].maxsolutions = _nj2;
11983 vinfos[3].jointtype = 1;
11984 vinfos[3].foffset = j3;
11985 vinfos[3].indices[0] = _ij3[0];
11986 vinfos[3].indices[1] = _ij3[1];
11987 vinfos[3].maxsolutions = _nj3;
11988 vinfos[4].jointtype = 1;
11989 vinfos[4].foffset = j4;
11990 vinfos[4].indices[0] = _ij4[0];
11991 vinfos[4].indices[1] = _ij4[1];
11992 vinfos[4].maxsolutions = _nj4;
11993 vinfos[5].jointtype = 1;
11994 vinfos[5].foffset = j5;
11995 vinfos[5].indices[0] = _ij5[0];
11996 vinfos[5].indices[1] = _ij5[1];
11997 vinfos[5].maxsolutions = _nj5;
11998 vinfos[6].jointtype = 1;
11999 vinfos[6].foffset = j6;
12000 vinfos[6].indices[0] = _ij6[0];
12001 vinfos[6].indices[1] = _ij6[1];
12002 vinfos[6].maxsolutions = _nj6;
12003 std::vector<int> vfree(0);
12004 solutions.AddSolution(vinfos,vfree);
12005 }
12006 }
12007 }
12008 
12009 }
12010 
12011 }
12012 
12013 } else
12014 {
12015 {
12016 IkReal j4array[1], cj4array[1], sj4array[1];
12017 bool j4valid[1]={false};
12018 _nj4 = 1;
12019 IkReal x581=((1.0)*gconst25);
12020 CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal((((gconst25*new_r01))+(((-1.0)*new_r10*x581)))),IkReal(((((-1.0)*new_r11*x581))+(((-1.0)*new_r00*x581)))),IKFAST_ATAN2_MAGTHRESH);
12021 if(!x582.valid){
12022 continue;
12023 }
12024 CheckValue<IkReal> x583=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
12025 if(!x583.valid){
12026 continue;
12027 }
12028 j4array[0]=((-1.5707963267949)+(x582.value)+(((1.5707963267949)*(x583.value))));
12029 sj4array[0]=IKsin(j4array[0]);
12030 cj4array[0]=IKcos(j4array[0]);
12031 if( j4array[0] > IKPI )
12032 {
12033  j4array[0]-=IK2PI;
12034 }
12035 else if( j4array[0] < -IKPI )
12036 { j4array[0]+=IK2PI;
12037 }
12038 j4valid[0] = true;
12039 for(int ij4 = 0; ij4 < 1; ++ij4)
12040 {
12041 if( !j4valid[ij4] )
12042 {
12043  continue;
12044 }
12045 _ij4[0] = ij4; _ij4[1] = -1;
12046 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
12047 {
12048 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12049 {
12050  j4valid[iij4]=false; _ij4[1] = iij4; break;
12051 }
12052 }
12053 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12054 {
12055 IkReal evalcond[8];
12056 IkReal x584=IKcos(j4);
12057 IkReal x585=IKsin(j4);
12058 IkReal x586=((1.0)*gconst26);
12059 IkReal x587=(gconst25*x584);
12060 IkReal x588=((1.0)*x585);
12061 IkReal x589=(x585*x586);
12062 evalcond[0]=(gconst25+((new_r01*x584))+((new_r11*x585)));
12063 evalcond[1]=(((gconst25*x585))+((gconst26*x584))+new_r11);
12064 evalcond[2]=(x587+new_r01+(((-1.0)*x589)));
12065 evalcond[3]=(gconst25+(((-1.0)*new_r00*x588))+((new_r10*x584)));
12066 evalcond[4]=((((-1.0)*new_r01*x588))+gconst26+((new_r11*x584)));
12067 evalcond[5]=(x587+new_r10+(((-1.0)*x589)));
12068 evalcond[6]=(((new_r00*x584))+(((-1.0)*x586))+((new_r10*x585)));
12069 evalcond[7]=((((-1.0)*x584*x586))+new_r00+(((-1.0)*gconst25*x588)));
12070 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12071 {
12072 continue;
12073 }
12074 }
12075 
12076 {
12077 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12078 vinfos[0].jointtype = 17;
12079 vinfos[0].foffset = j0;
12080 vinfos[0].indices[0] = _ij0[0];
12081 vinfos[0].indices[1] = _ij0[1];
12082 vinfos[0].maxsolutions = _nj0;
12083 vinfos[1].jointtype = 1;
12084 vinfos[1].foffset = j1;
12085 vinfos[1].indices[0] = _ij1[0];
12086 vinfos[1].indices[1] = _ij1[1];
12087 vinfos[1].maxsolutions = _nj1;
12088 vinfos[2].jointtype = 1;
12089 vinfos[2].foffset = j2;
12090 vinfos[2].indices[0] = _ij2[0];
12091 vinfos[2].indices[1] = _ij2[1];
12092 vinfos[2].maxsolutions = _nj2;
12093 vinfos[3].jointtype = 1;
12094 vinfos[3].foffset = j3;
12095 vinfos[3].indices[0] = _ij3[0];
12096 vinfos[3].indices[1] = _ij3[1];
12097 vinfos[3].maxsolutions = _nj3;
12098 vinfos[4].jointtype = 1;
12099 vinfos[4].foffset = j4;
12100 vinfos[4].indices[0] = _ij4[0];
12101 vinfos[4].indices[1] = _ij4[1];
12102 vinfos[4].maxsolutions = _nj4;
12103 vinfos[5].jointtype = 1;
12104 vinfos[5].foffset = j5;
12105 vinfos[5].indices[0] = _ij5[0];
12106 vinfos[5].indices[1] = _ij5[1];
12107 vinfos[5].maxsolutions = _nj5;
12108 vinfos[6].jointtype = 1;
12109 vinfos[6].foffset = j6;
12110 vinfos[6].indices[0] = _ij6[0];
12111 vinfos[6].indices[1] = _ij6[1];
12112 vinfos[6].maxsolutions = _nj6;
12113 std::vector<int> vfree(0);
12114 solutions.AddSolution(vinfos,vfree);
12115 }
12116 }
12117 }
12118 
12119 }
12120 
12121 }
12122 
12123 }
12124 } while(0);
12125 if( bgotonextstatement )
12126 {
12127 bool bgotonextstatement = true;
12128 do
12129 {
12130 IkReal x591 = ((new_r01*new_r01)+(new_r11*new_r11));
12131 if(IKabs(x591)==0){
12132 continue;
12133 }
12134 IkReal x590=pow(x591,-0.5);
12135 CheckValue<IkReal> x592 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12136 if(!x592.valid){
12137 continue;
12138 }
12139 IkReal gconst27=((3.14159265358979)+(((-1.0)*(x592.value))));
12140 IkReal gconst28=((1.0)*new_r01*x590);
12141 IkReal gconst29=((-1.0)*new_r11*x590);
12142 CheckValue<IkReal> x593 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12143 if(!x593.valid){
12144 continue;
12145 }
12146 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x593.value)+j6)))), 6.28318530717959)));
12147 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12148 {
12149 bgotonextstatement=false;
12150 {
12151 IkReal j4eval[3];
12152 CheckValue<IkReal> x596 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12153 if(!x596.valid){
12154 continue;
12155 }
12156 IkReal x594=((1.0)*(x596.value));
12157 IkReal x595=x590;
12158 sj5=0;
12159 cj5=-1.0;
12160 j5=3.14159265358979;
12161 sj6=gconst28;
12162 cj6=gconst29;
12163 j6=((3.14159265)+(((-1.0)*x594)));
12164 IkReal gconst27=((3.14159265358979)+(((-1.0)*x594)));
12165 IkReal gconst28=((1.0)*new_r01*x595);
12166 IkReal gconst29=((-1.0)*new_r11*x595);
12167 IkReal x597=new_r01*new_r01;
12168 IkReal x598=(((new_r10*new_r11))+((new_r00*new_r01)));
12169 IkReal x599=x590;
12170 IkReal x600=((1.0)*new_r01*x599);
12171 j4eval[0]=x598;
12172 j4eval[1]=IKsign(x598);
12173 j4eval[2]=((IKabs(((((-1.0)*new_r10*x600))+((x597*x599)))))+(IKabs(((((-1.0)*new_r11*x600))+(((-1.0)*new_r00*x600))))));
12174 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
12175 {
12176 {
12177 IkReal j4eval[2];
12178 CheckValue<IkReal> x603 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12179 if(!x603.valid){
12180 continue;
12181 }
12182 IkReal x601=((1.0)*(x603.value));
12183 IkReal x602=x590;
12184 sj5=0;
12185 cj5=-1.0;
12186 j5=3.14159265358979;
12187 sj6=gconst28;
12188 cj6=gconst29;
12189 j6=((3.14159265)+(((-1.0)*x601)));
12190 IkReal gconst27=((3.14159265358979)+(((-1.0)*x601)));
12191 IkReal gconst28=((1.0)*new_r01*x602);
12192 IkReal gconst29=((-1.0)*new_r11*x602);
12193 IkReal x604=((new_r01*new_r01)+(new_r11*new_r11));
12194 j4eval[0]=x604;
12195 j4eval[1]=IKsign(x604);
12196 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 )
12197 {
12198 {
12199 IkReal j4eval[1];
12200 CheckValue<IkReal> x607 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12201 if(!x607.valid){
12202 continue;
12203 }
12204 IkReal x605=((1.0)*(x607.value));
12205 IkReal x606=x590;
12206 sj5=0;
12207 cj5=-1.0;
12208 j5=3.14159265358979;
12209 sj6=gconst28;
12210 cj6=gconst29;
12211 j6=((3.14159265)+(((-1.0)*x605)));
12212 IkReal gconst27=((3.14159265358979)+(((-1.0)*x605)));
12213 IkReal gconst28=((1.0)*new_r01*x606);
12214 IkReal gconst29=((-1.0)*new_r11*x606);
12215 j4eval[0]=((new_r01*new_r01)+(new_r11*new_r11));
12216 if( IKabs(j4eval[0]) < 0.0000010000000000 )
12217 {
12218 {
12219 IkReal evalcond[5];
12220 bool bgotonextstatement = true;
12221 do
12222 {
12223 evalcond[0]=((gconst28*gconst28)+(gconst29*gconst29));
12224 evalcond[1]=new_r01;
12225 evalcond[2]=new_r00;
12226 evalcond[3]=new_r11;
12227 evalcond[4]=new_r10;
12228 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
12229 {
12230 bgotonextstatement=false;
12231 {
12232 IkReal j4array[4], cj4array[4], sj4array[4];
12233 bool j4valid[4]={false};
12234 _nj4 = 4;
12235 j4array[0]=0;
12236 sj4array[0]=IKsin(j4array[0]);
12237 cj4array[0]=IKcos(j4array[0]);
12238 j4array[1]=1.5707963267949;
12239 sj4array[1]=IKsin(j4array[1]);
12240 cj4array[1]=IKcos(j4array[1]);
12241 j4array[2]=3.14159265358979;
12242 sj4array[2]=IKsin(j4array[2]);
12243 cj4array[2]=IKcos(j4array[2]);
12244 j4array[3]=-1.5707963267949;
12245 sj4array[3]=IKsin(j4array[3]);
12246 cj4array[3]=IKcos(j4array[3]);
12247 if( j4array[0] > IKPI )
12248 {
12249  j4array[0]-=IK2PI;
12250 }
12251 else if( j4array[0] < -IKPI )
12252 { j4array[0]+=IK2PI;
12253 }
12254 j4valid[0] = true;
12255 if( j4array[1] > IKPI )
12256 {
12257  j4array[1]-=IK2PI;
12258 }
12259 else if( j4array[1] < -IKPI )
12260 { j4array[1]+=IK2PI;
12261 }
12262 j4valid[1] = true;
12263 if( j4array[2] > IKPI )
12264 {
12265  j4array[2]-=IK2PI;
12266 }
12267 else if( j4array[2] < -IKPI )
12268 { j4array[2]+=IK2PI;
12269 }
12270 j4valid[2] = true;
12271 if( j4array[3] > IKPI )
12272 {
12273  j4array[3]-=IK2PI;
12274 }
12275 else if( j4array[3] < -IKPI )
12276 { j4array[3]+=IK2PI;
12277 }
12278 j4valid[3] = true;
12279 for(int ij4 = 0; ij4 < 4; ++ij4)
12280 {
12281 if( !j4valid[ij4] )
12282 {
12283  continue;
12284 }
12285 _ij4[0] = ij4; _ij4[1] = -1;
12286 for(int iij4 = ij4+1; iij4 < 4; ++iij4)
12287 {
12288 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12289 {
12290  j4valid[iij4]=false; _ij4[1] = iij4; break;
12291 }
12292 }
12293 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12294 
12295 {
12296 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12297 vinfos[0].jointtype = 17;
12298 vinfos[0].foffset = j0;
12299 vinfos[0].indices[0] = _ij0[0];
12300 vinfos[0].indices[1] = _ij0[1];
12301 vinfos[0].maxsolutions = _nj0;
12302 vinfos[1].jointtype = 1;
12303 vinfos[1].foffset = j1;
12304 vinfos[1].indices[0] = _ij1[0];
12305 vinfos[1].indices[1] = _ij1[1];
12306 vinfos[1].maxsolutions = _nj1;
12307 vinfos[2].jointtype = 1;
12308 vinfos[2].foffset = j2;
12309 vinfos[2].indices[0] = _ij2[0];
12310 vinfos[2].indices[1] = _ij2[1];
12311 vinfos[2].maxsolutions = _nj2;
12312 vinfos[3].jointtype = 1;
12313 vinfos[3].foffset = j3;
12314 vinfos[3].indices[0] = _ij3[0];
12315 vinfos[3].indices[1] = _ij3[1];
12316 vinfos[3].maxsolutions = _nj3;
12317 vinfos[4].jointtype = 1;
12318 vinfos[4].foffset = j4;
12319 vinfos[4].indices[0] = _ij4[0];
12320 vinfos[4].indices[1] = _ij4[1];
12321 vinfos[4].maxsolutions = _nj4;
12322 vinfos[5].jointtype = 1;
12323 vinfos[5].foffset = j5;
12324 vinfos[5].indices[0] = _ij5[0];
12325 vinfos[5].indices[1] = _ij5[1];
12326 vinfos[5].maxsolutions = _nj5;
12327 vinfos[6].jointtype = 1;
12328 vinfos[6].foffset = j6;
12329 vinfos[6].indices[0] = _ij6[0];
12330 vinfos[6].indices[1] = _ij6[1];
12331 vinfos[6].maxsolutions = _nj6;
12332 std::vector<int> vfree(0);
12333 solutions.AddSolution(vinfos,vfree);
12334 }
12335 }
12336 }
12337 
12338 }
12339 } while(0);
12340 if( bgotonextstatement )
12341 {
12342 bool bgotonextstatement = true;
12343 do
12344 {
12345 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12346 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12347 {
12348 bgotonextstatement=false;
12349 {
12350 IkReal j4eval[1];
12351 CheckValue<IkReal> x609 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12352 if(!x609.valid){
12353 continue;
12354 }
12355 IkReal x608=((1.0)*(x609.value));
12356 sj5=0;
12357 cj5=-1.0;
12358 j5=3.14159265358979;
12359 sj6=gconst28;
12360 cj6=gconst29;
12361 j6=((3.14159265)+(((-1.0)*x608)));
12362 new_r11=0;
12363 new_r00=0;
12364 IkReal gconst27=((3.14159265358979)+(((-1.0)*x608)));
12365 IkReal x610 = new_r01*new_r01;
12366 if(IKabs(x610)==0){
12367 continue;
12368 }
12369 IkReal gconst28=((1.0)*new_r01*(pow(x610,-0.5)));
12370 IkReal gconst29=0;
12371 j4eval[0]=new_r01;
12372 if( IKabs(j4eval[0]) < 0.0000010000000000 )
12373 {
12374 {
12375 IkReal j4array[2], cj4array[2], sj4array[2];
12376 bool j4valid[2]={false};
12377 _nj4 = 2;
12378 CheckValue<IkReal> x611=IKPowWithIntegerCheck(gconst28,-1);
12379 if(!x611.valid){
12380 continue;
12381 }
12382 cj4array[0]=((-1.0)*new_r01*(x611.value));
12383 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
12384 {
12385  j4valid[0] = j4valid[1] = true;
12386  j4array[0] = IKacos(cj4array[0]);
12387  sj4array[0] = IKsin(j4array[0]);
12388  cj4array[1] = cj4array[0];
12389  j4array[1] = -j4array[0];
12390  sj4array[1] = -sj4array[0];
12391 }
12392 else if( isnan(cj4array[0]) )
12393 {
12394  // probably any value will work
12395  j4valid[0] = true;
12396  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
12397 }
12398 for(int ij4 = 0; ij4 < 2; ++ij4)
12399 {
12400 if( !j4valid[ij4] )
12401 {
12402  continue;
12403 }
12404 _ij4[0] = ij4; _ij4[1] = -1;
12405 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
12406 {
12407 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12408 {
12409  j4valid[iij4]=false; _ij4[1] = iij4; break;
12410 }
12411 }
12412 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12413 {
12414 IkReal evalcond[6];
12415 IkReal x612=IKsin(j4);
12416 IkReal x613=IKcos(j4);
12417 IkReal x614=((-1.0)*x612);
12418 evalcond[0]=(new_r10*x612);
12419 evalcond[1]=(gconst28*x614);
12420 evalcond[2]=(new_r01*x614);
12421 evalcond[3]=(gconst28+((new_r01*x613)));
12422 evalcond[4]=(gconst28+((new_r10*x613)));
12423 evalcond[5]=(((gconst28*x613))+new_r10);
12424 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12425 {
12426 continue;
12427 }
12428 }
12429 
12430 {
12431 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12432 vinfos[0].jointtype = 17;
12433 vinfos[0].foffset = j0;
12434 vinfos[0].indices[0] = _ij0[0];
12435 vinfos[0].indices[1] = _ij0[1];
12436 vinfos[0].maxsolutions = _nj0;
12437 vinfos[1].jointtype = 1;
12438 vinfos[1].foffset = j1;
12439 vinfos[1].indices[0] = _ij1[0];
12440 vinfos[1].indices[1] = _ij1[1];
12441 vinfos[1].maxsolutions = _nj1;
12442 vinfos[2].jointtype = 1;
12443 vinfos[2].foffset = j2;
12444 vinfos[2].indices[0] = _ij2[0];
12445 vinfos[2].indices[1] = _ij2[1];
12446 vinfos[2].maxsolutions = _nj2;
12447 vinfos[3].jointtype = 1;
12448 vinfos[3].foffset = j3;
12449 vinfos[3].indices[0] = _ij3[0];
12450 vinfos[3].indices[1] = _ij3[1];
12451 vinfos[3].maxsolutions = _nj3;
12452 vinfos[4].jointtype = 1;
12453 vinfos[4].foffset = j4;
12454 vinfos[4].indices[0] = _ij4[0];
12455 vinfos[4].indices[1] = _ij4[1];
12456 vinfos[4].maxsolutions = _nj4;
12457 vinfos[5].jointtype = 1;
12458 vinfos[5].foffset = j5;
12459 vinfos[5].indices[0] = _ij5[0];
12460 vinfos[5].indices[1] = _ij5[1];
12461 vinfos[5].maxsolutions = _nj5;
12462 vinfos[6].jointtype = 1;
12463 vinfos[6].foffset = j6;
12464 vinfos[6].indices[0] = _ij6[0];
12465 vinfos[6].indices[1] = _ij6[1];
12466 vinfos[6].maxsolutions = _nj6;
12467 std::vector<int> vfree(0);
12468 solutions.AddSolution(vinfos,vfree);
12469 }
12470 }
12471 }
12472 
12473 } else
12474 {
12475 {
12476 IkReal j4array[2], cj4array[2], sj4array[2];
12477 bool j4valid[2]={false};
12478 _nj4 = 2;
12479 CheckValue<IkReal> x615=IKPowWithIntegerCheck(new_r01,-1);
12480 if(!x615.valid){
12481 continue;
12482 }
12483 cj4array[0]=((-1.0)*gconst28*(x615.value));
12484 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
12485 {
12486  j4valid[0] = j4valid[1] = true;
12487  j4array[0] = IKacos(cj4array[0]);
12488  sj4array[0] = IKsin(j4array[0]);
12489  cj4array[1] = cj4array[0];
12490  j4array[1] = -j4array[0];
12491  sj4array[1] = -sj4array[0];
12492 }
12493 else if( isnan(cj4array[0]) )
12494 {
12495  // probably any value will work
12496  j4valid[0] = true;
12497  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
12498 }
12499 for(int ij4 = 0; ij4 < 2; ++ij4)
12500 {
12501 if( !j4valid[ij4] )
12502 {
12503  continue;
12504 }
12505 _ij4[0] = ij4; _ij4[1] = -1;
12506 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
12507 {
12508 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12509 {
12510  j4valid[iij4]=false; _ij4[1] = iij4; break;
12511 }
12512 }
12513 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12514 {
12515 IkReal evalcond[6];
12516 IkReal x616=IKsin(j4);
12517 IkReal x617=IKcos(j4);
12518 IkReal x618=(gconst28*x617);
12519 IkReal x619=((-1.0)*x616);
12520 evalcond[0]=(new_r10*x616);
12521 evalcond[1]=(gconst28*x619);
12522 evalcond[2]=(new_r01*x619);
12523 evalcond[3]=(x618+new_r01);
12524 evalcond[4]=(gconst28+((new_r10*x617)));
12525 evalcond[5]=(x618+new_r10);
12526 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12527 {
12528 continue;
12529 }
12530 }
12531 
12532 {
12533 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12534 vinfos[0].jointtype = 17;
12535 vinfos[0].foffset = j0;
12536 vinfos[0].indices[0] = _ij0[0];
12537 vinfos[0].indices[1] = _ij0[1];
12538 vinfos[0].maxsolutions = _nj0;
12539 vinfos[1].jointtype = 1;
12540 vinfos[1].foffset = j1;
12541 vinfos[1].indices[0] = _ij1[0];
12542 vinfos[1].indices[1] = _ij1[1];
12543 vinfos[1].maxsolutions = _nj1;
12544 vinfos[2].jointtype = 1;
12545 vinfos[2].foffset = j2;
12546 vinfos[2].indices[0] = _ij2[0];
12547 vinfos[2].indices[1] = _ij2[1];
12548 vinfos[2].maxsolutions = _nj2;
12549 vinfos[3].jointtype = 1;
12550 vinfos[3].foffset = j3;
12551 vinfos[3].indices[0] = _ij3[0];
12552 vinfos[3].indices[1] = _ij3[1];
12553 vinfos[3].maxsolutions = _nj3;
12554 vinfos[4].jointtype = 1;
12555 vinfos[4].foffset = j4;
12556 vinfos[4].indices[0] = _ij4[0];
12557 vinfos[4].indices[1] = _ij4[1];
12558 vinfos[4].maxsolutions = _nj4;
12559 vinfos[5].jointtype = 1;
12560 vinfos[5].foffset = j5;
12561 vinfos[5].indices[0] = _ij5[0];
12562 vinfos[5].indices[1] = _ij5[1];
12563 vinfos[5].maxsolutions = _nj5;
12564 vinfos[6].jointtype = 1;
12565 vinfos[6].foffset = j6;
12566 vinfos[6].indices[0] = _ij6[0];
12567 vinfos[6].indices[1] = _ij6[1];
12568 vinfos[6].maxsolutions = _nj6;
12569 std::vector<int> vfree(0);
12570 solutions.AddSolution(vinfos,vfree);
12571 }
12572 }
12573 }
12574 
12575 }
12576 
12577 }
12578 
12579 }
12580 } while(0);
12581 if( bgotonextstatement )
12582 {
12583 bool bgotonextstatement = true;
12584 do
12585 {
12586 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
12587 evalcond[1]=gconst28;
12588 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
12589 {
12590 bgotonextstatement=false;
12591 {
12592 IkReal j4eval[4];
12593 CheckValue<IkReal> x621 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12594 if(!x621.valid){
12595 continue;
12596 }
12597 IkReal x620=((1.0)*(x621.value));
12598 sj5=0;
12599 cj5=-1.0;
12600 j5=3.14159265358979;
12601 sj6=gconst28;
12602 cj6=gconst29;
12603 j6=((3.14159265)+(((-1.0)*x620)));
12604 new_r00=0;
12605 new_r10=0;
12606 new_r21=0;
12607 new_r22=0;
12608 IkReal gconst27=((3.14159265358979)+(((-1.0)*x620)));
12609 IkReal gconst28=((1.0)*new_r01);
12610 IkReal gconst29=((-1.0)*new_r11);
12611 j4eval[0]=1.0;
12612 j4eval[1]=1.0;
12613 j4eval[2]=new_r01;
12614 j4eval[3]=1.0;
12615 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 || IKabs(j4eval[3]) < 0.0000010000000000 )
12616 {
12617 {
12618 IkReal j4eval[3];
12619 CheckValue<IkReal> x623 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12620 if(!x623.valid){
12621 continue;
12622 }
12623 IkReal x622=((1.0)*(x623.value));
12624 sj5=0;
12625 cj5=-1.0;
12626 j5=3.14159265358979;
12627 sj6=gconst28;
12628 cj6=gconst29;
12629 j6=((3.14159265)+(((-1.0)*x622)));
12630 new_r00=0;
12631 new_r10=0;
12632 new_r21=0;
12633 new_r22=0;
12634 IkReal gconst27=((3.14159265358979)+(((-1.0)*x622)));
12635 IkReal gconst28=((1.0)*new_r01);
12636 IkReal gconst29=((-1.0)*new_r11);
12637 j4eval[0]=-1.0;
12638 j4eval[1]=-1.0;
12639 j4eval[2]=((IKabs(((-1.0)+(new_r01*new_r01))))+(IKabs(((1.0)*new_r01*new_r11))));
12640 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
12641 {
12642 {
12643 IkReal j4eval[3];
12644 CheckValue<IkReal> x625 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12645 if(!x625.valid){
12646 continue;
12647 }
12648 IkReal x624=((1.0)*(x625.value));
12649 sj5=0;
12650 cj5=-1.0;
12651 j5=3.14159265358979;
12652 sj6=gconst28;
12653 cj6=gconst29;
12654 j6=((3.14159265)+(((-1.0)*x624)));
12655 new_r00=0;
12656 new_r10=0;
12657 new_r21=0;
12658 new_r22=0;
12659 IkReal gconst27=((3.14159265358979)+(((-1.0)*x624)));
12660 IkReal gconst28=((1.0)*new_r01);
12661 IkReal gconst29=((-1.0)*new_r11);
12662 j4eval[0]=1.0;
12663 j4eval[1]=1.0;
12664 j4eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((1.0)+(((-2.0)*(new_r01*new_r01)))))));
12665 if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 )
12666 {
12667 continue; // 3 cases reached
12668 
12669 } else
12670 {
12671 {
12672 IkReal j4array[1], cj4array[1], sj4array[1];
12673 bool j4valid[1]={false};
12674 _nj4 = 1;
12675 IkReal x626=((1.0)*gconst28);
12676 CheckValue<IkReal> x627 = IKatan2WithCheck(IkReal((((gconst29*new_r01))+(((-1.0)*new_r11*x626)))),IkReal(((((-1.0)*new_r01*x626))+(((-1.0)*gconst29*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
12677 if(!x627.valid){
12678 continue;
12679 }
12680 CheckValue<IkReal> x628=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
12681 if(!x628.valid){
12682 continue;
12683 }
12684 j4array[0]=((-1.5707963267949)+(x627.value)+(((1.5707963267949)*(x628.value))));
12685 sj4array[0]=IKsin(j4array[0]);
12686 cj4array[0]=IKcos(j4array[0]);
12687 if( j4array[0] > IKPI )
12688 {
12689  j4array[0]-=IK2PI;
12690 }
12691 else if( j4array[0] < -IKPI )
12692 { j4array[0]+=IK2PI;
12693 }
12694 j4valid[0] = true;
12695 for(int ij4 = 0; ij4 < 1; ++ij4)
12696 {
12697 if( !j4valid[ij4] )
12698 {
12699  continue;
12700 }
12701 _ij4[0] = ij4; _ij4[1] = -1;
12702 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
12703 {
12704 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12705 {
12706  j4valid[iij4]=false; _ij4[1] = iij4; break;
12707 }
12708 }
12709 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12710 {
12711 IkReal evalcond[6];
12712 IkReal x629=IKsin(j4);
12713 IkReal x630=IKcos(j4);
12714 IkReal x631=(gconst28*x630);
12715 IkReal x632=((1.0)*x629);
12716 IkReal x633=(gconst29*x630);
12717 IkReal x634=(gconst29*x632);
12718 evalcond[0]=(gconst28+((new_r01*x630))+((new_r11*x629)));
12719 evalcond[1]=(x633+((gconst28*x629))+new_r11);
12720 evalcond[2]=((((-1.0)*x634))+x631);
12721 evalcond[3]=((((-1.0)*x634))+x631+new_r01);
12722 evalcond[4]=((((-1.0)*gconst28*x632))+(((-1.0)*x633)));
12723 evalcond[5]=((((-1.0)*new_r01*x632))+gconst29+((new_r11*x630)));
12724 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12725 {
12726 continue;
12727 }
12728 }
12729 
12730 {
12731 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12732 vinfos[0].jointtype = 17;
12733 vinfos[0].foffset = j0;
12734 vinfos[0].indices[0] = _ij0[0];
12735 vinfos[0].indices[1] = _ij0[1];
12736 vinfos[0].maxsolutions = _nj0;
12737 vinfos[1].jointtype = 1;
12738 vinfos[1].foffset = j1;
12739 vinfos[1].indices[0] = _ij1[0];
12740 vinfos[1].indices[1] = _ij1[1];
12741 vinfos[1].maxsolutions = _nj1;
12742 vinfos[2].jointtype = 1;
12743 vinfos[2].foffset = j2;
12744 vinfos[2].indices[0] = _ij2[0];
12745 vinfos[2].indices[1] = _ij2[1];
12746 vinfos[2].maxsolutions = _nj2;
12747 vinfos[3].jointtype = 1;
12748 vinfos[3].foffset = j3;
12749 vinfos[3].indices[0] = _ij3[0];
12750 vinfos[3].indices[1] = _ij3[1];
12751 vinfos[3].maxsolutions = _nj3;
12752 vinfos[4].jointtype = 1;
12753 vinfos[4].foffset = j4;
12754 vinfos[4].indices[0] = _ij4[0];
12755 vinfos[4].indices[1] = _ij4[1];
12756 vinfos[4].maxsolutions = _nj4;
12757 vinfos[5].jointtype = 1;
12758 vinfos[5].foffset = j5;
12759 vinfos[5].indices[0] = _ij5[0];
12760 vinfos[5].indices[1] = _ij5[1];
12761 vinfos[5].maxsolutions = _nj5;
12762 vinfos[6].jointtype = 1;
12763 vinfos[6].foffset = j6;
12764 vinfos[6].indices[0] = _ij6[0];
12765 vinfos[6].indices[1] = _ij6[1];
12766 vinfos[6].maxsolutions = _nj6;
12767 std::vector<int> vfree(0);
12768 solutions.AddSolution(vinfos,vfree);
12769 }
12770 }
12771 }
12772 
12773 }
12774 
12775 }
12776 
12777 } else
12778 {
12779 {
12780 IkReal j4array[1], cj4array[1], sj4array[1];
12781 bool j4valid[1]={false};
12782 _nj4 = 1;
12783 CheckValue<IkReal> x635=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst28*gconst28)))+(((-1.0)*(gconst29*gconst29))))),-1);
12784 if(!x635.valid){
12785 continue;
12786 }
12787 CheckValue<IkReal> x636 = IKatan2WithCheck(IkReal((gconst28*new_r11)),IkReal((gconst29*new_r11)),IKFAST_ATAN2_MAGTHRESH);
12788 if(!x636.valid){
12789 continue;
12790 }
12791 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x635.value)))+(x636.value));
12792 sj4array[0]=IKsin(j4array[0]);
12793 cj4array[0]=IKcos(j4array[0]);
12794 if( j4array[0] > IKPI )
12795 {
12796  j4array[0]-=IK2PI;
12797 }
12798 else if( j4array[0] < -IKPI )
12799 { j4array[0]+=IK2PI;
12800 }
12801 j4valid[0] = true;
12802 for(int ij4 = 0; ij4 < 1; ++ij4)
12803 {
12804 if( !j4valid[ij4] )
12805 {
12806  continue;
12807 }
12808 _ij4[0] = ij4; _ij4[1] = -1;
12809 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
12810 {
12811 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12812 {
12813  j4valid[iij4]=false; _ij4[1] = iij4; break;
12814 }
12815 }
12816 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12817 {
12818 IkReal evalcond[6];
12819 IkReal x637=IKsin(j4);
12820 IkReal x638=IKcos(j4);
12821 IkReal x639=(gconst28*x638);
12822 IkReal x640=((1.0)*x637);
12823 IkReal x641=(gconst29*x638);
12824 IkReal x642=(gconst29*x640);
12825 evalcond[0]=(gconst28+((new_r01*x638))+((new_r11*x637)));
12826 evalcond[1]=(((gconst28*x637))+x641+new_r11);
12827 evalcond[2]=((((-1.0)*x642))+x639);
12828 evalcond[3]=((((-1.0)*x642))+x639+new_r01);
12829 evalcond[4]=((((-1.0)*x641))+(((-1.0)*gconst28*x640)));
12830 evalcond[5]=(gconst29+(((-1.0)*new_r01*x640))+((new_r11*x638)));
12831 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12832 {
12833 continue;
12834 }
12835 }
12836 
12837 {
12838 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12839 vinfos[0].jointtype = 17;
12840 vinfos[0].foffset = j0;
12841 vinfos[0].indices[0] = _ij0[0];
12842 vinfos[0].indices[1] = _ij0[1];
12843 vinfos[0].maxsolutions = _nj0;
12844 vinfos[1].jointtype = 1;
12845 vinfos[1].foffset = j1;
12846 vinfos[1].indices[0] = _ij1[0];
12847 vinfos[1].indices[1] = _ij1[1];
12848 vinfos[1].maxsolutions = _nj1;
12849 vinfos[2].jointtype = 1;
12850 vinfos[2].foffset = j2;
12851 vinfos[2].indices[0] = _ij2[0];
12852 vinfos[2].indices[1] = _ij2[1];
12853 vinfos[2].maxsolutions = _nj2;
12854 vinfos[3].jointtype = 1;
12855 vinfos[3].foffset = j3;
12856 vinfos[3].indices[0] = _ij3[0];
12857 vinfos[3].indices[1] = _ij3[1];
12858 vinfos[3].maxsolutions = _nj3;
12859 vinfos[4].jointtype = 1;
12860 vinfos[4].foffset = j4;
12861 vinfos[4].indices[0] = _ij4[0];
12862 vinfos[4].indices[1] = _ij4[1];
12863 vinfos[4].maxsolutions = _nj4;
12864 vinfos[5].jointtype = 1;
12865 vinfos[5].foffset = j5;
12866 vinfos[5].indices[0] = _ij5[0];
12867 vinfos[5].indices[1] = _ij5[1];
12868 vinfos[5].maxsolutions = _nj5;
12869 vinfos[6].jointtype = 1;
12870 vinfos[6].foffset = j6;
12871 vinfos[6].indices[0] = _ij6[0];
12872 vinfos[6].indices[1] = _ij6[1];
12873 vinfos[6].maxsolutions = _nj6;
12874 std::vector<int> vfree(0);
12875 solutions.AddSolution(vinfos,vfree);
12876 }
12877 }
12878 }
12879 
12880 }
12881 
12882 }
12883 
12884 } else
12885 {
12886 {
12887 IkReal j4array[1], cj4array[1], sj4array[1];
12888 bool j4valid[1]={false};
12889 _nj4 = 1;
12890 CheckValue<IkReal> x643=IKPowWithIntegerCheck(IKsign((((gconst28*new_r01))+(((-1.0)*gconst29*new_r11)))),-1);
12891 if(!x643.valid){
12892 continue;
12893 }
12894 CheckValue<IkReal> x644 = IKatan2WithCheck(IkReal((gconst28*gconst29)),IkReal(((-1.0)*(gconst28*gconst28))),IKFAST_ATAN2_MAGTHRESH);
12895 if(!x644.valid){
12896 continue;
12897 }
12898 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x643.value)))+(x644.value));
12899 sj4array[0]=IKsin(j4array[0]);
12900 cj4array[0]=IKcos(j4array[0]);
12901 if( j4array[0] > IKPI )
12902 {
12903  j4array[0]-=IK2PI;
12904 }
12905 else if( j4array[0] < -IKPI )
12906 { j4array[0]+=IK2PI;
12907 }
12908 j4valid[0] = true;
12909 for(int ij4 = 0; ij4 < 1; ++ij4)
12910 {
12911 if( !j4valid[ij4] )
12912 {
12913  continue;
12914 }
12915 _ij4[0] = ij4; _ij4[1] = -1;
12916 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
12917 {
12918 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
12919 {
12920  j4valid[iij4]=false; _ij4[1] = iij4; break;
12921 }
12922 }
12923 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
12924 {
12925 IkReal evalcond[6];
12926 IkReal x645=IKsin(j4);
12927 IkReal x646=IKcos(j4);
12928 IkReal x647=(gconst28*x646);
12929 IkReal x648=((1.0)*x645);
12930 IkReal x649=(gconst29*x646);
12931 IkReal x650=(gconst29*x648);
12932 evalcond[0]=(gconst28+((new_r11*x645))+((new_r01*x646)));
12933 evalcond[1]=(((gconst28*x645))+x649+new_r11);
12934 evalcond[2]=((((-1.0)*x650))+x647);
12935 evalcond[3]=((((-1.0)*x650))+x647+new_r01);
12936 evalcond[4]=((((-1.0)*x649))+(((-1.0)*gconst28*x648)));
12937 evalcond[5]=(gconst29+((new_r11*x646))+(((-1.0)*new_r01*x648)));
12938 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12939 {
12940 continue;
12941 }
12942 }
12943 
12944 {
12945 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12946 vinfos[0].jointtype = 17;
12947 vinfos[0].foffset = j0;
12948 vinfos[0].indices[0] = _ij0[0];
12949 vinfos[0].indices[1] = _ij0[1];
12950 vinfos[0].maxsolutions = _nj0;
12951 vinfos[1].jointtype = 1;
12952 vinfos[1].foffset = j1;
12953 vinfos[1].indices[0] = _ij1[0];
12954 vinfos[1].indices[1] = _ij1[1];
12955 vinfos[1].maxsolutions = _nj1;
12956 vinfos[2].jointtype = 1;
12957 vinfos[2].foffset = j2;
12958 vinfos[2].indices[0] = _ij2[0];
12959 vinfos[2].indices[1] = _ij2[1];
12960 vinfos[2].maxsolutions = _nj2;
12961 vinfos[3].jointtype = 1;
12962 vinfos[3].foffset = j3;
12963 vinfos[3].indices[0] = _ij3[0];
12964 vinfos[3].indices[1] = _ij3[1];
12965 vinfos[3].maxsolutions = _nj3;
12966 vinfos[4].jointtype = 1;
12967 vinfos[4].foffset = j4;
12968 vinfos[4].indices[0] = _ij4[0];
12969 vinfos[4].indices[1] = _ij4[1];
12970 vinfos[4].maxsolutions = _nj4;
12971 vinfos[5].jointtype = 1;
12972 vinfos[5].foffset = j5;
12973 vinfos[5].indices[0] = _ij5[0];
12974 vinfos[5].indices[1] = _ij5[1];
12975 vinfos[5].maxsolutions = _nj5;
12976 vinfos[6].jointtype = 1;
12977 vinfos[6].foffset = j6;
12978 vinfos[6].indices[0] = _ij6[0];
12979 vinfos[6].indices[1] = _ij6[1];
12980 vinfos[6].maxsolutions = _nj6;
12981 std::vector<int> vfree(0);
12982 solutions.AddSolution(vinfos,vfree);
12983 }
12984 }
12985 }
12986 
12987 }
12988 
12989 }
12990 
12991 }
12992 } while(0);
12993 if( bgotonextstatement )
12994 {
12995 bool bgotonextstatement = true;
12996 do
12997 {
12998 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12999 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13000 {
13001 bgotonextstatement=false;
13002 {
13003 IkReal j4array[2], cj4array[2], sj4array[2];
13004 bool j4valid[2]={false};
13005 _nj4 = 2;
13006 CheckValue<IkReal> x651=IKPowWithIntegerCheck(gconst29,-1);
13007 if(!x651.valid){
13008 continue;
13009 }
13010 cj4array[0]=(new_r00*(x651.value));
13011 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
13012 {
13013  j4valid[0] = j4valid[1] = true;
13014  j4array[0] = IKacos(cj4array[0]);
13015  sj4array[0] = IKsin(j4array[0]);
13016  cj4array[1] = cj4array[0];
13017  j4array[1] = -j4array[0];
13018  sj4array[1] = -sj4array[0];
13019 }
13020 else if( isnan(cj4array[0]) )
13021 {
13022  // probably any value will work
13023  j4valid[0] = true;
13024  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
13025 }
13026 for(int ij4 = 0; ij4 < 2; ++ij4)
13027 {
13028 if( !j4valid[ij4] )
13029 {
13030  continue;
13031 }
13032 _ij4[0] = ij4; _ij4[1] = -1;
13033 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
13034 {
13035 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13036 {
13037  j4valid[iij4]=false; _ij4[1] = iij4; break;
13038 }
13039 }
13040 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13041 {
13042 IkReal evalcond[6];
13043 IkReal x652=IKsin(j4);
13044 IkReal x653=IKcos(j4);
13045 IkReal x654=((-1.0)*x652);
13046 evalcond[0]=(new_r11*x652);
13047 evalcond[1]=(gconst29*x654);
13048 evalcond[2]=(new_r00*x654);
13049 evalcond[3]=(((gconst29*x653))+new_r11);
13050 evalcond[4]=(gconst29+((new_r11*x653)));
13051 evalcond[5]=(((new_r00*x653))+(((-1.0)*gconst29)));
13052 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
13053 {
13054 continue;
13055 }
13056 }
13057 
13058 {
13059 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13060 vinfos[0].jointtype = 17;
13061 vinfos[0].foffset = j0;
13062 vinfos[0].indices[0] = _ij0[0];
13063 vinfos[0].indices[1] = _ij0[1];
13064 vinfos[0].maxsolutions = _nj0;
13065 vinfos[1].jointtype = 1;
13066 vinfos[1].foffset = j1;
13067 vinfos[1].indices[0] = _ij1[0];
13068 vinfos[1].indices[1] = _ij1[1];
13069 vinfos[1].maxsolutions = _nj1;
13070 vinfos[2].jointtype = 1;
13071 vinfos[2].foffset = j2;
13072 vinfos[2].indices[0] = _ij2[0];
13073 vinfos[2].indices[1] = _ij2[1];
13074 vinfos[2].maxsolutions = _nj2;
13075 vinfos[3].jointtype = 1;
13076 vinfos[3].foffset = j3;
13077 vinfos[3].indices[0] = _ij3[0];
13078 vinfos[3].indices[1] = _ij3[1];
13079 vinfos[3].maxsolutions = _nj3;
13080 vinfos[4].jointtype = 1;
13081 vinfos[4].foffset = j4;
13082 vinfos[4].indices[0] = _ij4[0];
13083 vinfos[4].indices[1] = _ij4[1];
13084 vinfos[4].maxsolutions = _nj4;
13085 vinfos[5].jointtype = 1;
13086 vinfos[5].foffset = j5;
13087 vinfos[5].indices[0] = _ij5[0];
13088 vinfos[5].indices[1] = _ij5[1];
13089 vinfos[5].maxsolutions = _nj5;
13090 vinfos[6].jointtype = 1;
13091 vinfos[6].foffset = j6;
13092 vinfos[6].indices[0] = _ij6[0];
13093 vinfos[6].indices[1] = _ij6[1];
13094 vinfos[6].maxsolutions = _nj6;
13095 std::vector<int> vfree(0);
13096 solutions.AddSolution(vinfos,vfree);
13097 }
13098 }
13099 }
13100 
13101 }
13102 } while(0);
13103 if( bgotonextstatement )
13104 {
13105 bool bgotonextstatement = true;
13106 do
13107 {
13108 evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01)));
13109 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13110 {
13111 bgotonextstatement=false;
13112 {
13113 IkReal j4eval[1];
13114 CheckValue<IkReal> x656 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13115 if(!x656.valid){
13116 continue;
13117 }
13118 IkReal x655=((1.0)*(x656.value));
13119 sj5=0;
13120 cj5=-1.0;
13121 j5=3.14159265358979;
13122 sj6=gconst28;
13123 cj6=gconst29;
13124 j6=((3.14159265)+(((-1.0)*x655)));
13125 new_r00=0;
13126 new_r01=0;
13127 new_r12=0;
13128 new_r22=0;
13129 IkReal gconst27=((3.14159265358979)+(((-1.0)*x655)));
13130 IkReal gconst28=0;
13131 IkReal x657 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
13132 if(IKabs(x657)==0){
13133 continue;
13134 }
13135 IkReal gconst29=((-1.0)*new_r11*(pow(x657,-0.5)));
13136 j4eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13137 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13138 {
13139 {
13140 IkReal j4eval[1];
13141 CheckValue<IkReal> x659 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13142 if(!x659.valid){
13143 continue;
13144 }
13145 IkReal x658=((1.0)*(x659.value));
13146 sj5=0;
13147 cj5=-1.0;
13148 j5=3.14159265358979;
13149 sj6=gconst28;
13150 cj6=gconst29;
13151 j6=((3.14159265)+(((-1.0)*x658)));
13152 new_r00=0;
13153 new_r01=0;
13154 new_r12=0;
13155 new_r22=0;
13156 IkReal gconst27=((3.14159265358979)+(((-1.0)*x658)));
13157 IkReal gconst28=0;
13158 IkReal x660 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
13159 if(IKabs(x660)==0){
13160 continue;
13161 }
13162 IkReal gconst29=((-1.0)*new_r11*(pow(x660,-0.5)));
13163 j4eval[0]=new_r11;
13164 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13165 {
13166 {
13167 IkReal j4eval[1];
13168 CheckValue<IkReal> x662 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13169 if(!x662.valid){
13170 continue;
13171 }
13172 IkReal x661=((1.0)*(x662.value));
13173 sj5=0;
13174 cj5=-1.0;
13175 j5=3.14159265358979;
13176 sj6=gconst28;
13177 cj6=gconst29;
13178 j6=((3.14159265)+(((-1.0)*x661)));
13179 new_r00=0;
13180 new_r01=0;
13181 new_r12=0;
13182 new_r22=0;
13183 IkReal gconst27=((3.14159265358979)+(((-1.0)*x661)));
13184 IkReal gconst28=0;
13185 IkReal x663 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
13186 if(IKabs(x663)==0){
13187 continue;
13188 }
13189 IkReal gconst29=((-1.0)*new_r11*(pow(x663,-0.5)));
13190 j4eval[0]=new_r10;
13191 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13192 {
13193 continue; // 3 cases reached
13194 
13195 } else
13196 {
13197 {
13198 IkReal j4array[1], cj4array[1], sj4array[1];
13199 bool j4valid[1]={false};
13200 _nj4 = 1;
13201 CheckValue<IkReal> x664=IKPowWithIntegerCheck(new_r10,-1);
13202 if(!x664.valid){
13203 continue;
13204 }
13205 CheckValue<IkReal> x665=IKPowWithIntegerCheck(gconst29,-1);
13206 if(!x665.valid){
13207 continue;
13208 }
13209 if( IKabs((gconst29*(x664.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x665.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst29*(x664.value)))+IKsqr(((-1.0)*new_r11*(x665.value)))-1) <= IKFAST_SINCOS_THRESH )
13210  continue;
13211 j4array[0]=IKatan2((gconst29*(x664.value)), ((-1.0)*new_r11*(x665.value)));
13212 sj4array[0]=IKsin(j4array[0]);
13213 cj4array[0]=IKcos(j4array[0]);
13214 if( j4array[0] > IKPI )
13215 {
13216  j4array[0]-=IK2PI;
13217 }
13218 else if( j4array[0] < -IKPI )
13219 { j4array[0]+=IK2PI;
13220 }
13221 j4valid[0] = true;
13222 for(int ij4 = 0; ij4 < 1; ++ij4)
13223 {
13224 if( !j4valid[ij4] )
13225 {
13226  continue;
13227 }
13228 _ij4[0] = ij4; _ij4[1] = -1;
13229 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13230 {
13231 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13232 {
13233  j4valid[iij4]=false; _ij4[1] = iij4; break;
13234 }
13235 }
13236 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13237 {
13238 IkReal evalcond[8];
13239 IkReal x666=IKsin(j4);
13240 IkReal x667=IKcos(j4);
13241 IkReal x668=((1.0)*gconst29);
13242 IkReal x669=((-1.0)*gconst29);
13243 evalcond[0]=(new_r11*x666);
13244 evalcond[1]=(new_r10*x667);
13245 evalcond[2]=(x666*x669);
13246 evalcond[3]=(x667*x669);
13247 evalcond[4]=(((gconst29*x667))+new_r11);
13248 evalcond[5]=(gconst29+((new_r11*x667)));
13249 evalcond[6]=((((-1.0)*x666*x668))+new_r10);
13250 evalcond[7]=(((new_r10*x666))+(((-1.0)*x668)));
13251 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13252 {
13253 continue;
13254 }
13255 }
13256 
13257 {
13258 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13259 vinfos[0].jointtype = 17;
13260 vinfos[0].foffset = j0;
13261 vinfos[0].indices[0] = _ij0[0];
13262 vinfos[0].indices[1] = _ij0[1];
13263 vinfos[0].maxsolutions = _nj0;
13264 vinfos[1].jointtype = 1;
13265 vinfos[1].foffset = j1;
13266 vinfos[1].indices[0] = _ij1[0];
13267 vinfos[1].indices[1] = _ij1[1];
13268 vinfos[1].maxsolutions = _nj1;
13269 vinfos[2].jointtype = 1;
13270 vinfos[2].foffset = j2;
13271 vinfos[2].indices[0] = _ij2[0];
13272 vinfos[2].indices[1] = _ij2[1];
13273 vinfos[2].maxsolutions = _nj2;
13274 vinfos[3].jointtype = 1;
13275 vinfos[3].foffset = j3;
13276 vinfos[3].indices[0] = _ij3[0];
13277 vinfos[3].indices[1] = _ij3[1];
13278 vinfos[3].maxsolutions = _nj3;
13279 vinfos[4].jointtype = 1;
13280 vinfos[4].foffset = j4;
13281 vinfos[4].indices[0] = _ij4[0];
13282 vinfos[4].indices[1] = _ij4[1];
13283 vinfos[4].maxsolutions = _nj4;
13284 vinfos[5].jointtype = 1;
13285 vinfos[5].foffset = j5;
13286 vinfos[5].indices[0] = _ij5[0];
13287 vinfos[5].indices[1] = _ij5[1];
13288 vinfos[5].maxsolutions = _nj5;
13289 vinfos[6].jointtype = 1;
13290 vinfos[6].foffset = j6;
13291 vinfos[6].indices[0] = _ij6[0];
13292 vinfos[6].indices[1] = _ij6[1];
13293 vinfos[6].maxsolutions = _nj6;
13294 std::vector<int> vfree(0);
13295 solutions.AddSolution(vinfos,vfree);
13296 }
13297 }
13298 }
13299 
13300 }
13301 
13302 }
13303 
13304 } else
13305 {
13306 {
13307 IkReal j4array[1], cj4array[1], sj4array[1];
13308 bool j4valid[1]={false};
13309 _nj4 = 1;
13310 CheckValue<IkReal> x670=IKPowWithIntegerCheck(gconst29,-1);
13311 if(!x670.valid){
13312 continue;
13313 }
13314 CheckValue<IkReal> x671=IKPowWithIntegerCheck(new_r11,-1);
13315 if(!x671.valid){
13316 continue;
13317 }
13318 if( IKabs((new_r10*(x670.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst29*(x671.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x670.value)))+IKsqr(((-1.0)*gconst29*(x671.value)))-1) <= IKFAST_SINCOS_THRESH )
13319  continue;
13320 j4array[0]=IKatan2((new_r10*(x670.value)), ((-1.0)*gconst29*(x671.value)));
13321 sj4array[0]=IKsin(j4array[0]);
13322 cj4array[0]=IKcos(j4array[0]);
13323 if( j4array[0] > IKPI )
13324 {
13325  j4array[0]-=IK2PI;
13326 }
13327 else if( j4array[0] < -IKPI )
13328 { j4array[0]+=IK2PI;
13329 }
13330 j4valid[0] = true;
13331 for(int ij4 = 0; ij4 < 1; ++ij4)
13332 {
13333 if( !j4valid[ij4] )
13334 {
13335  continue;
13336 }
13337 _ij4[0] = ij4; _ij4[1] = -1;
13338 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13339 {
13340 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13341 {
13342  j4valid[iij4]=false; _ij4[1] = iij4; break;
13343 }
13344 }
13345 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13346 {
13347 IkReal evalcond[8];
13348 IkReal x672=IKsin(j4);
13349 IkReal x673=IKcos(j4);
13350 IkReal x674=((1.0)*gconst29);
13351 IkReal x675=((-1.0)*gconst29);
13352 evalcond[0]=(new_r11*x672);
13353 evalcond[1]=(new_r10*x673);
13354 evalcond[2]=(x672*x675);
13355 evalcond[3]=(x673*x675);
13356 evalcond[4]=(((gconst29*x673))+new_r11);
13357 evalcond[5]=(((new_r11*x673))+gconst29);
13358 evalcond[6]=((((-1.0)*x672*x674))+new_r10);
13359 evalcond[7]=(((new_r10*x672))+(((-1.0)*x674)));
13360 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13361 {
13362 continue;
13363 }
13364 }
13365 
13366 {
13367 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13368 vinfos[0].jointtype = 17;
13369 vinfos[0].foffset = j0;
13370 vinfos[0].indices[0] = _ij0[0];
13371 vinfos[0].indices[1] = _ij0[1];
13372 vinfos[0].maxsolutions = _nj0;
13373 vinfos[1].jointtype = 1;
13374 vinfos[1].foffset = j1;
13375 vinfos[1].indices[0] = _ij1[0];
13376 vinfos[1].indices[1] = _ij1[1];
13377 vinfos[1].maxsolutions = _nj1;
13378 vinfos[2].jointtype = 1;
13379 vinfos[2].foffset = j2;
13380 vinfos[2].indices[0] = _ij2[0];
13381 vinfos[2].indices[1] = _ij2[1];
13382 vinfos[2].maxsolutions = _nj2;
13383 vinfos[3].jointtype = 1;
13384 vinfos[3].foffset = j3;
13385 vinfos[3].indices[0] = _ij3[0];
13386 vinfos[3].indices[1] = _ij3[1];
13387 vinfos[3].maxsolutions = _nj3;
13388 vinfos[4].jointtype = 1;
13389 vinfos[4].foffset = j4;
13390 vinfos[4].indices[0] = _ij4[0];
13391 vinfos[4].indices[1] = _ij4[1];
13392 vinfos[4].maxsolutions = _nj4;
13393 vinfos[5].jointtype = 1;
13394 vinfos[5].foffset = j5;
13395 vinfos[5].indices[0] = _ij5[0];
13396 vinfos[5].indices[1] = _ij5[1];
13397 vinfos[5].maxsolutions = _nj5;
13398 vinfos[6].jointtype = 1;
13399 vinfos[6].foffset = j6;
13400 vinfos[6].indices[0] = _ij6[0];
13401 vinfos[6].indices[1] = _ij6[1];
13402 vinfos[6].maxsolutions = _nj6;
13403 std::vector<int> vfree(0);
13404 solutions.AddSolution(vinfos,vfree);
13405 }
13406 }
13407 }
13408 
13409 }
13410 
13411 }
13412 
13413 } else
13414 {
13415 {
13416 IkReal j4array[1], cj4array[1], sj4array[1];
13417 bool j4valid[1]={false};
13418 _nj4 = 1;
13419 CheckValue<IkReal> x676 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
13420 if(!x676.valid){
13421 continue;
13422 }
13423 CheckValue<IkReal> x677=IKPowWithIntegerCheck(IKsign(gconst29),-1);
13424 if(!x677.valid){
13425 continue;
13426 }
13427 j4array[0]=((-1.5707963267949)+(x676.value)+(((1.5707963267949)*(x677.value))));
13428 sj4array[0]=IKsin(j4array[0]);
13429 cj4array[0]=IKcos(j4array[0]);
13430 if( j4array[0] > IKPI )
13431 {
13432  j4array[0]-=IK2PI;
13433 }
13434 else if( j4array[0] < -IKPI )
13435 { j4array[0]+=IK2PI;
13436 }
13437 j4valid[0] = true;
13438 for(int ij4 = 0; ij4 < 1; ++ij4)
13439 {
13440 if( !j4valid[ij4] )
13441 {
13442  continue;
13443 }
13444 _ij4[0] = ij4; _ij4[1] = -1;
13445 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13446 {
13447 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13448 {
13449  j4valid[iij4]=false; _ij4[1] = iij4; break;
13450 }
13451 }
13452 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13453 {
13454 IkReal evalcond[8];
13455 IkReal x678=IKsin(j4);
13456 IkReal x679=IKcos(j4);
13457 IkReal x680=((1.0)*gconst29);
13458 IkReal x681=((-1.0)*gconst29);
13459 evalcond[0]=(new_r11*x678);
13460 evalcond[1]=(new_r10*x679);
13461 evalcond[2]=(x678*x681);
13462 evalcond[3]=(x679*x681);
13463 evalcond[4]=(((gconst29*x679))+new_r11);
13464 evalcond[5]=(((new_r11*x679))+gconst29);
13465 evalcond[6]=((((-1.0)*x678*x680))+new_r10);
13466 evalcond[7]=(((new_r10*x678))+(((-1.0)*x680)));
13467 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13468 {
13469 continue;
13470 }
13471 }
13472 
13473 {
13474 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13475 vinfos[0].jointtype = 17;
13476 vinfos[0].foffset = j0;
13477 vinfos[0].indices[0] = _ij0[0];
13478 vinfos[0].indices[1] = _ij0[1];
13479 vinfos[0].maxsolutions = _nj0;
13480 vinfos[1].jointtype = 1;
13481 vinfos[1].foffset = j1;
13482 vinfos[1].indices[0] = _ij1[0];
13483 vinfos[1].indices[1] = _ij1[1];
13484 vinfos[1].maxsolutions = _nj1;
13485 vinfos[2].jointtype = 1;
13486 vinfos[2].foffset = j2;
13487 vinfos[2].indices[0] = _ij2[0];
13488 vinfos[2].indices[1] = _ij2[1];
13489 vinfos[2].maxsolutions = _nj2;
13490 vinfos[3].jointtype = 1;
13491 vinfos[3].foffset = j3;
13492 vinfos[3].indices[0] = _ij3[0];
13493 vinfos[3].indices[1] = _ij3[1];
13494 vinfos[3].maxsolutions = _nj3;
13495 vinfos[4].jointtype = 1;
13496 vinfos[4].foffset = j4;
13497 vinfos[4].indices[0] = _ij4[0];
13498 vinfos[4].indices[1] = _ij4[1];
13499 vinfos[4].maxsolutions = _nj4;
13500 vinfos[5].jointtype = 1;
13501 vinfos[5].foffset = j5;
13502 vinfos[5].indices[0] = _ij5[0];
13503 vinfos[5].indices[1] = _ij5[1];
13504 vinfos[5].maxsolutions = _nj5;
13505 vinfos[6].jointtype = 1;
13506 vinfos[6].foffset = j6;
13507 vinfos[6].indices[0] = _ij6[0];
13508 vinfos[6].indices[1] = _ij6[1];
13509 vinfos[6].maxsolutions = _nj6;
13510 std::vector<int> vfree(0);
13511 solutions.AddSolution(vinfos,vfree);
13512 }
13513 }
13514 }
13515 
13516 }
13517 
13518 }
13519 
13520 }
13521 } while(0);
13522 if( bgotonextstatement )
13523 {
13524 bool bgotonextstatement = true;
13525 do
13526 {
13527 evalcond[0]=IKabs(new_r01);
13528 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13529 {
13530 bgotonextstatement=false;
13531 {
13532 IkReal j4eval[1];
13533 CheckValue<IkReal> x683 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13534 if(!x683.valid){
13535 continue;
13536 }
13537 IkReal x682=((1.0)*(x683.value));
13538 sj5=0;
13539 cj5=-1.0;
13540 j5=3.14159265358979;
13541 sj6=gconst28;
13542 cj6=gconst29;
13543 j6=((3.14159265)+(((-1.0)*x682)));
13544 new_r01=0;
13545 IkReal gconst27=((3.14159265358979)+(((-1.0)*x682)));
13546 IkReal gconst28=0;
13547 IkReal x684 = new_r11*new_r11;
13548 if(IKabs(x684)==0){
13549 continue;
13550 }
13551 IkReal gconst29=((-1.0)*new_r11*(pow(x684,-0.5)));
13552 j4eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
13553 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13554 {
13555 {
13556 IkReal j4eval[1];
13557 CheckValue<IkReal> x686 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13558 if(!x686.valid){
13559 continue;
13560 }
13561 IkReal x685=((1.0)*(x686.value));
13562 sj5=0;
13563 cj5=-1.0;
13564 j5=3.14159265358979;
13565 sj6=gconst28;
13566 cj6=gconst29;
13567 j6=((3.14159265)+(((-1.0)*x685)));
13568 new_r01=0;
13569 IkReal gconst27=((3.14159265358979)+(((-1.0)*x685)));
13570 IkReal gconst28=0;
13571 IkReal x687 = new_r11*new_r11;
13572 if(IKabs(x687)==0){
13573 continue;
13574 }
13575 IkReal gconst29=((-1.0)*new_r11*(pow(x687,-0.5)));
13576 j4eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13577 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13578 {
13579 {
13580 IkReal j4eval[1];
13581 CheckValue<IkReal> x689 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
13582 if(!x689.valid){
13583 continue;
13584 }
13585 IkReal x688=((1.0)*(x689.value));
13586 sj5=0;
13587 cj5=-1.0;
13588 j5=3.14159265358979;
13589 sj6=gconst28;
13590 cj6=gconst29;
13591 j6=((3.14159265)+(((-1.0)*x688)));
13592 new_r01=0;
13593 IkReal gconst27=((3.14159265358979)+(((-1.0)*x688)));
13594 IkReal gconst28=0;
13595 IkReal x690 = new_r11*new_r11;
13596 if(IKabs(x690)==0){
13597 continue;
13598 }
13599 IkReal gconst29=((-1.0)*new_r11*(pow(x690,-0.5)));
13600 j4eval[0]=new_r11;
13601 if( IKabs(j4eval[0]) < 0.0000010000000000 )
13602 {
13603 continue; // 3 cases reached
13604 
13605 } else
13606 {
13607 {
13608 IkReal j4array[1], cj4array[1], sj4array[1];
13609 bool j4valid[1]={false};
13610 _nj4 = 1;
13611 CheckValue<IkReal> x691=IKPowWithIntegerCheck(gconst29,-1);
13612 if(!x691.valid){
13613 continue;
13614 }
13615 CheckValue<IkReal> x692=IKPowWithIntegerCheck(new_r11,-1);
13616 if(!x692.valid){
13617 continue;
13618 }
13619 if( IKabs((new_r10*(x691.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst29*(x692.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x691.value)))+IKsqr(((-1.0)*gconst29*(x692.value)))-1) <= IKFAST_SINCOS_THRESH )
13620  continue;
13621 j4array[0]=IKatan2((new_r10*(x691.value)), ((-1.0)*gconst29*(x692.value)));
13622 sj4array[0]=IKsin(j4array[0]);
13623 cj4array[0]=IKcos(j4array[0]);
13624 if( j4array[0] > IKPI )
13625 {
13626  j4array[0]-=IK2PI;
13627 }
13628 else if( j4array[0] < -IKPI )
13629 { j4array[0]+=IK2PI;
13630 }
13631 j4valid[0] = true;
13632 for(int ij4 = 0; ij4 < 1; ++ij4)
13633 {
13634 if( !j4valid[ij4] )
13635 {
13636  continue;
13637 }
13638 _ij4[0] = ij4; _ij4[1] = -1;
13639 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13640 {
13641 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13642 {
13643  j4valid[iij4]=false; _ij4[1] = iij4; break;
13644 }
13645 }
13646 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13647 {
13648 IkReal evalcond[8];
13649 IkReal x693=IKsin(j4);
13650 IkReal x694=IKcos(j4);
13651 IkReal x695=((1.0)*gconst29);
13652 evalcond[0]=(new_r11*x693);
13653 evalcond[1]=((-1.0)*gconst29*x693);
13654 evalcond[2]=(new_r11+((gconst29*x694)));
13655 evalcond[3]=(gconst29+((new_r11*x694)));
13656 evalcond[4]=((((-1.0)*x694*x695))+new_r00);
13657 evalcond[5]=((((-1.0)*x693*x695))+new_r10);
13658 evalcond[6]=((((-1.0)*new_r00*x693))+((new_r10*x694)));
13659 evalcond[7]=(((new_r10*x693))+(((-1.0)*x695))+((new_r00*x694)));
13660 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13661 {
13662 continue;
13663 }
13664 }
13665 
13666 {
13667 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13668 vinfos[0].jointtype = 17;
13669 vinfos[0].foffset = j0;
13670 vinfos[0].indices[0] = _ij0[0];
13671 vinfos[0].indices[1] = _ij0[1];
13672 vinfos[0].maxsolutions = _nj0;
13673 vinfos[1].jointtype = 1;
13674 vinfos[1].foffset = j1;
13675 vinfos[1].indices[0] = _ij1[0];
13676 vinfos[1].indices[1] = _ij1[1];
13677 vinfos[1].maxsolutions = _nj1;
13678 vinfos[2].jointtype = 1;
13679 vinfos[2].foffset = j2;
13680 vinfos[2].indices[0] = _ij2[0];
13681 vinfos[2].indices[1] = _ij2[1];
13682 vinfos[2].maxsolutions = _nj2;
13683 vinfos[3].jointtype = 1;
13684 vinfos[3].foffset = j3;
13685 vinfos[3].indices[0] = _ij3[0];
13686 vinfos[3].indices[1] = _ij3[1];
13687 vinfos[3].maxsolutions = _nj3;
13688 vinfos[4].jointtype = 1;
13689 vinfos[4].foffset = j4;
13690 vinfos[4].indices[0] = _ij4[0];
13691 vinfos[4].indices[1] = _ij4[1];
13692 vinfos[4].maxsolutions = _nj4;
13693 vinfos[5].jointtype = 1;
13694 vinfos[5].foffset = j5;
13695 vinfos[5].indices[0] = _ij5[0];
13696 vinfos[5].indices[1] = _ij5[1];
13697 vinfos[5].maxsolutions = _nj5;
13698 vinfos[6].jointtype = 1;
13699 vinfos[6].foffset = j6;
13700 vinfos[6].indices[0] = _ij6[0];
13701 vinfos[6].indices[1] = _ij6[1];
13702 vinfos[6].maxsolutions = _nj6;
13703 std::vector<int> vfree(0);
13704 solutions.AddSolution(vinfos,vfree);
13705 }
13706 }
13707 }
13708 
13709 }
13710 
13711 }
13712 
13713 } else
13714 {
13715 {
13716 IkReal j4array[1], cj4array[1], sj4array[1];
13717 bool j4valid[1]={false};
13718 _nj4 = 1;
13719 CheckValue<IkReal> x696 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
13720 if(!x696.valid){
13721 continue;
13722 }
13723 CheckValue<IkReal> x697=IKPowWithIntegerCheck(IKsign(gconst29),-1);
13724 if(!x697.valid){
13725 continue;
13726 }
13727 j4array[0]=((-1.5707963267949)+(x696.value)+(((1.5707963267949)*(x697.value))));
13728 sj4array[0]=IKsin(j4array[0]);
13729 cj4array[0]=IKcos(j4array[0]);
13730 if( j4array[0] > IKPI )
13731 {
13732  j4array[0]-=IK2PI;
13733 }
13734 else if( j4array[0] < -IKPI )
13735 { j4array[0]+=IK2PI;
13736 }
13737 j4valid[0] = true;
13738 for(int ij4 = 0; ij4 < 1; ++ij4)
13739 {
13740 if( !j4valid[ij4] )
13741 {
13742  continue;
13743 }
13744 _ij4[0] = ij4; _ij4[1] = -1;
13745 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13746 {
13747 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13748 {
13749  j4valid[iij4]=false; _ij4[1] = iij4; break;
13750 }
13751 }
13752 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13753 {
13754 IkReal evalcond[8];
13755 IkReal x698=IKsin(j4);
13756 IkReal x699=IKcos(j4);
13757 IkReal x700=((1.0)*gconst29);
13758 evalcond[0]=(new_r11*x698);
13759 evalcond[1]=((-1.0)*gconst29*x698);
13760 evalcond[2]=(new_r11+((gconst29*x699)));
13761 evalcond[3]=(gconst29+((new_r11*x699)));
13762 evalcond[4]=((((-1.0)*x699*x700))+new_r00);
13763 evalcond[5]=((((-1.0)*x698*x700))+new_r10);
13764 evalcond[6]=((((-1.0)*new_r00*x698))+((new_r10*x699)));
13765 evalcond[7]=(((new_r10*x698))+(((-1.0)*x700))+((new_r00*x699)));
13766 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13767 {
13768 continue;
13769 }
13770 }
13771 
13772 {
13773 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13774 vinfos[0].jointtype = 17;
13775 vinfos[0].foffset = j0;
13776 vinfos[0].indices[0] = _ij0[0];
13777 vinfos[0].indices[1] = _ij0[1];
13778 vinfos[0].maxsolutions = _nj0;
13779 vinfos[1].jointtype = 1;
13780 vinfos[1].foffset = j1;
13781 vinfos[1].indices[0] = _ij1[0];
13782 vinfos[1].indices[1] = _ij1[1];
13783 vinfos[1].maxsolutions = _nj1;
13784 vinfos[2].jointtype = 1;
13785 vinfos[2].foffset = j2;
13786 vinfos[2].indices[0] = _ij2[0];
13787 vinfos[2].indices[1] = _ij2[1];
13788 vinfos[2].maxsolutions = _nj2;
13789 vinfos[3].jointtype = 1;
13790 vinfos[3].foffset = j3;
13791 vinfos[3].indices[0] = _ij3[0];
13792 vinfos[3].indices[1] = _ij3[1];
13793 vinfos[3].maxsolutions = _nj3;
13794 vinfos[4].jointtype = 1;
13795 vinfos[4].foffset = j4;
13796 vinfos[4].indices[0] = _ij4[0];
13797 vinfos[4].indices[1] = _ij4[1];
13798 vinfos[4].maxsolutions = _nj4;
13799 vinfos[5].jointtype = 1;
13800 vinfos[5].foffset = j5;
13801 vinfos[5].indices[0] = _ij5[0];
13802 vinfos[5].indices[1] = _ij5[1];
13803 vinfos[5].maxsolutions = _nj5;
13804 vinfos[6].jointtype = 1;
13805 vinfos[6].foffset = j6;
13806 vinfos[6].indices[0] = _ij6[0];
13807 vinfos[6].indices[1] = _ij6[1];
13808 vinfos[6].maxsolutions = _nj6;
13809 std::vector<int> vfree(0);
13810 solutions.AddSolution(vinfos,vfree);
13811 }
13812 }
13813 }
13814 
13815 }
13816 
13817 }
13818 
13819 } else
13820 {
13821 {
13822 IkReal j4array[1], cj4array[1], sj4array[1];
13823 bool j4valid[1]={false};
13824 _nj4 = 1;
13825 CheckValue<IkReal> x701 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
13826 if(!x701.valid){
13827 continue;
13828 }
13829 CheckValue<IkReal> x702=IKPowWithIntegerCheck(IKsign(gconst29),-1);
13830 if(!x702.valid){
13831 continue;
13832 }
13833 j4array[0]=((-1.5707963267949)+(x701.value)+(((1.5707963267949)*(x702.value))));
13834 sj4array[0]=IKsin(j4array[0]);
13835 cj4array[0]=IKcos(j4array[0]);
13836 if( j4array[0] > IKPI )
13837 {
13838  j4array[0]-=IK2PI;
13839 }
13840 else if( j4array[0] < -IKPI )
13841 { j4array[0]+=IK2PI;
13842 }
13843 j4valid[0] = true;
13844 for(int ij4 = 0; ij4 < 1; ++ij4)
13845 {
13846 if( !j4valid[ij4] )
13847 {
13848  continue;
13849 }
13850 _ij4[0] = ij4; _ij4[1] = -1;
13851 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13852 {
13853 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13854 {
13855  j4valid[iij4]=false; _ij4[1] = iij4; break;
13856 }
13857 }
13858 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13859 {
13860 IkReal evalcond[8];
13861 IkReal x703=IKsin(j4);
13862 IkReal x704=IKcos(j4);
13863 IkReal x705=((1.0)*gconst29);
13864 evalcond[0]=(new_r11*x703);
13865 evalcond[1]=((-1.0)*gconst29*x703);
13866 evalcond[2]=(((gconst29*x704))+new_r11);
13867 evalcond[3]=(gconst29+((new_r11*x704)));
13868 evalcond[4]=((((-1.0)*x704*x705))+new_r00);
13869 evalcond[5]=((((-1.0)*x703*x705))+new_r10);
13870 evalcond[6]=((((-1.0)*new_r00*x703))+((new_r10*x704)));
13871 evalcond[7]=(((new_r00*x704))+(((-1.0)*x705))+((new_r10*x703)));
13872 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13873 {
13874 continue;
13875 }
13876 }
13877 
13878 {
13879 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13880 vinfos[0].jointtype = 17;
13881 vinfos[0].foffset = j0;
13882 vinfos[0].indices[0] = _ij0[0];
13883 vinfos[0].indices[1] = _ij0[1];
13884 vinfos[0].maxsolutions = _nj0;
13885 vinfos[1].jointtype = 1;
13886 vinfos[1].foffset = j1;
13887 vinfos[1].indices[0] = _ij1[0];
13888 vinfos[1].indices[1] = _ij1[1];
13889 vinfos[1].maxsolutions = _nj1;
13890 vinfos[2].jointtype = 1;
13891 vinfos[2].foffset = j2;
13892 vinfos[2].indices[0] = _ij2[0];
13893 vinfos[2].indices[1] = _ij2[1];
13894 vinfos[2].maxsolutions = _nj2;
13895 vinfos[3].jointtype = 1;
13896 vinfos[3].foffset = j3;
13897 vinfos[3].indices[0] = _ij3[0];
13898 vinfos[3].indices[1] = _ij3[1];
13899 vinfos[3].maxsolutions = _nj3;
13900 vinfos[4].jointtype = 1;
13901 vinfos[4].foffset = j4;
13902 vinfos[4].indices[0] = _ij4[0];
13903 vinfos[4].indices[1] = _ij4[1];
13904 vinfos[4].maxsolutions = _nj4;
13905 vinfos[5].jointtype = 1;
13906 vinfos[5].foffset = j5;
13907 vinfos[5].indices[0] = _ij5[0];
13908 vinfos[5].indices[1] = _ij5[1];
13909 vinfos[5].maxsolutions = _nj5;
13910 vinfos[6].jointtype = 1;
13911 vinfos[6].foffset = j6;
13912 vinfos[6].indices[0] = _ij6[0];
13913 vinfos[6].indices[1] = _ij6[1];
13914 vinfos[6].maxsolutions = _nj6;
13915 std::vector<int> vfree(0);
13916 solutions.AddSolution(vinfos,vfree);
13917 }
13918 }
13919 }
13920 
13921 }
13922 
13923 }
13924 
13925 }
13926 } while(0);
13927 if( bgotonextstatement )
13928 {
13929 bool bgotonextstatement = true;
13930 do
13931 {
13932 if( 1 )
13933 {
13934 bgotonextstatement=false;
13935 continue; // branch miss [j4]
13936 
13937 }
13938 } while(0);
13939 if( bgotonextstatement )
13940 {
13941 }
13942 }
13943 }
13944 }
13945 }
13946 }
13947 }
13948 }
13949 
13950 } else
13951 {
13952 {
13953 IkReal j4array[1], cj4array[1], sj4array[1];
13954 bool j4valid[1]={false};
13955 _nj4 = 1;
13956 IkReal x706=((1.0)*gconst28);
13957 CheckValue<IkReal> x707=IKPowWithIntegerCheck(IKsign(((gconst28*gconst28)+(gconst29*gconst29))),-1);
13958 if(!x707.valid){
13959 continue;
13960 }
13961 CheckValue<IkReal> x708 = IKatan2WithCheck(IkReal((((gconst29*new_r01))+(((-1.0)*new_r11*x706)))),IkReal(((((-1.0)*gconst29*new_r11))+(((-1.0)*new_r01*x706)))),IKFAST_ATAN2_MAGTHRESH);
13962 if(!x708.valid){
13963 continue;
13964 }
13965 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x707.value)))+(x708.value));
13966 sj4array[0]=IKsin(j4array[0]);
13967 cj4array[0]=IKcos(j4array[0]);
13968 if( j4array[0] > IKPI )
13969 {
13970  j4array[0]-=IK2PI;
13971 }
13972 else if( j4array[0] < -IKPI )
13973 { j4array[0]+=IK2PI;
13974 }
13975 j4valid[0] = true;
13976 for(int ij4 = 0; ij4 < 1; ++ij4)
13977 {
13978 if( !j4valid[ij4] )
13979 {
13980  continue;
13981 }
13982 _ij4[0] = ij4; _ij4[1] = -1;
13983 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
13984 {
13985 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
13986 {
13987  j4valid[iij4]=false; _ij4[1] = iij4; break;
13988 }
13989 }
13990 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
13991 {
13992 IkReal evalcond[8];
13993 IkReal x709=IKsin(j4);
13994 IkReal x710=IKcos(j4);
13995 IkReal x711=(gconst28*x710);
13996 IkReal x712=((1.0)*x709);
13997 IkReal x713=(gconst29*x710);
13998 IkReal x714=(gconst29*x712);
13999 evalcond[0]=(((new_r01*x710))+gconst28+((new_r11*x709)));
14000 evalcond[1]=(((gconst28*x709))+x713+new_r11);
14001 evalcond[2]=((((-1.0)*x714))+x711+new_r01);
14002 evalcond[3]=(gconst28+((new_r10*x710))+(((-1.0)*new_r00*x712)));
14003 evalcond[4]=(gconst29+((new_r11*x710))+(((-1.0)*new_r01*x712)));
14004 evalcond[5]=((((-1.0)*x714))+x711+new_r10);
14005 evalcond[6]=(((new_r00*x710))+(((-1.0)*gconst29))+((new_r10*x709)));
14006 evalcond[7]=((((-1.0)*gconst28*x712))+(((-1.0)*x713))+new_r00);
14007 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14008 {
14009 continue;
14010 }
14011 }
14012 
14013 {
14014 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14015 vinfos[0].jointtype = 17;
14016 vinfos[0].foffset = j0;
14017 vinfos[0].indices[0] = _ij0[0];
14018 vinfos[0].indices[1] = _ij0[1];
14019 vinfos[0].maxsolutions = _nj0;
14020 vinfos[1].jointtype = 1;
14021 vinfos[1].foffset = j1;
14022 vinfos[1].indices[0] = _ij1[0];
14023 vinfos[1].indices[1] = _ij1[1];
14024 vinfos[1].maxsolutions = _nj1;
14025 vinfos[2].jointtype = 1;
14026 vinfos[2].foffset = j2;
14027 vinfos[2].indices[0] = _ij2[0];
14028 vinfos[2].indices[1] = _ij2[1];
14029 vinfos[2].maxsolutions = _nj2;
14030 vinfos[3].jointtype = 1;
14031 vinfos[3].foffset = j3;
14032 vinfos[3].indices[0] = _ij3[0];
14033 vinfos[3].indices[1] = _ij3[1];
14034 vinfos[3].maxsolutions = _nj3;
14035 vinfos[4].jointtype = 1;
14036 vinfos[4].foffset = j4;
14037 vinfos[4].indices[0] = _ij4[0];
14038 vinfos[4].indices[1] = _ij4[1];
14039 vinfos[4].maxsolutions = _nj4;
14040 vinfos[5].jointtype = 1;
14041 vinfos[5].foffset = j5;
14042 vinfos[5].indices[0] = _ij5[0];
14043 vinfos[5].indices[1] = _ij5[1];
14044 vinfos[5].maxsolutions = _nj5;
14045 vinfos[6].jointtype = 1;
14046 vinfos[6].foffset = j6;
14047 vinfos[6].indices[0] = _ij6[0];
14048 vinfos[6].indices[1] = _ij6[1];
14049 vinfos[6].maxsolutions = _nj6;
14050 std::vector<int> vfree(0);
14051 solutions.AddSolution(vinfos,vfree);
14052 }
14053 }
14054 }
14055 
14056 }
14057 
14058 }
14059 
14060 } else
14061 {
14062 {
14063 IkReal j4array[1], cj4array[1], sj4array[1];
14064 bool j4valid[1]={false};
14065 _nj4 = 1;
14066 IkReal x715=((1.0)*gconst28);
14067 CheckValue<IkReal> x716 = IKatan2WithCheck(IkReal((((gconst29*new_r01))+(((-1.0)*new_r11*x715)))),IkReal(((((-1.0)*gconst29*new_r11))+(((-1.0)*new_r01*x715)))),IKFAST_ATAN2_MAGTHRESH);
14068 if(!x716.valid){
14069 continue;
14070 }
14071 CheckValue<IkReal> x717=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
14072 if(!x717.valid){
14073 continue;
14074 }
14075 j4array[0]=((-1.5707963267949)+(x716.value)+(((1.5707963267949)*(x717.value))));
14076 sj4array[0]=IKsin(j4array[0]);
14077 cj4array[0]=IKcos(j4array[0]);
14078 if( j4array[0] > IKPI )
14079 {
14080  j4array[0]-=IK2PI;
14081 }
14082 else if( j4array[0] < -IKPI )
14083 { j4array[0]+=IK2PI;
14084 }
14085 j4valid[0] = true;
14086 for(int ij4 = 0; ij4 < 1; ++ij4)
14087 {
14088 if( !j4valid[ij4] )
14089 {
14090  continue;
14091 }
14092 _ij4[0] = ij4; _ij4[1] = -1;
14093 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14094 {
14095 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14096 {
14097  j4valid[iij4]=false; _ij4[1] = iij4; break;
14098 }
14099 }
14100 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14101 {
14102 IkReal evalcond[8];
14103 IkReal x718=IKsin(j4);
14104 IkReal x719=IKcos(j4);
14105 IkReal x720=(gconst28*x719);
14106 IkReal x721=((1.0)*x718);
14107 IkReal x722=(gconst29*x719);
14108 IkReal x723=(gconst29*x721);
14109 evalcond[0]=(((new_r01*x719))+gconst28+((new_r11*x718)));
14110 evalcond[1]=(((gconst28*x718))+x722+new_r11);
14111 evalcond[2]=((((-1.0)*x723))+x720+new_r01);
14112 evalcond[3]=(gconst28+((new_r10*x719))+(((-1.0)*new_r00*x721)));
14113 evalcond[4]=(gconst29+((new_r11*x719))+(((-1.0)*new_r01*x721)));
14114 evalcond[5]=((((-1.0)*x723))+x720+new_r10);
14115 evalcond[6]=(((new_r00*x719))+(((-1.0)*gconst29))+((new_r10*x718)));
14116 evalcond[7]=((((-1.0)*gconst28*x721))+(((-1.0)*x722))+new_r00);
14117 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14118 {
14119 continue;
14120 }
14121 }
14122 
14123 {
14124 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14125 vinfos[0].jointtype = 17;
14126 vinfos[0].foffset = j0;
14127 vinfos[0].indices[0] = _ij0[0];
14128 vinfos[0].indices[1] = _ij0[1];
14129 vinfos[0].maxsolutions = _nj0;
14130 vinfos[1].jointtype = 1;
14131 vinfos[1].foffset = j1;
14132 vinfos[1].indices[0] = _ij1[0];
14133 vinfos[1].indices[1] = _ij1[1];
14134 vinfos[1].maxsolutions = _nj1;
14135 vinfos[2].jointtype = 1;
14136 vinfos[2].foffset = j2;
14137 vinfos[2].indices[0] = _ij2[0];
14138 vinfos[2].indices[1] = _ij2[1];
14139 vinfos[2].maxsolutions = _nj2;
14140 vinfos[3].jointtype = 1;
14141 vinfos[3].foffset = j3;
14142 vinfos[3].indices[0] = _ij3[0];
14143 vinfos[3].indices[1] = _ij3[1];
14144 vinfos[3].maxsolutions = _nj3;
14145 vinfos[4].jointtype = 1;
14146 vinfos[4].foffset = j4;
14147 vinfos[4].indices[0] = _ij4[0];
14148 vinfos[4].indices[1] = _ij4[1];
14149 vinfos[4].maxsolutions = _nj4;
14150 vinfos[5].jointtype = 1;
14151 vinfos[5].foffset = j5;
14152 vinfos[5].indices[0] = _ij5[0];
14153 vinfos[5].indices[1] = _ij5[1];
14154 vinfos[5].maxsolutions = _nj5;
14155 vinfos[6].jointtype = 1;
14156 vinfos[6].foffset = j6;
14157 vinfos[6].indices[0] = _ij6[0];
14158 vinfos[6].indices[1] = _ij6[1];
14159 vinfos[6].maxsolutions = _nj6;
14160 std::vector<int> vfree(0);
14161 solutions.AddSolution(vinfos,vfree);
14162 }
14163 }
14164 }
14165 
14166 }
14167 
14168 }
14169 
14170 } else
14171 {
14172 {
14173 IkReal j4array[1], cj4array[1], sj4array[1];
14174 bool j4valid[1]={false};
14175 _nj4 = 1;
14176 IkReal x724=((1.0)*gconst28);
14177 CheckValue<IkReal> x725 = IKatan2WithCheck(IkReal((((gconst28*new_r01))+(((-1.0)*new_r10*x724)))),IkReal(((((-1.0)*new_r00*x724))+(((-1.0)*new_r11*x724)))),IKFAST_ATAN2_MAGTHRESH);
14178 if(!x725.valid){
14179 continue;
14180 }
14181 CheckValue<IkReal> x726=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
14182 if(!x726.valid){
14183 continue;
14184 }
14185 j4array[0]=((-1.5707963267949)+(x725.value)+(((1.5707963267949)*(x726.value))));
14186 sj4array[0]=IKsin(j4array[0]);
14187 cj4array[0]=IKcos(j4array[0]);
14188 if( j4array[0] > IKPI )
14189 {
14190  j4array[0]-=IK2PI;
14191 }
14192 else if( j4array[0] < -IKPI )
14193 { j4array[0]+=IK2PI;
14194 }
14195 j4valid[0] = true;
14196 for(int ij4 = 0; ij4 < 1; ++ij4)
14197 {
14198 if( !j4valid[ij4] )
14199 {
14200  continue;
14201 }
14202 _ij4[0] = ij4; _ij4[1] = -1;
14203 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14204 {
14205 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14206 {
14207  j4valid[iij4]=false; _ij4[1] = iij4; break;
14208 }
14209 }
14210 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14211 {
14212 IkReal evalcond[8];
14213 IkReal x727=IKsin(j4);
14214 IkReal x728=IKcos(j4);
14215 IkReal x729=(gconst28*x728);
14216 IkReal x730=((1.0)*x727);
14217 IkReal x731=(gconst29*x728);
14218 IkReal x732=(gconst29*x730);
14219 evalcond[0]=(gconst28+((new_r01*x728))+((new_r11*x727)));
14220 evalcond[1]=(((gconst28*x727))+x731+new_r11);
14221 evalcond[2]=((((-1.0)*x732))+x729+new_r01);
14222 evalcond[3]=(gconst28+(((-1.0)*new_r00*x730))+((new_r10*x728)));
14223 evalcond[4]=(gconst29+(((-1.0)*new_r01*x730))+((new_r11*x728)));
14224 evalcond[5]=((((-1.0)*x732))+x729+new_r10);
14225 evalcond[6]=(((new_r00*x728))+(((-1.0)*gconst29))+((new_r10*x727)));
14226 evalcond[7]=((((-1.0)*gconst28*x730))+(((-1.0)*x731))+new_r00);
14227 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14228 {
14229 continue;
14230 }
14231 }
14232 
14233 {
14234 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14235 vinfos[0].jointtype = 17;
14236 vinfos[0].foffset = j0;
14237 vinfos[0].indices[0] = _ij0[0];
14238 vinfos[0].indices[1] = _ij0[1];
14239 vinfos[0].maxsolutions = _nj0;
14240 vinfos[1].jointtype = 1;
14241 vinfos[1].foffset = j1;
14242 vinfos[1].indices[0] = _ij1[0];
14243 vinfos[1].indices[1] = _ij1[1];
14244 vinfos[1].maxsolutions = _nj1;
14245 vinfos[2].jointtype = 1;
14246 vinfos[2].foffset = j2;
14247 vinfos[2].indices[0] = _ij2[0];
14248 vinfos[2].indices[1] = _ij2[1];
14249 vinfos[2].maxsolutions = _nj2;
14250 vinfos[3].jointtype = 1;
14251 vinfos[3].foffset = j3;
14252 vinfos[3].indices[0] = _ij3[0];
14253 vinfos[3].indices[1] = _ij3[1];
14254 vinfos[3].maxsolutions = _nj3;
14255 vinfos[4].jointtype = 1;
14256 vinfos[4].foffset = j4;
14257 vinfos[4].indices[0] = _ij4[0];
14258 vinfos[4].indices[1] = _ij4[1];
14259 vinfos[4].maxsolutions = _nj4;
14260 vinfos[5].jointtype = 1;
14261 vinfos[5].foffset = j5;
14262 vinfos[5].indices[0] = _ij5[0];
14263 vinfos[5].indices[1] = _ij5[1];
14264 vinfos[5].maxsolutions = _nj5;
14265 vinfos[6].jointtype = 1;
14266 vinfos[6].foffset = j6;
14267 vinfos[6].indices[0] = _ij6[0];
14268 vinfos[6].indices[1] = _ij6[1];
14269 vinfos[6].maxsolutions = _nj6;
14270 std::vector<int> vfree(0);
14271 solutions.AddSolution(vinfos,vfree);
14272 }
14273 }
14274 }
14275 
14276 }
14277 
14278 }
14279 
14280 }
14281 } while(0);
14282 if( bgotonextstatement )
14283 {
14284 bool bgotonextstatement = true;
14285 do
14286 {
14287 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14288 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14289 {
14290 bgotonextstatement=false;
14291 {
14292 IkReal j4eval[1];
14293 sj5=0;
14294 cj5=-1.0;
14295 j5=3.14159265358979;
14296 new_r11=0;
14297 new_r01=0;
14298 new_r22=0;
14299 new_r20=0;
14300 j4eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
14301 if( IKabs(j4eval[0]) < 0.0000010000000000 )
14302 {
14303 continue; // no branches [j4]
14304 
14305 } else
14306 {
14307 {
14308 IkReal j4array[2], cj4array[2], sj4array[2];
14309 bool j4valid[2]={false};
14310 _nj4 = 2;
14311 CheckValue<IkReal> x734 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
14312 if(!x734.valid){
14313 continue;
14314 }
14315 IkReal x733=x734.value;
14316 j4array[0]=((-1.0)*x733);
14317 sj4array[0]=IKsin(j4array[0]);
14318 cj4array[0]=IKcos(j4array[0]);
14319 j4array[1]=((3.14159265358979)+(((-1.0)*x733)));
14320 sj4array[1]=IKsin(j4array[1]);
14321 cj4array[1]=IKcos(j4array[1]);
14322 if( j4array[0] > IKPI )
14323 {
14324  j4array[0]-=IK2PI;
14325 }
14326 else if( j4array[0] < -IKPI )
14327 { j4array[0]+=IK2PI;
14328 }
14329 j4valid[0] = true;
14330 if( j4array[1] > IKPI )
14331 {
14332  j4array[1]-=IK2PI;
14333 }
14334 else if( j4array[1] < -IKPI )
14335 { j4array[1]+=IK2PI;
14336 }
14337 j4valid[1] = true;
14338 for(int ij4 = 0; ij4 < 2; ++ij4)
14339 {
14340 if( !j4valid[ij4] )
14341 {
14342  continue;
14343 }
14344 _ij4[0] = ij4; _ij4[1] = -1;
14345 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
14346 {
14347 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14348 {
14349  j4valid[iij4]=false; _ij4[1] = iij4; break;
14350 }
14351 }
14352 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14353 {
14354 IkReal evalcond[1];
14355 evalcond[0]=(((new_r10*(IKcos(j4))))+(((-1.0)*new_r00*(IKsin(j4)))));
14356 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
14357 {
14358 continue;
14359 }
14360 }
14361 
14362 {
14363 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14364 vinfos[0].jointtype = 17;
14365 vinfos[0].foffset = j0;
14366 vinfos[0].indices[0] = _ij0[0];
14367 vinfos[0].indices[1] = _ij0[1];
14368 vinfos[0].maxsolutions = _nj0;
14369 vinfos[1].jointtype = 1;
14370 vinfos[1].foffset = j1;
14371 vinfos[1].indices[0] = _ij1[0];
14372 vinfos[1].indices[1] = _ij1[1];
14373 vinfos[1].maxsolutions = _nj1;
14374 vinfos[2].jointtype = 1;
14375 vinfos[2].foffset = j2;
14376 vinfos[2].indices[0] = _ij2[0];
14377 vinfos[2].indices[1] = _ij2[1];
14378 vinfos[2].maxsolutions = _nj2;
14379 vinfos[3].jointtype = 1;
14380 vinfos[3].foffset = j3;
14381 vinfos[3].indices[0] = _ij3[0];
14382 vinfos[3].indices[1] = _ij3[1];
14383 vinfos[3].maxsolutions = _nj3;
14384 vinfos[4].jointtype = 1;
14385 vinfos[4].foffset = j4;
14386 vinfos[4].indices[0] = _ij4[0];
14387 vinfos[4].indices[1] = _ij4[1];
14388 vinfos[4].maxsolutions = _nj4;
14389 vinfos[5].jointtype = 1;
14390 vinfos[5].foffset = j5;
14391 vinfos[5].indices[0] = _ij5[0];
14392 vinfos[5].indices[1] = _ij5[1];
14393 vinfos[5].maxsolutions = _nj5;
14394 vinfos[6].jointtype = 1;
14395 vinfos[6].foffset = j6;
14396 vinfos[6].indices[0] = _ij6[0];
14397 vinfos[6].indices[1] = _ij6[1];
14398 vinfos[6].maxsolutions = _nj6;
14399 std::vector<int> vfree(0);
14400 solutions.AddSolution(vinfos,vfree);
14401 }
14402 }
14403 }
14404 
14405 }
14406 
14407 }
14408 
14409 }
14410 } while(0);
14411 if( bgotonextstatement )
14412 {
14413 bool bgotonextstatement = true;
14414 do
14415 {
14416 if( 1 )
14417 {
14418 bgotonextstatement=false;
14419 continue; // branch miss [j4]
14420 
14421 }
14422 } while(0);
14423 if( bgotonextstatement )
14424 {
14425 }
14426 }
14427 }
14428 }
14429 }
14430 }
14431 }
14432 }
14433 
14434 } else
14435 {
14436 {
14437 IkReal j4array[1], cj4array[1], sj4array[1];
14438 bool j4valid[1]={false};
14439 _nj4 = 1;
14440 IkReal x735=((1.0)*sj6);
14441 CheckValue<IkReal> x736 = IKatan2WithCheck(IkReal((((cj6*new_r01))+(((-1.0)*new_r11*x735)))),IkReal(((((-1.0)*cj6*new_r11))+(((-1.0)*new_r01*x735)))),IKFAST_ATAN2_MAGTHRESH);
14442 if(!x736.valid){
14443 continue;
14444 }
14445 CheckValue<IkReal> x737=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
14446 if(!x737.valid){
14447 continue;
14448 }
14449 j4array[0]=((-1.5707963267949)+(x736.value)+(((1.5707963267949)*(x737.value))));
14450 sj4array[0]=IKsin(j4array[0]);
14451 cj4array[0]=IKcos(j4array[0]);
14452 if( j4array[0] > IKPI )
14453 {
14454  j4array[0]-=IK2PI;
14455 }
14456 else if( j4array[0] < -IKPI )
14457 { j4array[0]+=IK2PI;
14458 }
14459 j4valid[0] = true;
14460 for(int ij4 = 0; ij4 < 1; ++ij4)
14461 {
14462 if( !j4valid[ij4] )
14463 {
14464  continue;
14465 }
14466 _ij4[0] = ij4; _ij4[1] = -1;
14467 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14468 {
14469 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14470 {
14471  j4valid[iij4]=false; _ij4[1] = iij4; break;
14472 }
14473 }
14474 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14475 {
14476 IkReal evalcond[8];
14477 IkReal x738=IKsin(j4);
14478 IkReal x739=IKcos(j4);
14479 IkReal x740=((1.0)*cj6);
14480 IkReal x741=(sj6*x739);
14481 IkReal x742=(sj6*x738);
14482 IkReal x743=((1.0)*x738);
14483 IkReal x744=(x738*x740);
14484 evalcond[0]=(sj6+((new_r01*x739))+((new_r11*x738)));
14485 evalcond[1]=(((cj6*x739))+x742+new_r11);
14486 evalcond[2]=(x741+new_r01+(((-1.0)*x744)));
14487 evalcond[3]=((((-1.0)*new_r00*x743))+sj6+((new_r10*x739)));
14488 evalcond[4]=((((-1.0)*new_r01*x743))+cj6+((new_r11*x739)));
14489 evalcond[5]=(x741+new_r10+(((-1.0)*x744)));
14490 evalcond[6]=(((new_r00*x739))+(((-1.0)*x740))+((new_r10*x738)));
14491 evalcond[7]=((((-1.0)*x742))+(((-1.0)*x739*x740))+new_r00);
14492 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14493 {
14494 continue;
14495 }
14496 }
14497 
14498 {
14499 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14500 vinfos[0].jointtype = 17;
14501 vinfos[0].foffset = j0;
14502 vinfos[0].indices[0] = _ij0[0];
14503 vinfos[0].indices[1] = _ij0[1];
14504 vinfos[0].maxsolutions = _nj0;
14505 vinfos[1].jointtype = 1;
14506 vinfos[1].foffset = j1;
14507 vinfos[1].indices[0] = _ij1[0];
14508 vinfos[1].indices[1] = _ij1[1];
14509 vinfos[1].maxsolutions = _nj1;
14510 vinfos[2].jointtype = 1;
14511 vinfos[2].foffset = j2;
14512 vinfos[2].indices[0] = _ij2[0];
14513 vinfos[2].indices[1] = _ij2[1];
14514 vinfos[2].maxsolutions = _nj2;
14515 vinfos[3].jointtype = 1;
14516 vinfos[3].foffset = j3;
14517 vinfos[3].indices[0] = _ij3[0];
14518 vinfos[3].indices[1] = _ij3[1];
14519 vinfos[3].maxsolutions = _nj3;
14520 vinfos[4].jointtype = 1;
14521 vinfos[4].foffset = j4;
14522 vinfos[4].indices[0] = _ij4[0];
14523 vinfos[4].indices[1] = _ij4[1];
14524 vinfos[4].maxsolutions = _nj4;
14525 vinfos[5].jointtype = 1;
14526 vinfos[5].foffset = j5;
14527 vinfos[5].indices[0] = _ij5[0];
14528 vinfos[5].indices[1] = _ij5[1];
14529 vinfos[5].maxsolutions = _nj5;
14530 vinfos[6].jointtype = 1;
14531 vinfos[6].foffset = j6;
14532 vinfos[6].indices[0] = _ij6[0];
14533 vinfos[6].indices[1] = _ij6[1];
14534 vinfos[6].maxsolutions = _nj6;
14535 std::vector<int> vfree(0);
14536 solutions.AddSolution(vinfos,vfree);
14537 }
14538 }
14539 }
14540 
14541 }
14542 
14543 }
14544 
14545 } else
14546 {
14547 {
14548 IkReal j4array[1], cj4array[1], sj4array[1];
14549 bool j4valid[1]={false};
14550 _nj4 = 1;
14551 IkReal x745=((1.0)*sj6);
14552 CheckValue<IkReal> x746 = IKatan2WithCheck(IkReal((((new_r01*sj6))+(((-1.0)*new_r10*x745)))),IkReal(((((-1.0)*new_r00*x745))+(((-1.0)*new_r11*x745)))),IKFAST_ATAN2_MAGTHRESH);
14553 if(!x746.valid){
14554 continue;
14555 }
14556 CheckValue<IkReal> x747=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
14557 if(!x747.valid){
14558 continue;
14559 }
14560 j4array[0]=((-1.5707963267949)+(x746.value)+(((1.5707963267949)*(x747.value))));
14561 sj4array[0]=IKsin(j4array[0]);
14562 cj4array[0]=IKcos(j4array[0]);
14563 if( j4array[0] > IKPI )
14564 {
14565  j4array[0]-=IK2PI;
14566 }
14567 else if( j4array[0] < -IKPI )
14568 { j4array[0]+=IK2PI;
14569 }
14570 j4valid[0] = true;
14571 for(int ij4 = 0; ij4 < 1; ++ij4)
14572 {
14573 if( !j4valid[ij4] )
14574 {
14575  continue;
14576 }
14577 _ij4[0] = ij4; _ij4[1] = -1;
14578 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14579 {
14580 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14581 {
14582  j4valid[iij4]=false; _ij4[1] = iij4; break;
14583 }
14584 }
14585 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14586 {
14587 IkReal evalcond[8];
14588 IkReal x748=IKsin(j4);
14589 IkReal x749=IKcos(j4);
14590 IkReal x750=((1.0)*cj6);
14591 IkReal x751=(sj6*x749);
14592 IkReal x752=(sj6*x748);
14593 IkReal x753=((1.0)*x748);
14594 IkReal x754=(x748*x750);
14595 evalcond[0]=(sj6+((new_r01*x749))+((new_r11*x748)));
14596 evalcond[1]=(((cj6*x749))+x752+new_r11);
14597 evalcond[2]=(x751+new_r01+(((-1.0)*x754)));
14598 evalcond[3]=(sj6+(((-1.0)*new_r00*x753))+((new_r10*x749)));
14599 evalcond[4]=((((-1.0)*new_r01*x753))+cj6+((new_r11*x749)));
14600 evalcond[5]=(x751+new_r10+(((-1.0)*x754)));
14601 evalcond[6]=(((new_r10*x748))+((new_r00*x749))+(((-1.0)*x750)));
14602 evalcond[7]=((((-1.0)*x752))+new_r00+(((-1.0)*x749*x750)));
14603 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14604 {
14605 continue;
14606 }
14607 }
14608 
14609 {
14610 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14611 vinfos[0].jointtype = 17;
14612 vinfos[0].foffset = j0;
14613 vinfos[0].indices[0] = _ij0[0];
14614 vinfos[0].indices[1] = _ij0[1];
14615 vinfos[0].maxsolutions = _nj0;
14616 vinfos[1].jointtype = 1;
14617 vinfos[1].foffset = j1;
14618 vinfos[1].indices[0] = _ij1[0];
14619 vinfos[1].indices[1] = _ij1[1];
14620 vinfos[1].maxsolutions = _nj1;
14621 vinfos[2].jointtype = 1;
14622 vinfos[2].foffset = j2;
14623 vinfos[2].indices[0] = _ij2[0];
14624 vinfos[2].indices[1] = _ij2[1];
14625 vinfos[2].maxsolutions = _nj2;
14626 vinfos[3].jointtype = 1;
14627 vinfos[3].foffset = j3;
14628 vinfos[3].indices[0] = _ij3[0];
14629 vinfos[3].indices[1] = _ij3[1];
14630 vinfos[3].maxsolutions = _nj3;
14631 vinfos[4].jointtype = 1;
14632 vinfos[4].foffset = j4;
14633 vinfos[4].indices[0] = _ij4[0];
14634 vinfos[4].indices[1] = _ij4[1];
14635 vinfos[4].maxsolutions = _nj4;
14636 vinfos[5].jointtype = 1;
14637 vinfos[5].foffset = j5;
14638 vinfos[5].indices[0] = _ij5[0];
14639 vinfos[5].indices[1] = _ij5[1];
14640 vinfos[5].maxsolutions = _nj5;
14641 vinfos[6].jointtype = 1;
14642 vinfos[6].foffset = j6;
14643 vinfos[6].indices[0] = _ij6[0];
14644 vinfos[6].indices[1] = _ij6[1];
14645 vinfos[6].maxsolutions = _nj6;
14646 std::vector<int> vfree(0);
14647 solutions.AddSolution(vinfos,vfree);
14648 }
14649 }
14650 }
14651 
14652 }
14653 
14654 }
14655 
14656 } else
14657 {
14658 {
14659 IkReal j4array[1], cj4array[1], sj4array[1];
14660 bool j4valid[1]={false};
14661 _nj4 = 1;
14662 CheckValue<IkReal> x755 = IKatan2WithCheck(IkReal(((-1.0)+(new_r01*new_r01)+(cj6*cj6))),IkReal(((((-1.0)*new_r01*new_r11))+(((-1.0)*cj6*sj6)))),IKFAST_ATAN2_MAGTHRESH);
14663 if(!x755.valid){
14664 continue;
14665 }
14666 CheckValue<IkReal> x756=IKPowWithIntegerCheck(IKsign((((new_r11*sj6))+((cj6*new_r01)))),-1);
14667 if(!x756.valid){
14668 continue;
14669 }
14670 j4array[0]=((-1.5707963267949)+(x755.value)+(((1.5707963267949)*(x756.value))));
14671 sj4array[0]=IKsin(j4array[0]);
14672 cj4array[0]=IKcos(j4array[0]);
14673 if( j4array[0] > IKPI )
14674 {
14675  j4array[0]-=IK2PI;
14676 }
14677 else if( j4array[0] < -IKPI )
14678 { j4array[0]+=IK2PI;
14679 }
14680 j4valid[0] = true;
14681 for(int ij4 = 0; ij4 < 1; ++ij4)
14682 {
14683 if( !j4valid[ij4] )
14684 {
14685  continue;
14686 }
14687 _ij4[0] = ij4; _ij4[1] = -1;
14688 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
14689 {
14690 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14691 {
14692  j4valid[iij4]=false; _ij4[1] = iij4; break;
14693 }
14694 }
14695 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14696 {
14697 IkReal evalcond[8];
14698 IkReal x757=IKsin(j4);
14699 IkReal x758=IKcos(j4);
14700 IkReal x759=((1.0)*cj6);
14701 IkReal x760=(sj6*x758);
14702 IkReal x761=(sj6*x757);
14703 IkReal x762=((1.0)*x757);
14704 IkReal x763=(x757*x759);
14705 evalcond[0]=(sj6+((new_r11*x757))+((new_r01*x758)));
14706 evalcond[1]=(((cj6*x758))+x761+new_r11);
14707 evalcond[2]=((((-1.0)*x763))+x760+new_r01);
14708 evalcond[3]=(sj6+(((-1.0)*new_r00*x762))+((new_r10*x758)));
14709 evalcond[4]=(cj6+((new_r11*x758))+(((-1.0)*new_r01*x762)));
14710 evalcond[5]=((((-1.0)*x763))+x760+new_r10);
14711 evalcond[6]=(((new_r00*x758))+((new_r10*x757))+(((-1.0)*x759)));
14712 evalcond[7]=((((-1.0)*x761))+(((-1.0)*x758*x759))+new_r00);
14713 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14714 {
14715 continue;
14716 }
14717 }
14718 
14719 {
14720 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14721 vinfos[0].jointtype = 17;
14722 vinfos[0].foffset = j0;
14723 vinfos[0].indices[0] = _ij0[0];
14724 vinfos[0].indices[1] = _ij0[1];
14725 vinfos[0].maxsolutions = _nj0;
14726 vinfos[1].jointtype = 1;
14727 vinfos[1].foffset = j1;
14728 vinfos[1].indices[0] = _ij1[0];
14729 vinfos[1].indices[1] = _ij1[1];
14730 vinfos[1].maxsolutions = _nj1;
14731 vinfos[2].jointtype = 1;
14732 vinfos[2].foffset = j2;
14733 vinfos[2].indices[0] = _ij2[0];
14734 vinfos[2].indices[1] = _ij2[1];
14735 vinfos[2].maxsolutions = _nj2;
14736 vinfos[3].jointtype = 1;
14737 vinfos[3].foffset = j3;
14738 vinfos[3].indices[0] = _ij3[0];
14739 vinfos[3].indices[1] = _ij3[1];
14740 vinfos[3].maxsolutions = _nj3;
14741 vinfos[4].jointtype = 1;
14742 vinfos[4].foffset = j4;
14743 vinfos[4].indices[0] = _ij4[0];
14744 vinfos[4].indices[1] = _ij4[1];
14745 vinfos[4].maxsolutions = _nj4;
14746 vinfos[5].jointtype = 1;
14747 vinfos[5].foffset = j5;
14748 vinfos[5].indices[0] = _ij5[0];
14749 vinfos[5].indices[1] = _ij5[1];
14750 vinfos[5].maxsolutions = _nj5;
14751 vinfos[6].jointtype = 1;
14752 vinfos[6].foffset = j6;
14753 vinfos[6].indices[0] = _ij6[0];
14754 vinfos[6].indices[1] = _ij6[1];
14755 vinfos[6].maxsolutions = _nj6;
14756 std::vector<int> vfree(0);
14757 solutions.AddSolution(vinfos,vfree);
14758 }
14759 }
14760 }
14761 
14762 }
14763 
14764 }
14765 
14766 }
14767 } while(0);
14768 if( bgotonextstatement )
14769 {
14770 bool bgotonextstatement = true;
14771 do
14772 {
14773 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
14774 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14775 {
14776 bgotonextstatement=false;
14777 {
14778 IkReal j4eval[1];
14779 new_r02=0;
14780 new_r12=0;
14781 new_r20=0;
14782 new_r21=0;
14783 j4eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14784 if( IKabs(j4eval[0]) < 0.0000010000000000 )
14785 {
14786 {
14787 IkReal j4eval[1];
14788 new_r02=0;
14789 new_r12=0;
14790 new_r20=0;
14791 new_r21=0;
14792 j4eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
14793 if( IKabs(j4eval[0]) < 0.0000010000000000 )
14794 {
14795 {
14796 IkReal j4eval[1];
14797 new_r02=0;
14798 new_r12=0;
14799 new_r20=0;
14800 new_r21=0;
14801 j4eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
14802 if( IKabs(j4eval[0]) < 0.0000010000000000 )
14803 {
14804 continue; // no branches [j4]
14805 
14806 } else
14807 {
14808 {
14809 IkReal j4array[2], cj4array[2], sj4array[2];
14810 bool j4valid[2]={false};
14811 _nj4 = 2;
14812 CheckValue<IkReal> x765 = IKatan2WithCheck(IkReal((new_r10*new_r22)),IkReal(((-1.0)*new_r00*new_r22)),IKFAST_ATAN2_MAGTHRESH);
14813 if(!x765.valid){
14814 continue;
14815 }
14816 IkReal x764=x765.value;
14817 j4array[0]=((-1.0)*x764);
14818 sj4array[0]=IKsin(j4array[0]);
14819 cj4array[0]=IKcos(j4array[0]);
14820 j4array[1]=((3.14159265358979)+(((-1.0)*x764)));
14821 sj4array[1]=IKsin(j4array[1]);
14822 cj4array[1]=IKcos(j4array[1]);
14823 if( j4array[0] > IKPI )
14824 {
14825  j4array[0]-=IK2PI;
14826 }
14827 else if( j4array[0] < -IKPI )
14828 { j4array[0]+=IK2PI;
14829 }
14830 j4valid[0] = true;
14831 if( j4array[1] > IKPI )
14832 {
14833  j4array[1]-=IK2PI;
14834 }
14835 else if( j4array[1] < -IKPI )
14836 { j4array[1]+=IK2PI;
14837 }
14838 j4valid[1] = true;
14839 for(int ij4 = 0; ij4 < 2; ++ij4)
14840 {
14841 if( !j4valid[ij4] )
14842 {
14843  continue;
14844 }
14845 _ij4[0] = ij4; _ij4[1] = -1;
14846 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
14847 {
14848 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14849 {
14850  j4valid[iij4]=false; _ij4[1] = iij4; break;
14851 }
14852 }
14853 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14854 {
14855 IkReal evalcond[5];
14856 IkReal x766=IKcos(j4);
14857 IkReal x767=IKsin(j4);
14858 IkReal x768=(new_r11*x766);
14859 IkReal x769=((1.0)*x767);
14860 evalcond[0]=(((new_r11*x767))+((new_r01*x766)));
14861 evalcond[1]=(((new_r10*x767))+((new_r00*x766)));
14862 evalcond[2]=(((new_r10*x766))+(((-1.0)*new_r00*x769)));
14863 evalcond[3]=(x768+(((-1.0)*new_r01*x769)));
14864 evalcond[4]=(((new_r22*x768))+(((-1.0)*new_r01*new_r22*x769)));
14865 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
14866 {
14867 continue;
14868 }
14869 }
14870 
14871 {
14872 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14873 vinfos[0].jointtype = 17;
14874 vinfos[0].foffset = j0;
14875 vinfos[0].indices[0] = _ij0[0];
14876 vinfos[0].indices[1] = _ij0[1];
14877 vinfos[0].maxsolutions = _nj0;
14878 vinfos[1].jointtype = 1;
14879 vinfos[1].foffset = j1;
14880 vinfos[1].indices[0] = _ij1[0];
14881 vinfos[1].indices[1] = _ij1[1];
14882 vinfos[1].maxsolutions = _nj1;
14883 vinfos[2].jointtype = 1;
14884 vinfos[2].foffset = j2;
14885 vinfos[2].indices[0] = _ij2[0];
14886 vinfos[2].indices[1] = _ij2[1];
14887 vinfos[2].maxsolutions = _nj2;
14888 vinfos[3].jointtype = 1;
14889 vinfos[3].foffset = j3;
14890 vinfos[3].indices[0] = _ij3[0];
14891 vinfos[3].indices[1] = _ij3[1];
14892 vinfos[3].maxsolutions = _nj3;
14893 vinfos[4].jointtype = 1;
14894 vinfos[4].foffset = j4;
14895 vinfos[4].indices[0] = _ij4[0];
14896 vinfos[4].indices[1] = _ij4[1];
14897 vinfos[4].maxsolutions = _nj4;
14898 vinfos[5].jointtype = 1;
14899 vinfos[5].foffset = j5;
14900 vinfos[5].indices[0] = _ij5[0];
14901 vinfos[5].indices[1] = _ij5[1];
14902 vinfos[5].maxsolutions = _nj5;
14903 vinfos[6].jointtype = 1;
14904 vinfos[6].foffset = j6;
14905 vinfos[6].indices[0] = _ij6[0];
14906 vinfos[6].indices[1] = _ij6[1];
14907 vinfos[6].maxsolutions = _nj6;
14908 std::vector<int> vfree(0);
14909 solutions.AddSolution(vinfos,vfree);
14910 }
14911 }
14912 }
14913 
14914 }
14915 
14916 }
14917 
14918 } else
14919 {
14920 {
14921 IkReal j4array[2], cj4array[2], sj4array[2];
14922 bool j4valid[2]={false};
14923 _nj4 = 2;
14924 CheckValue<IkReal> x771 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
14925 if(!x771.valid){
14926 continue;
14927 }
14928 IkReal x770=x771.value;
14929 j4array[0]=((-1.0)*x770);
14930 sj4array[0]=IKsin(j4array[0]);
14931 cj4array[0]=IKcos(j4array[0]);
14932 j4array[1]=((3.14159265358979)+(((-1.0)*x770)));
14933 sj4array[1]=IKsin(j4array[1]);
14934 cj4array[1]=IKcos(j4array[1]);
14935 if( j4array[0] > IKPI )
14936 {
14937  j4array[0]-=IK2PI;
14938 }
14939 else if( j4array[0] < -IKPI )
14940 { j4array[0]+=IK2PI;
14941 }
14942 j4valid[0] = true;
14943 if( j4array[1] > IKPI )
14944 {
14945  j4array[1]-=IK2PI;
14946 }
14947 else if( j4array[1] < -IKPI )
14948 { j4array[1]+=IK2PI;
14949 }
14950 j4valid[1] = true;
14951 for(int ij4 = 0; ij4 < 2; ++ij4)
14952 {
14953 if( !j4valid[ij4] )
14954 {
14955  continue;
14956 }
14957 _ij4[0] = ij4; _ij4[1] = -1;
14958 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
14959 {
14960 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
14961 {
14962  j4valid[iij4]=false; _ij4[1] = iij4; break;
14963 }
14964 }
14965 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
14966 {
14967 IkReal evalcond[5];
14968 IkReal x772=IKcos(j4);
14969 IkReal x773=IKsin(j4);
14970 IkReal x774=(new_r10*x772);
14971 IkReal x775=(new_r11*x772);
14972 IkReal x776=((1.0)*new_r00*x773);
14973 IkReal x777=((1.0)*new_r01*x773);
14974 evalcond[0]=(((new_r11*x773))+((new_r01*x772)));
14975 evalcond[1]=(x774+(((-1.0)*x776)));
14976 evalcond[2]=(x775+(((-1.0)*x777)));
14977 evalcond[3]=((((-1.0)*new_r22*x776))+((new_r22*x774)));
14978 evalcond[4]=((((-1.0)*new_r22*x777))+((new_r22*x775)));
14979 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
14980 {
14981 continue;
14982 }
14983 }
14984 
14985 {
14986 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
14987 vinfos[0].jointtype = 17;
14988 vinfos[0].foffset = j0;
14989 vinfos[0].indices[0] = _ij0[0];
14990 vinfos[0].indices[1] = _ij0[1];
14991 vinfos[0].maxsolutions = _nj0;
14992 vinfos[1].jointtype = 1;
14993 vinfos[1].foffset = j1;
14994 vinfos[1].indices[0] = _ij1[0];
14995 vinfos[1].indices[1] = _ij1[1];
14996 vinfos[1].maxsolutions = _nj1;
14997 vinfos[2].jointtype = 1;
14998 vinfos[2].foffset = j2;
14999 vinfos[2].indices[0] = _ij2[0];
15000 vinfos[2].indices[1] = _ij2[1];
15001 vinfos[2].maxsolutions = _nj2;
15002 vinfos[3].jointtype = 1;
15003 vinfos[3].foffset = j3;
15004 vinfos[3].indices[0] = _ij3[0];
15005 vinfos[3].indices[1] = _ij3[1];
15006 vinfos[3].maxsolutions = _nj3;
15007 vinfos[4].jointtype = 1;
15008 vinfos[4].foffset = j4;
15009 vinfos[4].indices[0] = _ij4[0];
15010 vinfos[4].indices[1] = _ij4[1];
15011 vinfos[4].maxsolutions = _nj4;
15012 vinfos[5].jointtype = 1;
15013 vinfos[5].foffset = j5;
15014 vinfos[5].indices[0] = _ij5[0];
15015 vinfos[5].indices[1] = _ij5[1];
15016 vinfos[5].maxsolutions = _nj5;
15017 vinfos[6].jointtype = 1;
15018 vinfos[6].foffset = j6;
15019 vinfos[6].indices[0] = _ij6[0];
15020 vinfos[6].indices[1] = _ij6[1];
15021 vinfos[6].maxsolutions = _nj6;
15022 std::vector<int> vfree(0);
15023 solutions.AddSolution(vinfos,vfree);
15024 }
15025 }
15026 }
15027 
15028 }
15029 
15030 }
15031 
15032 } else
15033 {
15034 {
15035 IkReal j4array[2], cj4array[2], sj4array[2];
15036 bool j4valid[2]={false};
15037 _nj4 = 2;
15038 CheckValue<IkReal> x779 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
15039 if(!x779.valid){
15040 continue;
15041 }
15042 IkReal x778=x779.value;
15043 j4array[0]=((-1.0)*x778);
15044 sj4array[0]=IKsin(j4array[0]);
15045 cj4array[0]=IKcos(j4array[0]);
15046 j4array[1]=((3.14159265358979)+(((-1.0)*x778)));
15047 sj4array[1]=IKsin(j4array[1]);
15048 cj4array[1]=IKcos(j4array[1]);
15049 if( j4array[0] > IKPI )
15050 {
15051  j4array[0]-=IK2PI;
15052 }
15053 else if( j4array[0] < -IKPI )
15054 { j4array[0]+=IK2PI;
15055 }
15056 j4valid[0] = true;
15057 if( j4array[1] > IKPI )
15058 {
15059  j4array[1]-=IK2PI;
15060 }
15061 else if( j4array[1] < -IKPI )
15062 { j4array[1]+=IK2PI;
15063 }
15064 j4valid[1] = true;
15065 for(int ij4 = 0; ij4 < 2; ++ij4)
15066 {
15067 if( !j4valid[ij4] )
15068 {
15069  continue;
15070 }
15071 _ij4[0] = ij4; _ij4[1] = -1;
15072 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
15073 {
15074 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15075 {
15076  j4valid[iij4]=false; _ij4[1] = iij4; break;
15077 }
15078 }
15079 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15080 {
15081 IkReal evalcond[5];
15082 IkReal x780=IKcos(j4);
15083 IkReal x781=IKsin(j4);
15084 IkReal x782=(new_r10*x780);
15085 IkReal x783=(new_r11*x780);
15086 IkReal x784=((1.0)*new_r00*x781);
15087 IkReal x785=((1.0)*new_r01*x781);
15088 evalcond[0]=(((new_r10*x781))+((new_r00*x780)));
15089 evalcond[1]=((((-1.0)*x784))+x782);
15090 evalcond[2]=((((-1.0)*x785))+x783);
15091 evalcond[3]=(((new_r22*x782))+(((-1.0)*new_r22*x784)));
15092 evalcond[4]=(((new_r22*x783))+(((-1.0)*new_r22*x785)));
15093 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
15094 {
15095 continue;
15096 }
15097 }
15098 
15099 {
15100 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15101 vinfos[0].jointtype = 17;
15102 vinfos[0].foffset = j0;
15103 vinfos[0].indices[0] = _ij0[0];
15104 vinfos[0].indices[1] = _ij0[1];
15105 vinfos[0].maxsolutions = _nj0;
15106 vinfos[1].jointtype = 1;
15107 vinfos[1].foffset = j1;
15108 vinfos[1].indices[0] = _ij1[0];
15109 vinfos[1].indices[1] = _ij1[1];
15110 vinfos[1].maxsolutions = _nj1;
15111 vinfos[2].jointtype = 1;
15112 vinfos[2].foffset = j2;
15113 vinfos[2].indices[0] = _ij2[0];
15114 vinfos[2].indices[1] = _ij2[1];
15115 vinfos[2].maxsolutions = _nj2;
15116 vinfos[3].jointtype = 1;
15117 vinfos[3].foffset = j3;
15118 vinfos[3].indices[0] = _ij3[0];
15119 vinfos[3].indices[1] = _ij3[1];
15120 vinfos[3].maxsolutions = _nj3;
15121 vinfos[4].jointtype = 1;
15122 vinfos[4].foffset = j4;
15123 vinfos[4].indices[0] = _ij4[0];
15124 vinfos[4].indices[1] = _ij4[1];
15125 vinfos[4].maxsolutions = _nj4;
15126 vinfos[5].jointtype = 1;
15127 vinfos[5].foffset = j5;
15128 vinfos[5].indices[0] = _ij5[0];
15129 vinfos[5].indices[1] = _ij5[1];
15130 vinfos[5].maxsolutions = _nj5;
15131 vinfos[6].jointtype = 1;
15132 vinfos[6].foffset = j6;
15133 vinfos[6].indices[0] = _ij6[0];
15134 vinfos[6].indices[1] = _ij6[1];
15135 vinfos[6].maxsolutions = _nj6;
15136 std::vector<int> vfree(0);
15137 solutions.AddSolution(vinfos,vfree);
15138 }
15139 }
15140 }
15141 
15142 }
15143 
15144 }
15145 
15146 }
15147 } while(0);
15148 if( bgotonextstatement )
15149 {
15150 bool bgotonextstatement = true;
15151 do
15152 {
15153 if( 1 )
15154 {
15155 bgotonextstatement=false;
15156 continue; // branch miss [j4]
15157 
15158 }
15159 } while(0);
15160 if( bgotonextstatement )
15161 {
15162 }
15163 }
15164 }
15165 }
15166 }
15167 
15168 } else
15169 {
15170 {
15171 IkReal j4array[1], cj4array[1], sj4array[1];
15172 bool j4valid[1]={false};
15173 _nj4 = 1;
15174 CheckValue<IkReal> x787=IKPowWithIntegerCheck(sj5,-1);
15175 if(!x787.valid){
15176 continue;
15177 }
15178 IkReal x786=x787.value;
15179 CheckValue<IkReal> x788=IKPowWithIntegerCheck(new_r11,-1);
15180 if(!x788.valid){
15181 continue;
15182 }
15183 if( IKabs((x786*(x788.value)*((((new_r01*new_r12))+(((-1.0)*sj5*sj6)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x786)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x786*(x788.value)*((((new_r01*new_r12))+(((-1.0)*sj5*sj6))))))+IKsqr(((-1.0)*new_r12*x786))-1) <= IKFAST_SINCOS_THRESH )
15184  continue;
15185 j4array[0]=IKatan2((x786*(x788.value)*((((new_r01*new_r12))+(((-1.0)*sj5*sj6))))), ((-1.0)*new_r12*x786));
15186 sj4array[0]=IKsin(j4array[0]);
15187 cj4array[0]=IKcos(j4array[0]);
15188 if( j4array[0] > IKPI )
15189 {
15190  j4array[0]-=IK2PI;
15191 }
15192 else if( j4array[0] < -IKPI )
15193 { j4array[0]+=IK2PI;
15194 }
15195 j4valid[0] = true;
15196 for(int ij4 = 0; ij4 < 1; ++ij4)
15197 {
15198 if( !j4valid[ij4] )
15199 {
15200  continue;
15201 }
15202 _ij4[0] = ij4; _ij4[1] = -1;
15203 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15204 {
15205 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15206 {
15207  j4valid[iij4]=false; _ij4[1] = iij4; break;
15208 }
15209 }
15210 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15211 {
15212 IkReal evalcond[18];
15213 IkReal x789=IKcos(j4);
15214 IkReal x790=IKsin(j4);
15215 IkReal x791=((1.0)*cj6);
15216 IkReal x792=(cj5*sj6);
15217 IkReal x793=((1.0)*cj5);
15218 IkReal x794=(cj5*cj6);
15219 IkReal x795=((1.0)*sj5);
15220 IkReal x796=(new_r10*x789);
15221 IkReal x797=(sj5*x789);
15222 IkReal x798=((1.0)*x789);
15223 IkReal x799=(new_r01*x790);
15224 IkReal x800=(new_r12*x789);
15225 IkReal x801=(new_r11*x789);
15226 IkReal x802=(new_r00*x790);
15227 IkReal x803=(new_r02*x790);
15228 evalcond[0]=(x797+new_r12);
15229 evalcond[1]=((((-1.0)*x790*x795))+new_r02);
15230 evalcond[2]=(((new_r12*x790))+((new_r02*x789)));
15231 evalcond[3]=(sj6+((new_r11*x790))+((new_r01*x789)));
15232 evalcond[4]=((((-1.0)*x803))+sj5+x800);
15233 evalcond[5]=(((x790*x794))+((sj6*x789))+new_r01);
15234 evalcond[6]=((((-1.0)*x791))+((new_r10*x790))+((new_r00*x789)));
15235 evalcond[7]=(((x790*x792))+(((-1.0)*x789*x791))+new_r00);
15236 evalcond[8]=(((sj6*x790))+(((-1.0)*cj5*x789*x791))+new_r11);
15237 evalcond[9]=((((-1.0)*x792*x798))+(((-1.0)*x790*x791))+new_r10);
15238 evalcond[10]=((((-1.0)*x802))+x796+(((-1.0)*x792)));
15239 evalcond[11]=((((-1.0)*cj5*x791))+(((-1.0)*x799))+x801);
15240 evalcond[12]=(((cj5*x800))+((new_r22*sj5))+(((-1.0)*x793*x803)));
15241 evalcond[13]=(((sj5*x796))+(((-1.0)*x795*x802))+(((-1.0)*new_r20*x793)));
15242 evalcond[14]=((((-1.0)*new_r21*x793))+(((-1.0)*x795*x799))+((new_r11*x797)));
15243 evalcond[15]=((1.0)+(((-1.0)*new_r22*x793))+((new_r12*x797))+(((-1.0)*x795*x803)));
15244 evalcond[16]=((((-1.0)*sj6))+((new_r20*sj5))+((cj5*x796))+(((-1.0)*x793*x802)));
15245 evalcond[17]=(((cj5*x801))+(((-1.0)*x791))+(((-1.0)*x793*x799))+((new_r21*sj5)));
15246 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
15247 {
15248 continue;
15249 }
15250 }
15251 
15252 {
15253 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15254 vinfos[0].jointtype = 17;
15255 vinfos[0].foffset = j0;
15256 vinfos[0].indices[0] = _ij0[0];
15257 vinfos[0].indices[1] = _ij0[1];
15258 vinfos[0].maxsolutions = _nj0;
15259 vinfos[1].jointtype = 1;
15260 vinfos[1].foffset = j1;
15261 vinfos[1].indices[0] = _ij1[0];
15262 vinfos[1].indices[1] = _ij1[1];
15263 vinfos[1].maxsolutions = _nj1;
15264 vinfos[2].jointtype = 1;
15265 vinfos[2].foffset = j2;
15266 vinfos[2].indices[0] = _ij2[0];
15267 vinfos[2].indices[1] = _ij2[1];
15268 vinfos[2].maxsolutions = _nj2;
15269 vinfos[3].jointtype = 1;
15270 vinfos[3].foffset = j3;
15271 vinfos[3].indices[0] = _ij3[0];
15272 vinfos[3].indices[1] = _ij3[1];
15273 vinfos[3].maxsolutions = _nj3;
15274 vinfos[4].jointtype = 1;
15275 vinfos[4].foffset = j4;
15276 vinfos[4].indices[0] = _ij4[0];
15277 vinfos[4].indices[1] = _ij4[1];
15278 vinfos[4].maxsolutions = _nj4;
15279 vinfos[5].jointtype = 1;
15280 vinfos[5].foffset = j5;
15281 vinfos[5].indices[0] = _ij5[0];
15282 vinfos[5].indices[1] = _ij5[1];
15283 vinfos[5].maxsolutions = _nj5;
15284 vinfos[6].jointtype = 1;
15285 vinfos[6].foffset = j6;
15286 vinfos[6].indices[0] = _ij6[0];
15287 vinfos[6].indices[1] = _ij6[1];
15288 vinfos[6].maxsolutions = _nj6;
15289 std::vector<int> vfree(0);
15290 solutions.AddSolution(vinfos,vfree);
15291 }
15292 }
15293 }
15294 
15295 }
15296 
15297 }
15298 
15299 } else
15300 {
15301 {
15302 IkReal j4array[1], cj4array[1], sj4array[1];
15303 bool j4valid[1]={false};
15304 _nj4 = 1;
15305 CheckValue<IkReal> x804=IKPowWithIntegerCheck(IKsign(sj5),-1);
15306 if(!x804.valid){
15307 continue;
15308 }
15309 CheckValue<IkReal> x805 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH);
15310 if(!x805.valid){
15311 continue;
15312 }
15313 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x804.value)))+(x805.value));
15314 sj4array[0]=IKsin(j4array[0]);
15315 cj4array[0]=IKcos(j4array[0]);
15316 if( j4array[0] > IKPI )
15317 {
15318  j4array[0]-=IK2PI;
15319 }
15320 else if( j4array[0] < -IKPI )
15321 { j4array[0]+=IK2PI;
15322 }
15323 j4valid[0] = true;
15324 for(int ij4 = 0; ij4 < 1; ++ij4)
15325 {
15326 if( !j4valid[ij4] )
15327 {
15328  continue;
15329 }
15330 _ij4[0] = ij4; _ij4[1] = -1;
15331 for(int iij4 = ij4+1; iij4 < 1; ++iij4)
15332 {
15333 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
15334 {
15335  j4valid[iij4]=false; _ij4[1] = iij4; break;
15336 }
15337 }
15338 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
15339 {
15340 IkReal evalcond[18];
15341 IkReal x806=IKcos(j4);
15342 IkReal x807=IKsin(j4);
15343 IkReal x808=((1.0)*cj6);
15344 IkReal x809=(cj5*sj6);
15345 IkReal x810=((1.0)*cj5);
15346 IkReal x811=(cj5*cj6);
15347 IkReal x812=((1.0)*sj5);
15348 IkReal x813=(new_r10*x806);
15349 IkReal x814=(sj5*x806);
15350 IkReal x815=((1.0)*x806);
15351 IkReal x816=(new_r01*x807);
15352 IkReal x817=(new_r12*x806);
15353 IkReal x818=(new_r11*x806);
15354 IkReal x819=(new_r00*x807);
15355 IkReal x820=(new_r02*x807);
15356 evalcond[0]=(new_r12+x814);
15357 evalcond[1]=((((-1.0)*x807*x812))+new_r02);
15358 evalcond[2]=(((new_r02*x806))+((new_r12*x807)));
15359 evalcond[3]=(sj6+((new_r11*x807))+((new_r01*x806)));
15360 evalcond[4]=(sj5+(((-1.0)*x820))+x817);
15361 evalcond[5]=(((x807*x811))+new_r01+((sj6*x806)));
15362 evalcond[6]=(((new_r00*x806))+((new_r10*x807))+(((-1.0)*x808)));
15363 evalcond[7]=((((-1.0)*x806*x808))+new_r00+((x807*x809)));
15364 evalcond[8]=((((-1.0)*cj5*x806*x808))+new_r11+((sj6*x807)));
15365 evalcond[9]=((((-1.0)*x807*x808))+(((-1.0)*x809*x815))+new_r10);
15366 evalcond[10]=((((-1.0)*x809))+(((-1.0)*x819))+x813);
15367 evalcond[11]=((((-1.0)*cj5*x808))+(((-1.0)*x816))+x818);
15368 evalcond[12]=((((-1.0)*x810*x820))+((new_r22*sj5))+((cj5*x817)));
15369 evalcond[13]=((((-1.0)*x812*x819))+((sj5*x813))+(((-1.0)*new_r20*x810)));
15370 evalcond[14]=((((-1.0)*x812*x816))+((new_r11*x814))+(((-1.0)*new_r21*x810)));
15371 evalcond[15]=((1.0)+(((-1.0)*x812*x820))+((new_r12*x814))+(((-1.0)*new_r22*x810)));
15372 evalcond[16]=((((-1.0)*sj6))+((new_r20*sj5))+(((-1.0)*x810*x819))+((cj5*x813)));
15373 evalcond[17]=((((-1.0)*x810*x816))+((cj5*x818))+((new_r21*sj5))+(((-1.0)*x808)));
15374 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
15375 {
15376 continue;
15377 }
15378 }
15379 
15380 {
15381 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
15382 vinfos[0].jointtype = 17;
15383 vinfos[0].foffset = j0;
15384 vinfos[0].indices[0] = _ij0[0];
15385 vinfos[0].indices[1] = _ij0[1];
15386 vinfos[0].maxsolutions = _nj0;
15387 vinfos[1].jointtype = 1;
15388 vinfos[1].foffset = j1;
15389 vinfos[1].indices[0] = _ij1[0];
15390 vinfos[1].indices[1] = _ij1[1];
15391 vinfos[1].maxsolutions = _nj1;
15392 vinfos[2].jointtype = 1;
15393 vinfos[2].foffset = j2;
15394 vinfos[2].indices[0] = _ij2[0];
15395 vinfos[2].indices[1] = _ij2[1];
15396 vinfos[2].maxsolutions = _nj2;
15397 vinfos[3].jointtype = 1;
15398 vinfos[3].foffset = j3;
15399 vinfos[3].indices[0] = _ij3[0];
15400 vinfos[3].indices[1] = _ij3[1];
15401 vinfos[3].maxsolutions = _nj3;
15402 vinfos[4].jointtype = 1;
15403 vinfos[4].foffset = j4;
15404 vinfos[4].indices[0] = _ij4[0];
15405 vinfos[4].indices[1] = _ij4[1];
15406 vinfos[4].maxsolutions = _nj4;
15407 vinfos[5].jointtype = 1;
15408 vinfos[5].foffset = j5;
15409 vinfos[5].indices[0] = _ij5[0];
15410 vinfos[5].indices[1] = _ij5[1];
15411 vinfos[5].maxsolutions = _nj5;
15412 vinfos[6].jointtype = 1;
15413 vinfos[6].foffset = j6;
15414 vinfos[6].indices[0] = _ij6[0];
15415 vinfos[6].indices[1] = _ij6[1];
15416 vinfos[6].maxsolutions = _nj6;
15417 std::vector<int> vfree(0);
15418 solutions.AddSolution(vinfos,vfree);
15419 }
15420 }
15421 }
15422 
15423 }
15424 
15425 }
15426 }
15427 }
15428 
15429 }
15430 
15431 }
15432 }
15433 }
15434 }
15435 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
15436 {
15437  using std::complex;
15438  if( rawcoeffs[0] == 0 ) {
15439  // solve with one reduced degree
15440  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
15441  return;
15442  }
15443  IKFAST_ASSERT(rawcoeffs[0] != 0);
15444  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
15445  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
15446  complex<IkReal> coeffs[3];
15447  const int maxsteps = 110;
15448  for(int i = 0; i < 3; ++i) {
15449  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
15450  }
15451  complex<IkReal> roots[3];
15452  IkReal err[3];
15453  roots[0] = complex<IkReal>(1,0);
15454  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
15455  err[0] = 1.0;
15456  err[1] = 1.0;
15457  for(int i = 2; i < 3; ++i) {
15458  roots[i] = roots[i-1]*roots[1];
15459  err[i] = 1.0;
15460  }
15461  for(int step = 0; step < maxsteps; ++step) {
15462  bool changed = false;
15463  for(int i = 0; i < 3; ++i) {
15464  if ( err[i] >= tol ) {
15465  changed = true;
15466  // evaluate
15467  complex<IkReal> x = roots[i] + coeffs[0];
15468  for(int j = 1; j < 3; ++j) {
15469  x = roots[i] * x + coeffs[j];
15470  }
15471  for(int j = 0; j < 3; ++j) {
15472  if( i != j ) {
15473  if( roots[i] != roots[j] ) {
15474  x /= (roots[i] - roots[j]);
15475  }
15476  }
15477  }
15478  roots[i] -= x;
15479  err[i] = abs(x);
15480  }
15481  }
15482  if( !changed ) {
15483  break;
15484  }
15485  }
15486 
15487  numroots = 0;
15488  bool visited[3] = {false};
15489  for(int i = 0; i < 3; ++i) {
15490  if( !visited[i] ) {
15491  // might be a multiple root, in which case it will have more error than the other roots
15492  // find any neighboring roots, and take the average
15493  complex<IkReal> newroot=roots[i];
15494  int n = 1;
15495  for(int j = i+1; j < 3; ++j) {
15496  // care about error in real much more than imaginary
15497  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
15498  newroot += roots[j];
15499  n += 1;
15500  visited[j] = true;
15501  }
15502  }
15503  if( n > 1 ) {
15504  newroot /= n;
15505  }
15506  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
15507  if( IKabs(imag(newroot)) < tolsqrt ) {
15508  rawroots[numroots++] = real(newroot);
15509  }
15510  }
15511  }
15512 }
15513 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
15514  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
15515  if( det < 0 ) {
15516  numroots=0;
15517  }
15518  else if( det == 0 ) {
15519  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
15520  numroots = 1;
15521  }
15522  else {
15523  det = IKsqrt(det);
15524  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
15525  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
15526  numroots = 2;
15527  }
15528 }
15529 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
15530 {
15531  using std::complex;
15532  if( rawcoeffs[0] == 0 ) {
15533  // solve with one reduced degree
15534  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
15535  return;
15536  }
15537  IKFAST_ASSERT(rawcoeffs[0] != 0);
15538  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
15539  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
15540  complex<IkReal> coeffs[4];
15541  const int maxsteps = 110;
15542  for(int i = 0; i < 4; ++i) {
15543  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
15544  }
15545  complex<IkReal> roots[4];
15546  IkReal err[4];
15547  roots[0] = complex<IkReal>(1,0);
15548  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
15549  err[0] = 1.0;
15550  err[1] = 1.0;
15551  for(int i = 2; i < 4; ++i) {
15552  roots[i] = roots[i-1]*roots[1];
15553  err[i] = 1.0;
15554  }
15555  for(int step = 0; step < maxsteps; ++step) {
15556  bool changed = false;
15557  for(int i = 0; i < 4; ++i) {
15558  if ( err[i] >= tol ) {
15559  changed = true;
15560  // evaluate
15561  complex<IkReal> x = roots[i] + coeffs[0];
15562  for(int j = 1; j < 4; ++j) {
15563  x = roots[i] * x + coeffs[j];
15564  }
15565  for(int j = 0; j < 4; ++j) {
15566  if( i != j ) {
15567  if( roots[i] != roots[j] ) {
15568  x /= (roots[i] - roots[j]);
15569  }
15570  }
15571  }
15572  roots[i] -= x;
15573  err[i] = abs(x);
15574  }
15575  }
15576  if( !changed ) {
15577  break;
15578  }
15579  }
15580 
15581  numroots = 0;
15582  bool visited[4] = {false};
15583  for(int i = 0; i < 4; ++i) {
15584  if( !visited[i] ) {
15585  // might be a multiple root, in which case it will have more error than the other roots
15586  // find any neighboring roots, and take the average
15587  complex<IkReal> newroot=roots[i];
15588  int n = 1;
15589  for(int j = i+1; j < 4; ++j) {
15590  // care about error in real much more than imaginary
15591  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
15592  newroot += roots[j];
15593  n += 1;
15594  visited[j] = true;
15595  }
15596  }
15597  if( n > 1 ) {
15598  newroot /= n;
15599  }
15600  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
15601  if( IKabs(imag(newroot)) < tolsqrt ) {
15602  rawroots[numroots++] = real(newroot);
15603  }
15604  }
15605  }
15606 }
15607 };
15608 
15609 
15612 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
15613 IKSolver solver;
15614 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
15615 }
15616 
15617 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
15618 IKSolver solver;
15619 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
15620 }
15621 
15622 IKFAST_API const char* GetKinematicsHash() { return "96beac7e82b827e22012aa525faae055"; }
15623 
15624 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
15625 
15626 #ifdef IKFAST_NAMESPACE
15627 } // end namespace
15628 #endif
15629 
15630 #ifndef IKFAST_NO_MAIN
15631 #include <stdio.h>
15632 #include <stdlib.h>
15633 #ifdef IKFAST_NAMESPACE
15634 using namespace IKFAST_NAMESPACE;
15635 #endif
15636 int main(int argc, char** argv)
15637 {
15638  if( argc != 12+GetNumFreeParameters()+1 ) {
15639  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
15640  "Returns the ik solutions given the transformation of the end effector specified by\n"
15641  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
15642  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
15643  return 1;
15644  }
15645 
15646  IkSolutionList<IkReal> solutions;
15647  std::vector<IkReal> vfree(GetNumFreeParameters());
15648  IkReal eerot[9],eetrans[3];
15649  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
15650  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
15651  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
15652  for(std::size_t i = 0; i < vfree.size(); ++i)
15653  vfree[i] = atof(argv[13+i]);
15654  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
15655 
15656  if( !bSuccess ) {
15657  fprintf(stderr,"Failed to get ik solution\n");
15658  return -1;
15659  }
15660 
15661  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
15662  std::vector<IkReal> solvalues(GetNumJoints());
15663  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
15664  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
15665  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
15666  std::vector<IkReal> vsolfree(sol.GetFree().size());
15667  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
15668  for( std::size_t j = 0; j < solvalues.size(); ++j)
15669  printf("%.15f, ", solvalues[j]);
15670  printf("\n");
15671  }
15672  return 0;
15673 }
15674 
15675 #endif
IKabs
float IKabs(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:92
ikfast
Definition: ikfast.h:45
IKlog
float IKlog(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:98
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:368
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:247
IKPI_2
#define IKPI_2
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:62
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:177
IKsin
float IKsin(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:169
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15612
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
IKsqrt
float IKsqrt(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:175
ikfast::IkSolutionBase::GetFree
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...
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:3165
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:113
IKacos
float IKacos(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:155
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
ikfast::IkSolutionListBase::AddSolution
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
IKPI
#define IKPI
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:61
IKsqr
float IKsqr(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:95
main
int main(int argc, char **argv)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15636
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15435
CheckValue::valid
bool valid
Definition: ikfast.cpp:208
IKtan
float IKtan(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:173
IKsign
float IKsign(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:226
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:300
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:118
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:212
dgetrs_
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)
IKasin
float IKasin(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:122
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:366
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKcos
float IKcos(float f)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:171
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:381
pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15529
step
unsigned int step
IKatan2
float IKatan2(float fy, float fx)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:180
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15622
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:26
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
IKfmod
float IKfmod(float x, float y)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:138
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15624
GetIkType
IKFAST_API int GetIkType()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:372
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:370
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:50
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:108
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:103
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15617
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:15513
dgeev_
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)
x
double x
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:367
IK2PI
#define IK2PI
Definition: choreo_kr5_arc_workspace_rail_robot_manipulator_ikfast_solver.cpp:60

choreo_kr5_arc_workspace_ikfast_rail_robot_manipulator_plugin
Author(s):
autogenerated on Thu Jul 18 2019 03:58:46