prbt_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
25 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
26 using namespace ikfast;
27 
28 // check if the included ikfast version matches what this file was compiled with
29 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
30 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a);
31 
32 #include <cmath>
33 #include <vector>
34 #include <limits>
35 #include <algorithm>
36 #include <complex>
37 
38 #ifndef IKFAST_ASSERT
39 #include <stdexcept>
40 #include <sstream>
41 #include <iostream>
42 
43 #ifdef _MSC_VER
44 #ifndef __PRETTY_FUNCTION__
45 #define __PRETTY_FUNCTION__ __FUNCDNAME__
46 #endif
47 #endif
48 
49 #ifndef __PRETTY_FUNCTION__
50 #define __PRETTY_FUNCTION__ __func__
51 #endif
52 
53 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
54 
55 #endif
56 
57 #if defined(_MSC_VER)
58 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
59 #else
60 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
61 #endif
62 
63 #define IK2PI ((IkReal)6.28318530717959)
64 #define IKPI ((IkReal)3.14159265358979)
65 #define IKPI_2 ((IkReal)1.57079632679490)
66 
67 #ifdef _MSC_VER
68 #ifndef isnan
69 #define isnan _isnan
70 #endif
71 #ifndef isinf
72 #define isinf _isinf
73 #endif
74 //#ifndef isfinite
75 //#define isfinite _isfinite
76 //#endif
77 #endif // _MSC_VER
78 
79 // lapack routines
80 extern "C" {
81  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
82  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
83  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
84  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
85  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);
86  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
87 }
88 
89 using namespace std; // necessary to get std math routines
90 
91 #ifdef IKFAST_NAMESPACE
92 namespace IKFAST_NAMESPACE {
93 #endif
94 
95 inline float IKabs(float f) { return fabsf(f); }
96 inline double IKabs(double f) { return fabs(f); }
97 
98 inline float IKsqr(float f) { return f*f; }
99 inline double IKsqr(double f) { return f*f; }
100 
101 inline float IKlog(float f) { return logf(f); }
102 inline double IKlog(double f) { return log(f); }
103 
104 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
105 #ifndef IKFAST_SINCOS_THRESH
106 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
107 #endif
108 
109 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
110 #ifndef IKFAST_ATAN2_MAGTHRESH
111 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
112 #endif
113 
114 // minimum distance of separate solutions
115 #ifndef IKFAST_SOLUTION_THRESH
116 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
117 #endif
118 
119 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
120 #ifndef IKFAST_EVALCOND_THRESH
121 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
122 #endif
123 
124 
125 inline float IKasin(float f)
126 {
127 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
128 if( f <= -1 ) return float(-IKPI_2);
129 else if( f >= 1 ) return float(IKPI_2);
130 return asinf(f);
131 }
132 inline double IKasin(double f)
133 {
134 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
135 if( f <= -1 ) return -IKPI_2;
136 else if( f >= 1 ) return IKPI_2;
137 return asin(f);
138 }
139 
140 // return positive value in [0,y)
141 inline float IKfmod(float x, float y)
142 {
143  while(x < 0) {
144  x += y;
145  }
146  return fmodf(x,y);
147 }
148 
149 // return positive value in [0,y)
150 inline double IKfmod(double x, double y)
151 {
152  while(x < 0) {
153  x += y;
154  }
155  return fmod(x,y);
156 }
157 
158 inline float IKacos(float f)
159 {
160 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
161 if( f <= -1 ) return float(IKPI);
162 else if( f >= 1 ) return float(0);
163 return acosf(f);
164 }
165 inline double IKacos(double f)
166 {
167 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
168 if( f <= -1 ) return IKPI;
169 else if( f >= 1 ) return 0;
170 return acos(f);
171 }
172 inline float IKsin(float f) { return sinf(f); }
173 inline double IKsin(double f) { return sin(f); }
174 inline float IKcos(float f) { return cosf(f); }
175 inline double IKcos(double f) { return cos(f); }
176 inline float IKtan(float f) { return tanf(f); }
177 inline double IKtan(double f) { return tan(f); }
178 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
179 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
180 inline float IKatan2Simple(float fy, float fx) {
181  return atan2f(fy,fx);
182 }
183 inline float IKatan2(float fy, float fx) {
184  if( isnan(fy) ) {
185  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
186  return float(IKPI_2);
187  }
188  else if( isnan(fx) ) {
189  return 0;
190  }
191  return atan2f(fy,fx);
192 }
193 inline double IKatan2Simple(double fy, double fx) {
194  return atan2(fy,fx);
195 }
196 inline double IKatan2(double fy, double fx) {
197  if( isnan(fy) ) {
198  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
199  return IKPI_2;
200  }
201  else if( isnan(fx) ) {
202  return 0;
203  }
204  return atan2(fy,fx);
205 }
206 
207 template <typename T>
208 struct CheckValue
209 {
210  T value;
211  bool valid;
212 };
213 
214 template <typename T>
215 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
216 {
217  CheckValue<T> ret;
218  ret.valid = false;
219  ret.value = 0;
220  if( !isnan(fy) && !isnan(fx) ) {
221  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
222  ret.value = IKatan2Simple(fy,fx);
223  ret.valid = true;
224  }
225  }
226  return ret;
227 }
228 
229 inline float IKsign(float f) {
230  if( f > 0 ) {
231  return float(1);
232  }
233  else if( f < 0 ) {
234  return float(-1);
235  }
236  return 0;
237 }
238 
239 inline double IKsign(double f) {
240  if( f > 0 ) {
241  return 1.0;
242  }
243  else if( f < 0 ) {
244  return -1.0;
245  }
246  return 0;
247 }
248 
249 template <typename T>
250 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
251 {
252  CheckValue<T> ret;
253  ret.valid = true;
254  if( n == 0 ) {
255  ret.value = 1.0;
256  return ret;
257  }
258  else if( n == 1 )
259  {
260  ret.value = f;
261  return ret;
262  }
263  else if( n < 0 )
264  {
265  if( f == 0 )
266  {
267  ret.valid = false;
268  ret.value = (T)1.0e30;
269  return ret;
270  }
271  if( n == -1 ) {
272  ret.value = T(1.0)/f;
273  return ret;
274  }
275  }
276 
277  int num = n > 0 ? n : -n;
278  if( num == 2 ) {
279  ret.value = f*f;
280  }
281  else if( num == 3 ) {
282  ret.value = f*f*f;
283  }
284  else {
285  ret.value = 1.0;
286  while(num>0) {
287  if( num & 1 ) {
288  ret.value *= f;
289  }
290  num >>= 1;
291  f *= f;
292  }
293  }
294 
295  if( n < 0 ) {
296  ret.value = T(1.0)/ret.value;
297  }
298  return ret;
299 }
300 
303 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
304 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46;
305 x0=IKcos(j[0]);
306 x1=IKcos(j[1]);
307 x2=IKcos(j[2]);
308 x3=IKsin(j[1]);
309 x4=IKsin(j[2]);
310 x5=IKsin(j[3]);
311 x6=IKcos(j[3]);
312 x7=IKsin(j[0]);
313 x8=IKcos(j[5]);
314 x9=IKsin(j[5]);
315 x10=IKsin(j[4]);
316 x11=IKcos(j[4]);
317 x12=((1.0)*x7);
318 x13=((1.0)*x6);
319 x14=((0.307)*x7);
320 x15=((0.084)*x3);
321 x16=((0.084)*x7);
322 x17=((1.0)*x0);
323 x18=((1.0)*x10);
324 x19=((0.35)*x3);
325 x20=((0.084)*x6);
326 x21=((0.307)*x0);
327 x22=((0.084)*x0);
328 x23=(x1*x2);
329 x24=(x1*x4);
330 x25=((-1.0)*x6);
331 x26=(x2*x3);
332 x27=(x5*x7);
333 x28=(x3*x4);
334 x29=(x10*x6);
335 x30=(x17*x5);
336 x31=((1.0)*x26);
337 x32=((((-1.0)*x31))+x24);
338 x33=((((-1.0)*x24))+x31);
339 x34=((((1.0)*x23))+(((1.0)*x28)));
340 x35=(x11*x32);
341 x36=(x33*x5);
342 x37=(x17*(((((-1.0)*x24))+x26)));
343 x38=(x12*(((((-1.0)*x24))+x26)));
344 x39=(x17*(((((-1.0)*x23))+(((-1.0)*x28)))));
345 x40=(x12*(((((-1.0)*x23))+(((-1.0)*x28)))));
346 x41=(x10*x38);
347 x42=(x39*x5);
348 x43=(x39*x6);
349 x44=(((x0*x6))+((x40*x5)));
350 x45=(x30+((x25*x40)));
351 x46=(x11*x45);
352 eerot[0]=(((x9*(((((-1.0)*x12*x6))+x42))))+((x8*((((x10*x37))+((x11*(((((-1.0)*x27))+((x25*x39)))))))))));
353 eerot[1]=((((-1.0)*x9*((((x18*x37))+(((1.0)*x11*(((((-1.0)*x12*x5))+(((-1.0)*x13*x39))))))))))+((x8*((x42+((x25*x7)))))));
354 eerot[2]=(((x10*((x43+x27))))+((x11*x37)));
355 IkReal x47=((1.0)*x24);
356 eetrans[0]=(((x21*x26))+((x0*x19))+((x11*(((((-1.0)*x22*x47))+((x0*x15*x2))))))+(((-1.0)*x21*x47))+((x10*((((x20*x39))+((x16*x5)))))));
357 eerot[3]=(((x44*x9))+((x8*((x46+x41)))));
358 eerot[4]=(((x9*(((((-1.0)*x41))+(((-1.0)*x46))))))+((x44*x8)));
359 eerot[5]=(((x10*(((((-1.0)*x30))+((x40*x6))))))+((x11*x38)));
360 IkReal x48=((1.0)*x24);
361 eetrans[1]=(((x19*x7))+((x14*x26))+((x10*((((x20*x40))+(((-1.0)*x22*x5))))))+((x11*(((((-1.0)*x16*x48))+((x15*x2*x7))))))+(((-1.0)*x14*x48)));
362 eerot[6]=(((x8*((((x10*x34))+((x35*x6))))))+((x36*x9)));
363 eerot[7]=(((x9*(((((-1.0)*x18*x34))+(((-1.0)*x13*x35))))))+((x36*x8)));
364 eerot[8]=(((x29*x33))+((x11*x34)));
365 eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11*(((((0.084)*x23))+((x15*x4))))))+(((0.307)*x23))+(((0.307)*x28)));
366 }
367 
368 IKFAST_API int GetNumFreeParameters() { return 0; }
369 IKFAST_API int* GetFreeParameters() { return NULL; }
370 IKFAST_API int GetNumJoints() { return 6; }
371 
372 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
373 
374 IKFAST_API int GetIkType() { return 0x67000001; }
375 
376 class IKSolver {
377 public:
378 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
379 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
380 
381 IkReal j100, cj100, sj100;
382 unsigned char _ij100[2], _nj100;
383 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
384 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
385 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
386  solutions.Clear();
387 r00 = eerot[0*3+0];
388 r01 = eerot[0*3+1];
389 r02 = eerot[0*3+2];
390 r10 = eerot[1*3+0];
391 r11 = eerot[1*3+1];
392 r12 = eerot[1*3+2];
393 r20 = eerot[2*3+0];
394 r21 = eerot[2*3+1];
395 r22 = eerot[2*3+2];
396 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
397 
398 new_r00=r00;
399 new_r01=r01;
400 new_r02=r02;
401 new_px=(px+(((-0.084)*r02)));
402 new_r10=r10;
403 new_r11=r11;
404 new_r12=r12;
405 new_py=(py+(((-0.084)*r12)));
406 new_r20=r20;
407 new_r21=r21;
408 new_r22=r22;
409 new_pz=((-0.2604)+pz+(((-0.084)*r22)));
410 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
411 IkReal x49=((1.0)*px);
412 IkReal x50=((1.0)*pz);
413 IkReal x51=((1.0)*py);
414 pp=((px*px)+(py*py)+(pz*pz));
415 npx=(((px*r00))+((py*r10))+((pz*r20)));
416 npy=(((px*r01))+((py*r11))+((pz*r21)));
417 npz=(((px*r02))+((py*r12))+((pz*r22)));
418 rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
419 rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
420 rxp0_2=((((-1.0)*r10*x49))+((py*r00)));
421 rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
422 rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
423 rxp1_2=((((-1.0)*r11*x49))+((py*r01)));
424 rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
425 rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
426 rxp2_2=((((-1.0)*r12*x49))+((py*r02)));
427 {
428 IkReal j2array[2], cj2array[2], sj2array[2];
429 bool j2valid[2]={false};
430 _nj2 = 2;
431 cj2array[0]=((-1.00860400186133)+(((4.65332712889716)*pp)));
432 if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
433 {
434  j2valid[0] = j2valid[1] = true;
435  j2array[0] = IKacos(cj2array[0]);
436  sj2array[0] = IKsin(j2array[0]);
437  cj2array[1] = cj2array[0];
438  j2array[1] = -j2array[0];
439  sj2array[1] = -sj2array[0];
440 }
441 else if( isnan(cj2array[0]) )
442 {
443  // probably any value will work
444  j2valid[0] = true;
445  cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
446 }
447 for(int ij2 = 0; ij2 < 2; ++ij2)
448 {
449 if( !j2valid[ij2] )
450 {
451  continue;
452 }
453 _ij2[0] = ij2; _ij2[1] = -1;
454 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
455 {
456 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
457 {
458  j2valid[iij2]=false; _ij2[1] = iij2; break;
459 }
460 }
461 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
462 
463 {
464 IkReal j0eval[1];
465 j0eval[0]=((IKabs(px))+(IKabs(py)));
466 if( IKabs(j0eval[0]) < 0.0000010000000000 )
467 {
468 {
469 IkReal j1eval[2];
470 j1eval[0]=((IKabs(sj2))+(((3.25732899022801)*(IKabs(((0.35)+(((0.307)*cj2))))))));
471 j1eval[1]=((1.29974853844603)+(sj2*sj2)+(cj2*cj2)+(((2.28013029315961)*cj2)));
472 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
473 {
474 continue; // no branches [j0, j1]
475 
476 } else
477 {
478 {
479 IkReal j1array[2], cj1array[2], sj1array[2];
480 bool j1valid[2]={false};
481 _nj1 = 2;
482 IkReal x52=((0.35)+(((0.307)*cj2)));
483 CheckValue<IkReal> x55 = IKatan2WithCheck(IkReal(x52),IkReal(((0.307)*sj2)),IKFAST_ATAN2_MAGTHRESH);
484 if(!x55.valid){
485 continue;
486 }
487 IkReal x53=((1.0)*(x55.value));
488 if((((((0.094249)*(sj2*sj2)))+(x52*x52))) < -0.00001)
489 continue;
490 CheckValue<IkReal> x56=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.094249)*(sj2*sj2)))+(x52*x52)))),-1);
491 if(!x56.valid){
492 continue;
493 }
494 if( ((pz*(x56.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x56.value))) > 1+IKFAST_SINCOS_THRESH )
495  continue;
496 IkReal x54=IKasin((pz*(x56.value)));
497 j1array[0]=(x54+(((-1.0)*x53)));
498 sj1array[0]=IKsin(j1array[0]);
499 cj1array[0]=IKcos(j1array[0]);
500 j1array[1]=((3.14159265358979)+(((-1.0)*x53))+(((-1.0)*x54)));
501 sj1array[1]=IKsin(j1array[1]);
502 cj1array[1]=IKcos(j1array[1]);
503 if( j1array[0] > IKPI )
504 {
505  j1array[0]-=IK2PI;
506 }
507 else if( j1array[0] < -IKPI )
508 { j1array[0]+=IK2PI;
509 }
510 j1valid[0] = true;
511 if( j1array[1] > IKPI )
512 {
513  j1array[1]-=IK2PI;
514 }
515 else if( j1array[1] < -IKPI )
516 { j1array[1]+=IK2PI;
517 }
518 j1valid[1] = true;
519 for(int ij1 = 0; ij1 < 2; ++ij1)
520 {
521 if( !j1valid[ij1] )
522 {
523  continue;
524 }
525 _ij1[0] = ij1; _ij1[1] = -1;
526 for(int iij1 = ij1+1; iij1 < 2; ++iij1)
527 {
528 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
529 {
530  j1valid[iij1]=false; _ij1[1] = iij1; break;
531 }
532 }
533 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
534 
535 // The remainder of this scope is removed as it does not return a solution.
536 // This is the branch (IKabs(px))+(IKabs(py)), from the kinematics we know,
537 // that j0 is free. So we simply choose 4 values and pass directly to the
538 // rotationfunction (orientation computation is unchanged).
539 _nj0=4;
540 for(int ij0=1; ij0 < _nj0; ++ij0)
541 {
542 j0=(IkReal)(ij0 - 1) * (3.14159265358979) / 2.; // -pi/2, 0, pi/2, pi
543 cj0=IKcos(j0);
544 sj0=IKsin(j0);
545 _ij0[0] = ij0;
546 rotationfunction0(solutions);
547 }
548 
549 }
550 }
551 
552 }
553 
554 }
555 
556 } else
557 {
558 {
559 IkReal j0array[2], cj0array[2], sj0array[2];
560 bool j0valid[2]={false};
561 _nj0 = 2;
562 CheckValue<IkReal> x1691 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
563 if(!x1691.valid){
564 continue;
565 }
566 IkReal x1690=x1691.value;
567 j0array[0]=((-1.0)*x1690);
568 sj0array[0]=IKsin(j0array[0]);
569 cj0array[0]=IKcos(j0array[0]);
570 j0array[1]=((3.14159265358979)+(((-1.0)*x1690)));
571 sj0array[1]=IKsin(j0array[1]);
572 cj0array[1]=IKcos(j0array[1]);
573 if( j0array[0] > IKPI )
574 {
575  j0array[0]-=IK2PI;
576 }
577 else if( j0array[0] < -IKPI )
578 { j0array[0]+=IK2PI;
579 }
580 j0valid[0] = true;
581 if( j0array[1] > IKPI )
582 {
583  j0array[1]-=IK2PI;
584 }
585 else if( j0array[1] < -IKPI )
586 { j0array[1]+=IK2PI;
587 }
588 j0valid[1] = true;
589 for(int ij0 = 0; ij0 < 2; ++ij0)
590 {
591 if( !j0valid[ij0] )
592 {
593  continue;
594 }
595 _ij0[0] = ij0; _ij0[1] = -1;
596 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
597 {
598 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
599 {
600  j0valid[iij0]=false; _ij0[1] = iij0; break;
601 }
602 }
603 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
604 
605 {
606 IkReal j1eval[3];
607 IkReal x1692=((307000.0)*cj2);
608 IkReal x1693=(cj0*px);
609 IkReal x1694=((307000.0)*sj2);
610 IkReal x1695=(py*sj0);
611 j1eval[0]=((-1.00860400186133)+(((-1.0)*cj2)));
612 j1eval[1]=((IKabs(((((-1.0)*pz*x1692))+(((-350000.0)*pz))+((x1693*x1694))+((x1694*x1695)))))+(IKabs(((((-1.0)*x1692*x1693))+(((-1.0)*x1692*x1695))+(((-1.0)*pz*x1694))+(((-350000.0)*x1693))+(((-350000.0)*x1695))))));
613 j1eval[2]=IKsign(((-216749.0)+(((-214900.0)*cj2))));
614 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
615 {
616 {
617 IkReal j1eval[3];
618 IkReal x1696=(py*sj0);
619 IkReal x1697=((1000.0)*pz);
620 IkReal x1698=(pz*sj2);
621 IkReal x1699=(cj0*px);
622 IkReal x1700=(cj2*x1699);
623 j1eval[0]=((((-1.0)*x1700))+x1698+(((-1.0)*cj2*x1696))+(((-1.1400651465798)*x1696))+(((-1.1400651465798)*x1699)));
624 j1eval[1]=((IKabs(((((-1.0)*x1696*x1697))+(((-1.0)*x1697*x1699))+(((94.249)*cj2*sj2))+(((107.45)*sj2)))))+(IKabs(((-122.5)+(((-94.249)*(cj2*cj2)))+(((-214.9)*cj2))+((pz*x1697))))));
625 j1eval[2]=IKsign(((((-307.0)*x1700))+(((-350.0)*x1699))+(((-350.0)*x1696))+(((-307.0)*cj2*x1696))+(((307.0)*x1698))));
626 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
627 {
628 {
629 IkReal j1eval[3];
630 IkReal x1701=(cj0*px);
631 IkReal x1702=((1.0)*cj2);
632 IkReal x1703=((7000.0)*pz);
633 IkReal x1704=(py*sj0);
634 IkReal x1705=((2149.0)*cj2);
635 IkReal x1706=(pz*sj2);
636 IkReal x1707=((3070.0)*pp);
637 j1eval[0]=((((-1.1400651465798)*x1704))+(((-1.1400651465798)*x1701))+(((-1.0)*x1702*x1704))+x1706+(((-1.0)*x1701*x1702)));
638 j1eval[1]=((IKabs(((-98.8785)+(((-86.73057)*cj2))+(((-1.0)*cj2*x1707))+(((-3500.0)*pp))+((pz*x1703)))))+(IKabs(((((-1.0)*x1703*x1704))+((sj2*x1707))+(((-1.0)*x1701*x1703))+(((86.73057)*sj2))))));
639 j1eval[2]=IKsign(((((2149.0)*x1706))+(((-2450.0)*x1704))+(((-2450.0)*x1701))+(((-1.0)*x1704*x1705))+(((-1.0)*x1701*x1705))));
640 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
641 {
642 continue; // no branches [j1]
643 
644 } else
645 {
646 {
647 IkReal j1array[1], cj1array[1], sj1array[1];
648 bool j1valid[1]={false};
649 _nj1 = 1;
650 IkReal x1708=((7000.0)*pz);
651 IkReal x1709=(cj0*px);
652 IkReal x1710=(py*sj0);
653 IkReal x1711=((2149.0)*cj2);
654 IkReal x1712=((3070.0)*pp);
655 CheckValue<IkReal> x1713 = IKatan2WithCheck(IkReal(((-98.8785)+(((-86.73057)*cj2))+(((-3500.0)*pp))+(((-1.0)*cj2*x1712))+((pz*x1708)))),IkReal(((((-1.0)*x1708*x1709))+(((-1.0)*x1708*x1710))+((sj2*x1712))+(((86.73057)*sj2)))),IKFAST_ATAN2_MAGTHRESH);
656 if(!x1713.valid){
657 continue;
658 }
659 CheckValue<IkReal> x1714=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1710*x1711))+(((-1.0)*x1709*x1711))+(((-2450.0)*x1709))+(((-2450.0)*x1710))+(((2149.0)*pz*sj2)))),-1);
660 if(!x1714.valid){
661 continue;
662 }
663 j1array[0]=((-1.5707963267949)+(x1713.value)+(((1.5707963267949)*(x1714.value))));
664 sj1array[0]=IKsin(j1array[0]);
665 cj1array[0]=IKcos(j1array[0]);
666 if( j1array[0] > IKPI )
667 {
668  j1array[0]-=IK2PI;
669 }
670 else if( j1array[0] < -IKPI )
671 { j1array[0]+=IK2PI;
672 }
673 j1valid[0] = true;
674 for(int ij1 = 0; ij1 < 1; ++ij1)
675 {
676 if( !j1valid[ij1] )
677 {
678  continue;
679 }
680 _ij1[0] = ij1; _ij1[1] = -1;
681 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
682 {
683 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
684 {
685  j1valid[iij1]=false; _ij1[1] = iij1; break;
686 }
687 }
688 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
689 {
690 IkReal evalcond[5];
691 IkReal x1715=IKcos(j1);
692 IkReal x1716=IKsin(j1);
693 IkReal x1717=(cj0*px);
694 IkReal x1718=(py*sj0);
695 IkReal x1719=((0.307)*x1715);
696 IkReal x1720=((1.0)*x1718);
697 IkReal x1721=(pz*x1715);
698 IkReal x1722=((0.7)*x1716);
699 IkReal x1723=((0.307)*x1716);
700 evalcond[0]=((((-1.0)*pz))+((cj2*x1719))+(((0.35)*x1715))+((sj2*x1723)));
701 evalcond[1]=((((-1.0)*x1715*x1720))+(((-1.0)*x1715*x1717))+(((-0.307)*sj2))+((pz*x1716)));
702 evalcond[2]=((((-0.35)*x1716))+x1717+x1718+(((-1.0)*cj2*x1723))+((sj2*x1719)));
703 evalcond[3]=((-0.028251)+((x1718*x1722))+((x1717*x1722))+(((-1.0)*pp))+(((0.7)*x1721)));
704 evalcond[4]=((0.35)+(((-1.0)*x1716*x1717))+(((-1.0)*x1721))+(((-1.0)*x1716*x1720))+(((0.307)*cj2)));
705 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 )
706 {
707 continue;
708 }
709 }
710 
711 rotationfunction0(solutions);
712 }
713 }
714 
715 }
716 
717 }
718 
719 } else
720 {
721 {
722 IkReal j1array[1], cj1array[1], sj1array[1];
723 bool j1valid[1]={false};
724 _nj1 = 1;
725 IkReal x1724=((307.0)*cj2);
726 IkReal x1725=(cj0*px);
727 IkReal x1726=(py*sj0);
728 IkReal x1727=((1000.0)*pz);
729 CheckValue<IkReal> x1728 = IKatan2WithCheck(IkReal(((-122.5)+(((-94.249)*(cj2*cj2)))+((pz*x1727))+(((-214.9)*cj2)))),IkReal(((((-1.0)*x1726*x1727))+(((94.249)*cj2*sj2))+(((107.45)*sj2))+(((-1.0)*x1725*x1727)))),IKFAST_ATAN2_MAGTHRESH);
730 if(!x1728.valid){
731 continue;
732 }
733 CheckValue<IkReal> x1729=IKPowWithIntegerCheck(IKsign(((((-350.0)*x1726))+(((-350.0)*x1725))+(((307.0)*pz*sj2))+(((-1.0)*x1724*x1725))+(((-1.0)*x1724*x1726)))),-1);
734 if(!x1729.valid){
735 continue;
736 }
737 j1array[0]=((-1.5707963267949)+(x1728.value)+(((1.5707963267949)*(x1729.value))));
738 sj1array[0]=IKsin(j1array[0]);
739 cj1array[0]=IKcos(j1array[0]);
740 if( j1array[0] > IKPI )
741 {
742  j1array[0]-=IK2PI;
743 }
744 else if( j1array[0] < -IKPI )
745 { j1array[0]+=IK2PI;
746 }
747 j1valid[0] = true;
748 for(int ij1 = 0; ij1 < 1; ++ij1)
749 {
750 if( !j1valid[ij1] )
751 {
752  continue;
753 }
754 _ij1[0] = ij1; _ij1[1] = -1;
755 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
756 {
757 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
758 {
759  j1valid[iij1]=false; _ij1[1] = iij1; break;
760 }
761 }
762 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
763 {
764 IkReal evalcond[5];
765 IkReal x1730=IKcos(j1);
766 IkReal x1731=IKsin(j1);
767 IkReal x1732=(cj0*px);
768 IkReal x1733=(py*sj0);
769 IkReal x1734=((0.307)*x1730);
770 IkReal x1735=((1.0)*x1733);
771 IkReal x1736=(pz*x1730);
772 IkReal x1737=((0.7)*x1731);
773 IkReal x1738=((0.307)*x1731);
774 evalcond[0]=(((cj2*x1734))+(((0.35)*x1730))+(((-1.0)*pz))+((sj2*x1738)));
775 evalcond[1]=(((pz*x1731))+(((-0.307)*sj2))+(((-1.0)*x1730*x1735))+(((-1.0)*x1730*x1732)));
776 evalcond[2]=(x1733+x1732+(((-1.0)*cj2*x1738))+(((-0.35)*x1731))+((sj2*x1734)));
777 evalcond[3]=((-0.028251)+(((-1.0)*pp))+(((0.7)*x1736))+((x1732*x1737))+((x1733*x1737)));
778 evalcond[4]=((0.35)+(((-1.0)*x1731*x1735))+(((-1.0)*x1731*x1732))+(((-1.0)*x1736))+(((0.307)*cj2)));
779 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 )
780 {
781 continue;
782 }
783 }
784 
785 rotationfunction0(solutions);
786 }
787 }
788 
789 }
790 
791 }
792 
793 } else
794 {
795 {
796 IkReal j1array[1], cj1array[1], sj1array[1];
797 bool j1valid[1]={false};
798 _nj1 = 1;
799 IkReal x1739=((307000.0)*cj2);
800 IkReal x1740=(cj0*px);
801 IkReal x1741=((307000.0)*sj2);
802 IkReal x1742=(py*sj0);
803 CheckValue<IkReal> x1743=IKPowWithIntegerCheck(IKsign(((-216749.0)+(((-214900.0)*cj2)))),-1);
804 if(!x1743.valid){
805 continue;
806 }
807 CheckValue<IkReal> x1744 = IKatan2WithCheck(IkReal(((((-350000.0)*x1742))+(((-350000.0)*x1740))+(((-1.0)*x1739*x1740))+(((-1.0)*x1739*x1742))+(((-1.0)*pz*x1741)))),IkReal(((((-1.0)*pz*x1739))+(((-350000.0)*pz))+((x1740*x1741))+((x1741*x1742)))),IKFAST_ATAN2_MAGTHRESH);
808 if(!x1744.valid){
809 continue;
810 }
811 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1743.value)))+(x1744.value));
812 sj1array[0]=IKsin(j1array[0]);
813 cj1array[0]=IKcos(j1array[0]);
814 if( j1array[0] > IKPI )
815 {
816  j1array[0]-=IK2PI;
817 }
818 else if( j1array[0] < -IKPI )
819 { j1array[0]+=IK2PI;
820 }
821 j1valid[0] = true;
822 for(int ij1 = 0; ij1 < 1; ++ij1)
823 {
824 if( !j1valid[ij1] )
825 {
826  continue;
827 }
828 _ij1[0] = ij1; _ij1[1] = -1;
829 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
830 {
831 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
832 {
833  j1valid[iij1]=false; _ij1[1] = iij1; break;
834 }
835 }
836 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
837 {
838 IkReal evalcond[5];
839 IkReal x1745=IKcos(j1);
840 IkReal x1746=IKsin(j1);
841 IkReal x1747=(cj0*px);
842 IkReal x1748=(py*sj0);
843 IkReal x1749=((0.307)*x1745);
844 IkReal x1750=((1.0)*x1748);
845 IkReal x1751=(pz*x1745);
846 IkReal x1752=((0.7)*x1746);
847 IkReal x1753=((0.307)*x1746);
848 evalcond[0]=((((0.35)*x1745))+((cj2*x1749))+(((-1.0)*pz))+((sj2*x1753)));
849 evalcond[1]=(((pz*x1746))+(((-0.307)*sj2))+(((-1.0)*x1745*x1750))+(((-1.0)*x1745*x1747)));
850 evalcond[2]=(x1748+x1747+(((-1.0)*cj2*x1753))+((sj2*x1749))+(((-0.35)*x1746)));
851 evalcond[3]=((-0.028251)+((x1747*x1752))+(((-1.0)*pp))+((x1748*x1752))+(((0.7)*x1751)));
852 evalcond[4]=((0.35)+(((-1.0)*x1746*x1747))+(((-1.0)*x1746*x1750))+(((-1.0)*x1751))+(((0.307)*cj2)));
853 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 )
854 {
855 continue;
856 }
857 }
858 
859 rotationfunction0(solutions);
860 }
861 }
862 
863 }
864 
865 }
866 }
867 }
868 
869 }
870 
871 }
872 }
873 }
874 }
875 return solutions.GetNumSolutions()>0;
876 }
877 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
878 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
879 IkReal x70=(r11*sj0);
880 IkReal x71=(cj0*r02);
881 IkReal x72=(r10*sj0);
882 IkReal x73=((1.0)*sj0);
883 IkReal x74=(cj2*sj1);
884 IkReal x75=(cj1*sj2);
885 IkReal x76=(r12*sj0);
886 IkReal x77=(((sj1*sj2))+((cj1*cj2)));
887 IkReal x78=(x75+(((-1.0)*x74)));
888 IkReal x79=(x74+(((-1.0)*x75)));
889 IkReal x80=(sj0*x79);
890 IkReal x81=(cj0*x79);
891 IkReal x82=(cj0*x77);
892 new_r00=(((r00*x82))+((x72*x77))+((r20*x78)));
893 new_r01=(((r21*x78))+((r01*x82))+((x70*x77)));
894 new_r02=(((r22*x78))+((x76*x77))+((x71*x77)));
895 new_r10=((((-1.0)*r00*x73))+((cj0*r10)));
896 new_r11=((((-1.0)*r01*x73))+((cj0*r11)));
897 new_r12=((((-1.0)*r02*x73))+((cj0*r12)));
898 new_r20=(((r00*x81))+((x72*x79))+((r20*x77)));
899 new_r21=(((r21*x77))+((r01*x81))+((x70*x79)));
900 new_r22=(((r22*x77))+((x76*x79))+((x71*x79)));
901 {
902 IkReal j4array[2], cj4array[2], sj4array[2];
903 bool j4valid[2]={false};
904 _nj4 = 2;
905 cj4array[0]=new_r22;
906 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
907 {
908  j4valid[0] = j4valid[1] = true;
909  j4array[0] = IKacos(cj4array[0]);
910  sj4array[0] = IKsin(j4array[0]);
911  cj4array[1] = cj4array[0];
912  j4array[1] = -j4array[0];
913  sj4array[1] = -sj4array[0];
914 }
915 else if( isnan(cj4array[0]) )
916 {
917  // probably any value will work
918  j4valid[0] = true;
919  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
920 }
921 for(int ij4 = 0; ij4 < 2; ++ij4)
922 {
923 if( !j4valid[ij4] )
924 {
925  continue;
926 }
927 _ij4[0] = ij4; _ij4[1] = -1;
928 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
929 {
930 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
931 {
932  j4valid[iij4]=false; _ij4[1] = iij4; break;
933 }
934 }
935 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
936 
937 {
938 IkReal j5eval[3];
939 j5eval[0]=sj4;
940 j5eval[1]=IKsign(sj4);
941 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
942 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
943 {
944 {
945 IkReal j3eval[3];
946 j3eval[0]=sj4;
947 j3eval[1]=IKsign(sj4);
948 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
949 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
950 {
951 {
952 IkReal j3eval[2];
953 j3eval[0]=new_r12;
954 j3eval[1]=sj4;
955 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
956 {
957 {
958 IkReal evalcond[5];
959 bool bgotonextstatement = true;
960 do
961 {
962 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
963 evalcond[1]=new_r02;
964 evalcond[2]=new_r12;
965 evalcond[3]=new_r21;
966 evalcond[4]=new_r20;
967 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 )
968 {
969 bgotonextstatement=false;
970 IkReal j5mul = 1;
971 j5=0;
972 j3mul=-1.0;
973 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 )
974  continue;
975 j3=IKatan2(((-1.0)*new_r01), new_r00);
976 {
977 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
978 vinfos[0].jointtype = 1;
979 vinfos[0].foffset = j0;
980 vinfos[0].indices[0] = _ij0[0];
981 vinfos[0].indices[1] = _ij0[1];
982 vinfos[0].maxsolutions = _nj0;
983 vinfos[1].jointtype = 1;
984 vinfos[1].foffset = j1;
985 vinfos[1].indices[0] = _ij1[0];
986 vinfos[1].indices[1] = _ij1[1];
987 vinfos[1].maxsolutions = _nj1;
988 vinfos[2].jointtype = 1;
989 vinfos[2].foffset = j2;
990 vinfos[2].indices[0] = _ij2[0];
991 vinfos[2].indices[1] = _ij2[1];
992 vinfos[2].maxsolutions = _nj2;
993 vinfos[3].jointtype = 1;
994 vinfos[3].foffset = j3;
995 vinfos[3].fmul = j3mul;
996 vinfos[3].freeind = 0;
997 vinfos[3].maxsolutions = 0;
998 vinfos[4].jointtype = 1;
999 vinfos[4].foffset = j4;
1000 vinfos[4].indices[0] = _ij4[0];
1001 vinfos[4].indices[1] = _ij4[1];
1002 vinfos[4].maxsolutions = _nj4;
1003 vinfos[5].jointtype = 1;
1004 vinfos[5].foffset = j5;
1005 vinfos[5].fmul = j5mul;
1006 vinfos[5].freeind = 0;
1007 vinfos[5].maxsolutions = 0;
1008 std::vector<int> vfree(1);
1009 vfree[0] = 5;
1010 solutions.AddSolution(vinfos,vfree);
1011 }
1012 
1013 }
1014 } while(0);
1015 if( bgotonextstatement )
1016 {
1017 bool bgotonextstatement = true;
1018 do
1019 {
1020 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
1021 evalcond[1]=new_r02;
1022 evalcond[2]=new_r12;
1023 evalcond[3]=new_r21;
1024 evalcond[4]=new_r20;
1025 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 )
1026 {
1027 bgotonextstatement=false;
1028 IkReal j5mul = 1;
1029 j5=0;
1030 j3mul=1.0;
1031 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 )
1032  continue;
1033 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
1034 {
1035 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1036 vinfos[0].jointtype = 1;
1037 vinfos[0].foffset = j0;
1038 vinfos[0].indices[0] = _ij0[0];
1039 vinfos[0].indices[1] = _ij0[1];
1040 vinfos[0].maxsolutions = _nj0;
1041 vinfos[1].jointtype = 1;
1042 vinfos[1].foffset = j1;
1043 vinfos[1].indices[0] = _ij1[0];
1044 vinfos[1].indices[1] = _ij1[1];
1045 vinfos[1].maxsolutions = _nj1;
1046 vinfos[2].jointtype = 1;
1047 vinfos[2].foffset = j2;
1048 vinfos[2].indices[0] = _ij2[0];
1049 vinfos[2].indices[1] = _ij2[1];
1050 vinfos[2].maxsolutions = _nj2;
1051 vinfos[3].jointtype = 1;
1052 vinfos[3].foffset = j3;
1053 vinfos[3].fmul = j3mul;
1054 vinfos[3].freeind = 0;
1055 vinfos[3].maxsolutions = 0;
1056 vinfos[4].jointtype = 1;
1057 vinfos[4].foffset = j4;
1058 vinfos[4].indices[0] = _ij4[0];
1059 vinfos[4].indices[1] = _ij4[1];
1060 vinfos[4].maxsolutions = _nj4;
1061 vinfos[5].jointtype = 1;
1062 vinfos[5].foffset = j5;
1063 vinfos[5].fmul = j5mul;
1064 vinfos[5].freeind = 0;
1065 vinfos[5].maxsolutions = 0;
1066 std::vector<int> vfree(1);
1067 vfree[0] = 5;
1068 solutions.AddSolution(vinfos,vfree);
1069 }
1070 
1071 }
1072 } while(0);
1073 if( bgotonextstatement )
1074 {
1075 bool bgotonextstatement = true;
1076 do
1077 {
1078 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1079 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1080 {
1081 bgotonextstatement=false;
1082 {
1083 IkReal j3eval[1];
1084 new_r21=0;
1085 new_r20=0;
1086 new_r02=0;
1087 new_r12=0;
1088 IkReal x83=new_r22*new_r22;
1089 IkReal x84=((16.0)*new_r10);
1090 IkReal x85=((16.0)*new_r01);
1091 IkReal x86=((16.0)*new_r22);
1092 IkReal x87=((8.0)*new_r11);
1093 IkReal x88=((8.0)*new_r00);
1094 IkReal x89=(x83*x84);
1095 IkReal x90=(x83*x85);
1096 j3eval[0]=((IKabs((x89+(((-1.0)*x84)))))+(IKabs((((new_r22*x88))+(((-1.0)*x83*x87)))))+(IKabs(((((16.0)*new_r00))+((new_r11*x86))+(((-32.0)*new_r00*x83)))))+(IKabs((x85+(((-1.0)*x90)))))+(IKabs((((new_r22*x87))+(((-1.0)*x88)))))+(IKabs(((((16.0)*new_r11*x83))+((new_r00*x86))+(((-32.0)*new_r11)))))+(IKabs((x90+(((-1.0)*x85)))))+(IKabs((x84+(((-1.0)*x89))))));
1097 if( IKabs(j3eval[0]) < 0.0000000100000000 )
1098 {
1099 continue; // no branches [j3, j5]
1100 
1101 } else
1102 {
1103 IkReal op[4+1], zeror[4];
1104 int numroots;
1105 IkReal j3evalpoly[1];
1106 IkReal x91=new_r22*new_r22;
1107 IkReal x92=((16.0)*new_r10);
1108 IkReal x93=(new_r11*new_r22);
1109 IkReal x94=(x91*x92);
1110 IkReal x95=((((8.0)*x93))+(((-8.0)*new_r00)));
1111 op[0]=x95;
1112 op[1]=(x92+(((-1.0)*x94)));
1113 op[2]=((((16.0)*x93))+(((16.0)*new_r00))+(((-32.0)*new_r00*x91)));
1114 op[3]=(x94+(((-1.0)*x92)));
1115 op[4]=x95;
1116 polyroots4(op,zeror,numroots);
1117 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1118 int numsolutions = 0;
1119 for(int ij3 = 0; ij3 < numroots; ++ij3)
1120 {
1121 IkReal htj3 = zeror[ij3];
1122 tempj3array[0]=((2.0)*(atan(htj3)));
1123 for(int kj3 = 0; kj3 < 1; ++kj3)
1124 {
1125 j3array[numsolutions] = tempj3array[kj3];
1126 if( j3array[numsolutions] > IKPI )
1127 {
1128  j3array[numsolutions]-=IK2PI;
1129 }
1130 else if( j3array[numsolutions] < -IKPI )
1131 {
1132  j3array[numsolutions]+=IK2PI;
1133 }
1134 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1135 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1136 numsolutions++;
1137 }
1138 }
1139 bool j3valid[4]={true,true,true,true};
1140 _nj3 = 4;
1141 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1142  {
1143 if( !j3valid[ij3] )
1144 {
1145  continue;
1146 }
1147  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1148 htj3 = IKtan(j3/2);
1149 
1150 IkReal x96=((16.0)*new_r01);
1151 IkReal x97=new_r22*new_r22;
1152 IkReal x98=(new_r00*new_r22);
1153 IkReal x99=((8.0)*x98);
1154 IkReal x100=(new_r11*x97);
1155 IkReal x101=(x96*x97);
1156 IkReal x102=((8.0)*x100);
1157 j3evalpoly[0]=((((-1.0)*x102))+(((htj3*htj3*htj3)*(((((-1.0)*x101))+x96))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x102))+x99))))+((htj3*((x101+(((-1.0)*x96))))))+(((htj3*htj3)*(((((16.0)*x98))+(((16.0)*x100))+(((-32.0)*new_r11))))))+x99);
1158 if( IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1159 {
1160  continue;
1161 }
1162 _ij3[0] = ij3; _ij3[1] = -1;
1163 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1164 {
1165 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1166 {
1167  j3valid[iij3]=false; _ij3[1] = iij3; break;
1168 }
1169 }
1170 {
1171 IkReal j5eval[3];
1172 new_r21=0;
1173 new_r20=0;
1174 new_r02=0;
1175 new_r12=0;
1176 IkReal x103=cj3*cj3;
1177 IkReal x104=(cj3*new_r22);
1178 IkReal x105=((-1.0)+x103+(((-1.0)*x103*(new_r22*new_r22))));
1179 j5eval[0]=x105;
1180 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x104)))))+(IKabs((((new_r00*sj3))+((new_r01*x104))))));
1181 j5eval[2]=IKsign(x105);
1182 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1183 {
1184 {
1185 IkReal j5eval[1];
1186 new_r21=0;
1187 new_r20=0;
1188 new_r02=0;
1189 new_r12=0;
1190 j5eval[0]=new_r22;
1191 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1192 {
1193 {
1194 IkReal j5eval[1];
1195 new_r21=0;
1196 new_r20=0;
1197 new_r02=0;
1198 new_r12=0;
1199 j5eval[0]=sj3;
1200 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1201 {
1202 {
1203 IkReal evalcond[1];
1204 bool bgotonextstatement = true;
1205 do
1206 {
1207 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1208 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1209 {
1210 bgotonextstatement=false;
1211 {
1212 IkReal j5array[1], cj5array[1], sj5array[1];
1213 bool j5valid[1]={false};
1214 _nj5 = 1;
1215 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1216  continue;
1217 j5array[0]=IKatan2(new_r10, new_r11);
1218 sj5array[0]=IKsin(j5array[0]);
1219 cj5array[0]=IKcos(j5array[0]);
1220 if( j5array[0] > IKPI )
1221 {
1222  j5array[0]-=IK2PI;
1223 }
1224 else if( j5array[0] < -IKPI )
1225 { j5array[0]+=IK2PI;
1226 }
1227 j5valid[0] = true;
1228 for(int ij5 = 0; ij5 < 1; ++ij5)
1229 {
1230 if( !j5valid[ij5] )
1231 {
1232  continue;
1233 }
1234 _ij5[0] = ij5; _ij5[1] = -1;
1235 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1236 {
1237 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1238 {
1239  j5valid[iij5]=false; _ij5[1] = iij5; break;
1240 }
1241 }
1242 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1243 {
1244 IkReal evalcond[4];
1245 IkReal x106=IKsin(j5);
1246 IkReal x107=IKcos(j5);
1247 evalcond[0]=x107;
1248 evalcond[1]=((-1.0)*x106);
1249 evalcond[2]=(x106+(((-1.0)*new_r10)));
1250 evalcond[3]=(x107+(((-1.0)*new_r11)));
1251 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 )
1252 {
1253 continue;
1254 }
1255 }
1256 
1257 {
1258 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1259 vinfos[0].jointtype = 1;
1260 vinfos[0].foffset = j0;
1261 vinfos[0].indices[0] = _ij0[0];
1262 vinfos[0].indices[1] = _ij0[1];
1263 vinfos[0].maxsolutions = _nj0;
1264 vinfos[1].jointtype = 1;
1265 vinfos[1].foffset = j1;
1266 vinfos[1].indices[0] = _ij1[0];
1267 vinfos[1].indices[1] = _ij1[1];
1268 vinfos[1].maxsolutions = _nj1;
1269 vinfos[2].jointtype = 1;
1270 vinfos[2].foffset = j2;
1271 vinfos[2].indices[0] = _ij2[0];
1272 vinfos[2].indices[1] = _ij2[1];
1273 vinfos[2].maxsolutions = _nj2;
1274 vinfos[3].jointtype = 1;
1275 vinfos[3].foffset = j3;
1276 vinfos[3].indices[0] = _ij3[0];
1277 vinfos[3].indices[1] = _ij3[1];
1278 vinfos[3].maxsolutions = _nj3;
1279 vinfos[4].jointtype = 1;
1280 vinfos[4].foffset = j4;
1281 vinfos[4].indices[0] = _ij4[0];
1282 vinfos[4].indices[1] = _ij4[1];
1283 vinfos[4].maxsolutions = _nj4;
1284 vinfos[5].jointtype = 1;
1285 vinfos[5].foffset = j5;
1286 vinfos[5].indices[0] = _ij5[0];
1287 vinfos[5].indices[1] = _ij5[1];
1288 vinfos[5].maxsolutions = _nj5;
1289 std::vector<int> vfree(0);
1290 solutions.AddSolution(vinfos,vfree);
1291 }
1292 }
1293 }
1294 
1295 }
1296 } while(0);
1297 if( bgotonextstatement )
1298 {
1299 bool bgotonextstatement = true;
1300 do
1301 {
1302 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1303 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1304 {
1305 bgotonextstatement=false;
1306 {
1307 IkReal j5array[1], cj5array[1], sj5array[1];
1308 bool j5valid[1]={false};
1309 _nj5 = 1;
1310 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
1311  continue;
1312 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1313 sj5array[0]=IKsin(j5array[0]);
1314 cj5array[0]=IKcos(j5array[0]);
1315 if( j5array[0] > IKPI )
1316 {
1317  j5array[0]-=IK2PI;
1318 }
1319 else if( j5array[0] < -IKPI )
1320 { j5array[0]+=IK2PI;
1321 }
1322 j5valid[0] = true;
1323 for(int ij5 = 0; ij5 < 1; ++ij5)
1324 {
1325 if( !j5valid[ij5] )
1326 {
1327  continue;
1328 }
1329 _ij5[0] = ij5; _ij5[1] = -1;
1330 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1331 {
1332 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1333 {
1334  j5valid[iij5]=false; _ij5[1] = iij5; break;
1335 }
1336 }
1337 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1338 {
1339 IkReal evalcond[4];
1340 IkReal x108=IKcos(j5);
1341 IkReal x109=IKsin(j5);
1342 evalcond[0]=x108;
1343 evalcond[1]=(x109+new_r10);
1344 evalcond[2]=(x108+new_r11);
1345 evalcond[3]=((-1.0)*x109);
1346 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 )
1347 {
1348 continue;
1349 }
1350 }
1351 
1352 {
1353 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1354 vinfos[0].jointtype = 1;
1355 vinfos[0].foffset = j0;
1356 vinfos[0].indices[0] = _ij0[0];
1357 vinfos[0].indices[1] = _ij0[1];
1358 vinfos[0].maxsolutions = _nj0;
1359 vinfos[1].jointtype = 1;
1360 vinfos[1].foffset = j1;
1361 vinfos[1].indices[0] = _ij1[0];
1362 vinfos[1].indices[1] = _ij1[1];
1363 vinfos[1].maxsolutions = _nj1;
1364 vinfos[2].jointtype = 1;
1365 vinfos[2].foffset = j2;
1366 vinfos[2].indices[0] = _ij2[0];
1367 vinfos[2].indices[1] = _ij2[1];
1368 vinfos[2].maxsolutions = _nj2;
1369 vinfos[3].jointtype = 1;
1370 vinfos[3].foffset = j3;
1371 vinfos[3].indices[0] = _ij3[0];
1372 vinfos[3].indices[1] = _ij3[1];
1373 vinfos[3].maxsolutions = _nj3;
1374 vinfos[4].jointtype = 1;
1375 vinfos[4].foffset = j4;
1376 vinfos[4].indices[0] = _ij4[0];
1377 vinfos[4].indices[1] = _ij4[1];
1378 vinfos[4].maxsolutions = _nj4;
1379 vinfos[5].jointtype = 1;
1380 vinfos[5].foffset = j5;
1381 vinfos[5].indices[0] = _ij5[0];
1382 vinfos[5].indices[1] = _ij5[1];
1383 vinfos[5].maxsolutions = _nj5;
1384 std::vector<int> vfree(0);
1385 solutions.AddSolution(vinfos,vfree);
1386 }
1387 }
1388 }
1389 
1390 }
1391 } while(0);
1392 if( bgotonextstatement )
1393 {
1394 bool bgotonextstatement = true;
1395 do
1396 {
1397 CheckValue<IkReal> x110=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1398 if(!x110.valid){
1399 continue;
1400 }
1401 if((x110.value) < -0.00001)
1402 continue;
1403 IkReal gconst18=((-1.0)*(IKsqrt(x110.value)));
1404 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1405 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1406 {
1407 bgotonextstatement=false;
1408 {
1409 IkReal j5eval[1];
1410 new_r21=0;
1411 new_r20=0;
1412 new_r02=0;
1413 new_r12=0;
1414 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1415 continue;
1416 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))));
1417 cj3=gconst18;
1418 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1419  continue;
1420 j3=IKacos(gconst18);
1421 CheckValue<IkReal> x111=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1422 if(!x111.valid){
1423 continue;
1424 }
1425 if((x111.value) < -0.00001)
1426 continue;
1427 IkReal gconst18=((-1.0)*(IKsqrt(x111.value)));
1428 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1429 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1430 {
1431 {
1432 IkReal j5array[1], cj5array[1], sj5array[1];
1433 bool j5valid[1]={false};
1434 _nj5 = 1;
1435 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1436 continue;
1437 CheckValue<IkReal> x112=IKPowWithIntegerCheck(gconst18,-1);
1438 if(!x112.valid){
1439 continue;
1440 }
1441 if( IKabs((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x112.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x112.value)))-1) <= IKFAST_SINCOS_THRESH )
1442  continue;
1443 j5array[0]=IKatan2((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x112.value)));
1444 sj5array[0]=IKsin(j5array[0]);
1445 cj5array[0]=IKcos(j5array[0]);
1446 if( j5array[0] > IKPI )
1447 {
1448  j5array[0]-=IK2PI;
1449 }
1450 else if( j5array[0] < -IKPI )
1451 { j5array[0]+=IK2PI;
1452 }
1453 j5valid[0] = true;
1454 for(int ij5 = 0; ij5 < 1; ++ij5)
1455 {
1456 if( !j5valid[ij5] )
1457 {
1458  continue;
1459 }
1460 _ij5[0] = ij5; _ij5[1] = -1;
1461 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1462 {
1463 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1464 {
1465  j5valid[iij5]=false; _ij5[1] = iij5; break;
1466 }
1467 }
1468 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1469 {
1470 IkReal evalcond[8];
1471 IkReal x113=IKcos(j5);
1472 IkReal x114=IKsin(j5);
1473 IkReal x115=((1.0)*gconst18);
1474 if((((1.0)+(((-1.0)*gconst18*x115)))) < -0.00001)
1475 continue;
1476 IkReal x116=IKsqrt(((1.0)+(((-1.0)*gconst18*x115))));
1477 evalcond[0]=x113;
1478 evalcond[1]=((-1.0)*x114);
1479 evalcond[2]=((((-1.0)*x113*x115))+new_r11);
1480 evalcond[3]=((((-1.0)*x114*x115))+new_r10);
1481 evalcond[4]=(((x113*x116))+new_r01);
1482 evalcond[5]=(((x114*x116))+new_r00);
1483 evalcond[6]=((((-1.0)*new_r10*x115))+x114+((new_r00*x116)));
1484 evalcond[7]=((((-1.0)*new_r11*x115))+x113+((new_r01*x116)));
1485 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 )
1486 {
1487 continue;
1488 }
1489 }
1490 
1491 {
1492 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1493 vinfos[0].jointtype = 1;
1494 vinfos[0].foffset = j0;
1495 vinfos[0].indices[0] = _ij0[0];
1496 vinfos[0].indices[1] = _ij0[1];
1497 vinfos[0].maxsolutions = _nj0;
1498 vinfos[1].jointtype = 1;
1499 vinfos[1].foffset = j1;
1500 vinfos[1].indices[0] = _ij1[0];
1501 vinfos[1].indices[1] = _ij1[1];
1502 vinfos[1].maxsolutions = _nj1;
1503 vinfos[2].jointtype = 1;
1504 vinfos[2].foffset = j2;
1505 vinfos[2].indices[0] = _ij2[0];
1506 vinfos[2].indices[1] = _ij2[1];
1507 vinfos[2].maxsolutions = _nj2;
1508 vinfos[3].jointtype = 1;
1509 vinfos[3].foffset = j3;
1510 vinfos[3].indices[0] = _ij3[0];
1511 vinfos[3].indices[1] = _ij3[1];
1512 vinfos[3].maxsolutions = _nj3;
1513 vinfos[4].jointtype = 1;
1514 vinfos[4].foffset = j4;
1515 vinfos[4].indices[0] = _ij4[0];
1516 vinfos[4].indices[1] = _ij4[1];
1517 vinfos[4].maxsolutions = _nj4;
1518 vinfos[5].jointtype = 1;
1519 vinfos[5].foffset = j5;
1520 vinfos[5].indices[0] = _ij5[0];
1521 vinfos[5].indices[1] = _ij5[1];
1522 vinfos[5].maxsolutions = _nj5;
1523 std::vector<int> vfree(0);
1524 solutions.AddSolution(vinfos,vfree);
1525 }
1526 }
1527 }
1528 
1529 } else
1530 {
1531 {
1532 IkReal j5array[1], cj5array[1], sj5array[1];
1533 bool j5valid[1]={false};
1534 _nj5 = 1;
1535 CheckValue<IkReal> x117=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1536 if(!x117.valid){
1537 continue;
1538 }
1539 CheckValue<IkReal> x118 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1540 if(!x118.valid){
1541 continue;
1542 }
1543 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.value)))+(x118.value));
1544 sj5array[0]=IKsin(j5array[0]);
1545 cj5array[0]=IKcos(j5array[0]);
1546 if( j5array[0] > IKPI )
1547 {
1548  j5array[0]-=IK2PI;
1549 }
1550 else if( j5array[0] < -IKPI )
1551 { j5array[0]+=IK2PI;
1552 }
1553 j5valid[0] = true;
1554 for(int ij5 = 0; ij5 < 1; ++ij5)
1555 {
1556 if( !j5valid[ij5] )
1557 {
1558  continue;
1559 }
1560 _ij5[0] = ij5; _ij5[1] = -1;
1561 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1562 {
1563 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1564 {
1565  j5valid[iij5]=false; _ij5[1] = iij5; break;
1566 }
1567 }
1568 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1569 {
1570 IkReal evalcond[8];
1571 IkReal x119=IKcos(j5);
1572 IkReal x120=IKsin(j5);
1573 IkReal x121=((1.0)*gconst18);
1574 if((((1.0)+(((-1.0)*gconst18*x121)))) < -0.00001)
1575 continue;
1576 IkReal x122=IKsqrt(((1.0)+(((-1.0)*gconst18*x121))));
1577 evalcond[0]=x119;
1578 evalcond[1]=((-1.0)*x120);
1579 evalcond[2]=((((-1.0)*x119*x121))+new_r11);
1580 evalcond[3]=((((-1.0)*x120*x121))+new_r10);
1581 evalcond[4]=(new_r01+((x119*x122)));
1582 evalcond[5]=(((x120*x122))+new_r00);
1583 evalcond[6]=((((-1.0)*new_r10*x121))+((new_r00*x122))+x120);
1584 evalcond[7]=(((new_r01*x122))+x119+(((-1.0)*new_r11*x121)));
1585 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 )
1586 {
1587 continue;
1588 }
1589 }
1590 
1591 {
1592 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1593 vinfos[0].jointtype = 1;
1594 vinfos[0].foffset = j0;
1595 vinfos[0].indices[0] = _ij0[0];
1596 vinfos[0].indices[1] = _ij0[1];
1597 vinfos[0].maxsolutions = _nj0;
1598 vinfos[1].jointtype = 1;
1599 vinfos[1].foffset = j1;
1600 vinfos[1].indices[0] = _ij1[0];
1601 vinfos[1].indices[1] = _ij1[1];
1602 vinfos[1].maxsolutions = _nj1;
1603 vinfos[2].jointtype = 1;
1604 vinfos[2].foffset = j2;
1605 vinfos[2].indices[0] = _ij2[0];
1606 vinfos[2].indices[1] = _ij2[1];
1607 vinfos[2].maxsolutions = _nj2;
1608 vinfos[3].jointtype = 1;
1609 vinfos[3].foffset = j3;
1610 vinfos[3].indices[0] = _ij3[0];
1611 vinfos[3].indices[1] = _ij3[1];
1612 vinfos[3].maxsolutions = _nj3;
1613 vinfos[4].jointtype = 1;
1614 vinfos[4].foffset = j4;
1615 vinfos[4].indices[0] = _ij4[0];
1616 vinfos[4].indices[1] = _ij4[1];
1617 vinfos[4].maxsolutions = _nj4;
1618 vinfos[5].jointtype = 1;
1619 vinfos[5].foffset = j5;
1620 vinfos[5].indices[0] = _ij5[0];
1621 vinfos[5].indices[1] = _ij5[1];
1622 vinfos[5].maxsolutions = _nj5;
1623 std::vector<int> vfree(0);
1624 solutions.AddSolution(vinfos,vfree);
1625 }
1626 }
1627 }
1628 
1629 }
1630 
1631 }
1632 
1633 }
1634 } while(0);
1635 if( bgotonextstatement )
1636 {
1637 bool bgotonextstatement = true;
1638 do
1639 {
1640 CheckValue<IkReal> x123=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1641 if(!x123.valid){
1642 continue;
1643 }
1644 if((x123.value) < -0.00001)
1645 continue;
1646 IkReal gconst18=((-1.0)*(IKsqrt(x123.value)));
1647 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1648 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1649 {
1650 bgotonextstatement=false;
1651 {
1652 IkReal j5eval[1];
1653 new_r21=0;
1654 new_r20=0;
1655 new_r02=0;
1656 new_r12=0;
1657 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1658 continue;
1659 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))));
1660 cj3=gconst18;
1661 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1662  continue;
1663 j3=((-1.0)*(IKacos(gconst18)));
1664 CheckValue<IkReal> x124=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1665 if(!x124.valid){
1666 continue;
1667 }
1668 if((x124.value) < -0.00001)
1669 continue;
1670 IkReal gconst18=((-1.0)*(IKsqrt(x124.value)));
1671 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1672 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1673 {
1674 {
1675 IkReal j5array[1], cj5array[1], sj5array[1];
1676 bool j5valid[1]={false};
1677 _nj5 = 1;
1678 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1679 continue;
1680 CheckValue<IkReal> x125=IKPowWithIntegerCheck(gconst18,-1);
1681 if(!x125.valid){
1682 continue;
1683 }
1684 if( IKabs((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x125.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x125.value)))-1) <= IKFAST_SINCOS_THRESH )
1685  continue;
1686 j5array[0]=IKatan2((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x125.value)));
1687 sj5array[0]=IKsin(j5array[0]);
1688 cj5array[0]=IKcos(j5array[0]);
1689 if( j5array[0] > IKPI )
1690 {
1691  j5array[0]-=IK2PI;
1692 }
1693 else if( j5array[0] < -IKPI )
1694 { j5array[0]+=IK2PI;
1695 }
1696 j5valid[0] = true;
1697 for(int ij5 = 0; ij5 < 1; ++ij5)
1698 {
1699 if( !j5valid[ij5] )
1700 {
1701  continue;
1702 }
1703 _ij5[0] = ij5; _ij5[1] = -1;
1704 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1705 {
1706 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1707 {
1708  j5valid[iij5]=false; _ij5[1] = iij5; break;
1709 }
1710 }
1711 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1712 {
1713 IkReal evalcond[8];
1714 IkReal x126=IKcos(j5);
1715 IkReal x127=IKsin(j5);
1716 IkReal x128=((1.0)*gconst18);
1717 if((((1.0)+(((-1.0)*gconst18*x128)))) < -0.00001)
1718 continue;
1719 IkReal x129=IKsqrt(((1.0)+(((-1.0)*gconst18*x128))));
1720 IkReal x130=((1.0)*x129);
1721 evalcond[0]=x126;
1722 evalcond[1]=((-1.0)*x127);
1723 evalcond[2]=((((-1.0)*x126*x128))+new_r11);
1724 evalcond[3]=(new_r10+(((-1.0)*x127*x128)));
1725 evalcond[4]=((((-1.0)*x126*x130))+new_r01);
1726 evalcond[5]=(new_r00+(((-1.0)*x127*x130)));
1727 evalcond[6]=((((-1.0)*new_r10*x128))+(((-1.0)*new_r00*x130))+x127);
1728 evalcond[7]=((((-1.0)*new_r01*x130))+x126+(((-1.0)*new_r11*x128)));
1729 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 )
1730 {
1731 continue;
1732 }
1733 }
1734 
1735 {
1736 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1737 vinfos[0].jointtype = 1;
1738 vinfos[0].foffset = j0;
1739 vinfos[0].indices[0] = _ij0[0];
1740 vinfos[0].indices[1] = _ij0[1];
1741 vinfos[0].maxsolutions = _nj0;
1742 vinfos[1].jointtype = 1;
1743 vinfos[1].foffset = j1;
1744 vinfos[1].indices[0] = _ij1[0];
1745 vinfos[1].indices[1] = _ij1[1];
1746 vinfos[1].maxsolutions = _nj1;
1747 vinfos[2].jointtype = 1;
1748 vinfos[2].foffset = j2;
1749 vinfos[2].indices[0] = _ij2[0];
1750 vinfos[2].indices[1] = _ij2[1];
1751 vinfos[2].maxsolutions = _nj2;
1752 vinfos[3].jointtype = 1;
1753 vinfos[3].foffset = j3;
1754 vinfos[3].indices[0] = _ij3[0];
1755 vinfos[3].indices[1] = _ij3[1];
1756 vinfos[3].maxsolutions = _nj3;
1757 vinfos[4].jointtype = 1;
1758 vinfos[4].foffset = j4;
1759 vinfos[4].indices[0] = _ij4[0];
1760 vinfos[4].indices[1] = _ij4[1];
1761 vinfos[4].maxsolutions = _nj4;
1762 vinfos[5].jointtype = 1;
1763 vinfos[5].foffset = j5;
1764 vinfos[5].indices[0] = _ij5[0];
1765 vinfos[5].indices[1] = _ij5[1];
1766 vinfos[5].maxsolutions = _nj5;
1767 std::vector<int> vfree(0);
1768 solutions.AddSolution(vinfos,vfree);
1769 }
1770 }
1771 }
1772 
1773 } else
1774 {
1775 {
1776 IkReal j5array[1], cj5array[1], sj5array[1];
1777 bool j5valid[1]={false};
1778 _nj5 = 1;
1779 CheckValue<IkReal> x131=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1780 if(!x131.valid){
1781 continue;
1782 }
1783 CheckValue<IkReal> x132 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1784 if(!x132.valid){
1785 continue;
1786 }
1787 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x131.value)))+(x132.value));
1788 sj5array[0]=IKsin(j5array[0]);
1789 cj5array[0]=IKcos(j5array[0]);
1790 if( j5array[0] > IKPI )
1791 {
1792  j5array[0]-=IK2PI;
1793 }
1794 else if( j5array[0] < -IKPI )
1795 { j5array[0]+=IK2PI;
1796 }
1797 j5valid[0] = true;
1798 for(int ij5 = 0; ij5 < 1; ++ij5)
1799 {
1800 if( !j5valid[ij5] )
1801 {
1802  continue;
1803 }
1804 _ij5[0] = ij5; _ij5[1] = -1;
1805 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1806 {
1807 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1808 {
1809  j5valid[iij5]=false; _ij5[1] = iij5; break;
1810 }
1811 }
1812 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1813 {
1814 IkReal evalcond[8];
1815 IkReal x133=IKcos(j5);
1816 IkReal x134=IKsin(j5);
1817 IkReal x135=((1.0)*gconst18);
1818 if((((1.0)+(((-1.0)*gconst18*x135)))) < -0.00001)
1819 continue;
1820 IkReal x136=IKsqrt(((1.0)+(((-1.0)*gconst18*x135))));
1821 IkReal x137=((1.0)*x136);
1822 evalcond[0]=x133;
1823 evalcond[1]=((-1.0)*x134);
1824 evalcond[2]=((((-1.0)*x133*x135))+new_r11);
1825 evalcond[3]=((((-1.0)*x134*x135))+new_r10);
1826 evalcond[4]=((((-1.0)*x133*x137))+new_r01);
1827 evalcond[5]=((((-1.0)*x134*x137))+new_r00);
1828 evalcond[6]=((((-1.0)*new_r10*x135))+(((-1.0)*new_r00*x137))+x134);
1829 evalcond[7]=((((-1.0)*new_r11*x135))+(((-1.0)*new_r01*x137))+x133);
1830 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 )
1831 {
1832 continue;
1833 }
1834 }
1835 
1836 {
1837 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1838 vinfos[0].jointtype = 1;
1839 vinfos[0].foffset = j0;
1840 vinfos[0].indices[0] = _ij0[0];
1841 vinfos[0].indices[1] = _ij0[1];
1842 vinfos[0].maxsolutions = _nj0;
1843 vinfos[1].jointtype = 1;
1844 vinfos[1].foffset = j1;
1845 vinfos[1].indices[0] = _ij1[0];
1846 vinfos[1].indices[1] = _ij1[1];
1847 vinfos[1].maxsolutions = _nj1;
1848 vinfos[2].jointtype = 1;
1849 vinfos[2].foffset = j2;
1850 vinfos[2].indices[0] = _ij2[0];
1851 vinfos[2].indices[1] = _ij2[1];
1852 vinfos[2].maxsolutions = _nj2;
1853 vinfos[3].jointtype = 1;
1854 vinfos[3].foffset = j3;
1855 vinfos[3].indices[0] = _ij3[0];
1856 vinfos[3].indices[1] = _ij3[1];
1857 vinfos[3].maxsolutions = _nj3;
1858 vinfos[4].jointtype = 1;
1859 vinfos[4].foffset = j4;
1860 vinfos[4].indices[0] = _ij4[0];
1861 vinfos[4].indices[1] = _ij4[1];
1862 vinfos[4].maxsolutions = _nj4;
1863 vinfos[5].jointtype = 1;
1864 vinfos[5].foffset = j5;
1865 vinfos[5].indices[0] = _ij5[0];
1866 vinfos[5].indices[1] = _ij5[1];
1867 vinfos[5].maxsolutions = _nj5;
1868 std::vector<int> vfree(0);
1869 solutions.AddSolution(vinfos,vfree);
1870 }
1871 }
1872 }
1873 
1874 }
1875 
1876 }
1877 
1878 }
1879 } while(0);
1880 if( bgotonextstatement )
1881 {
1882 bool bgotonextstatement = true;
1883 do
1884 {
1885 CheckValue<IkReal> x138=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1886 if(!x138.valid){
1887 continue;
1888 }
1889 if((x138.value) < -0.00001)
1890 continue;
1891 IkReal gconst19=IKsqrt(x138.value);
1892 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
1893 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1894 {
1895 bgotonextstatement=false;
1896 {
1897 IkReal j5eval[1];
1898 new_r21=0;
1899 new_r20=0;
1900 new_r02=0;
1901 new_r12=0;
1902 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1903 continue;
1904 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))));
1905 cj3=gconst19;
1906 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
1907  continue;
1908 j3=IKacos(gconst19);
1909 CheckValue<IkReal> x139=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1910 if(!x139.valid){
1911 continue;
1912 }
1913 if((x139.value) < -0.00001)
1914 continue;
1915 IkReal gconst19=IKsqrt(x139.value);
1916 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1917 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1918 {
1919 {
1920 IkReal j5array[1], cj5array[1], sj5array[1];
1921 bool j5valid[1]={false};
1922 _nj5 = 1;
1923 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1924 continue;
1925 CheckValue<IkReal> x140=IKPowWithIntegerCheck(gconst19,-1);
1926 if(!x140.valid){
1927 continue;
1928 }
1929 if( IKabs((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x140.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))))+IKsqr((new_r11*(x140.value)))-1) <= IKFAST_SINCOS_THRESH )
1930  continue;
1931 j5array[0]=IKatan2((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))), (new_r11*(x140.value)));
1932 sj5array[0]=IKsin(j5array[0]);
1933 cj5array[0]=IKcos(j5array[0]);
1934 if( j5array[0] > IKPI )
1935 {
1936  j5array[0]-=IK2PI;
1937 }
1938 else if( j5array[0] < -IKPI )
1939 { j5array[0]+=IK2PI;
1940 }
1941 j5valid[0] = true;
1942 for(int ij5 = 0; ij5 < 1; ++ij5)
1943 {
1944 if( !j5valid[ij5] )
1945 {
1946  continue;
1947 }
1948 _ij5[0] = ij5; _ij5[1] = -1;
1949 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1950 {
1951 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1952 {
1953  j5valid[iij5]=false; _ij5[1] = iij5; break;
1954 }
1955 }
1956 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1957 {
1958 IkReal evalcond[8];
1959 IkReal x141=IKcos(j5);
1960 IkReal x142=IKsin(j5);
1961 IkReal x143=((1.0)*gconst19);
1962 if((((1.0)+(((-1.0)*gconst19*x143)))) < -0.00001)
1963 continue;
1964 IkReal x144=IKsqrt(((1.0)+(((-1.0)*gconst19*x143))));
1965 evalcond[0]=x141;
1966 evalcond[1]=((-1.0)*x142);
1967 evalcond[2]=((((-1.0)*x141*x143))+new_r11);
1968 evalcond[3]=((((-1.0)*x142*x143))+new_r10);
1969 evalcond[4]=(((x141*x144))+new_r01);
1970 evalcond[5]=(((x142*x144))+new_r00);
1971 evalcond[6]=(((new_r00*x144))+x142+(((-1.0)*new_r10*x143)));
1972 evalcond[7]=(((new_r01*x144))+(((-1.0)*new_r11*x143))+x141);
1973 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 )
1974 {
1975 continue;
1976 }
1977 }
1978 
1979 {
1980 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1981 vinfos[0].jointtype = 1;
1982 vinfos[0].foffset = j0;
1983 vinfos[0].indices[0] = _ij0[0];
1984 vinfos[0].indices[1] = _ij0[1];
1985 vinfos[0].maxsolutions = _nj0;
1986 vinfos[1].jointtype = 1;
1987 vinfos[1].foffset = j1;
1988 vinfos[1].indices[0] = _ij1[0];
1989 vinfos[1].indices[1] = _ij1[1];
1990 vinfos[1].maxsolutions = _nj1;
1991 vinfos[2].jointtype = 1;
1992 vinfos[2].foffset = j2;
1993 vinfos[2].indices[0] = _ij2[0];
1994 vinfos[2].indices[1] = _ij2[1];
1995 vinfos[2].maxsolutions = _nj2;
1996 vinfos[3].jointtype = 1;
1997 vinfos[3].foffset = j3;
1998 vinfos[3].indices[0] = _ij3[0];
1999 vinfos[3].indices[1] = _ij3[1];
2000 vinfos[3].maxsolutions = _nj3;
2001 vinfos[4].jointtype = 1;
2002 vinfos[4].foffset = j4;
2003 vinfos[4].indices[0] = _ij4[0];
2004 vinfos[4].indices[1] = _ij4[1];
2005 vinfos[4].maxsolutions = _nj4;
2006 vinfos[5].jointtype = 1;
2007 vinfos[5].foffset = j5;
2008 vinfos[5].indices[0] = _ij5[0];
2009 vinfos[5].indices[1] = _ij5[1];
2010 vinfos[5].maxsolutions = _nj5;
2011 std::vector<int> vfree(0);
2012 solutions.AddSolution(vinfos,vfree);
2013 }
2014 }
2015 }
2016 
2017 } else
2018 {
2019 {
2020 IkReal j5array[1], cj5array[1], sj5array[1];
2021 bool j5valid[1]={false};
2022 _nj5 = 1;
2023 CheckValue<IkReal> x145=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2024 if(!x145.valid){
2025 continue;
2026 }
2027 CheckValue<IkReal> x146 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2028 if(!x146.valid){
2029 continue;
2030 }
2031 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x145.value)))+(x146.value));
2032 sj5array[0]=IKsin(j5array[0]);
2033 cj5array[0]=IKcos(j5array[0]);
2034 if( j5array[0] > IKPI )
2035 {
2036  j5array[0]-=IK2PI;
2037 }
2038 else if( j5array[0] < -IKPI )
2039 { j5array[0]+=IK2PI;
2040 }
2041 j5valid[0] = true;
2042 for(int ij5 = 0; ij5 < 1; ++ij5)
2043 {
2044 if( !j5valid[ij5] )
2045 {
2046  continue;
2047 }
2048 _ij5[0] = ij5; _ij5[1] = -1;
2049 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2050 {
2051 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2052 {
2053  j5valid[iij5]=false; _ij5[1] = iij5; break;
2054 }
2055 }
2056 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2057 {
2058 IkReal evalcond[8];
2059 IkReal x147=IKcos(j5);
2060 IkReal x148=IKsin(j5);
2061 IkReal x149=((1.0)*gconst19);
2062 if((((1.0)+(((-1.0)*gconst19*x149)))) < -0.00001)
2063 continue;
2064 IkReal x150=IKsqrt(((1.0)+(((-1.0)*gconst19*x149))));
2065 evalcond[0]=x147;
2066 evalcond[1]=((-1.0)*x148);
2067 evalcond[2]=((((-1.0)*x147*x149))+new_r11);
2068 evalcond[3]=((((-1.0)*x148*x149))+new_r10);
2069 evalcond[4]=(((x147*x150))+new_r01);
2070 evalcond[5]=(((x148*x150))+new_r00);
2071 evalcond[6]=(((new_r00*x150))+x148+(((-1.0)*new_r10*x149)));
2072 evalcond[7]=(((new_r01*x150))+(((-1.0)*new_r11*x149))+x147);
2073 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 )
2074 {
2075 continue;
2076 }
2077 }
2078 
2079 {
2080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2081 vinfos[0].jointtype = 1;
2082 vinfos[0].foffset = j0;
2083 vinfos[0].indices[0] = _ij0[0];
2084 vinfos[0].indices[1] = _ij0[1];
2085 vinfos[0].maxsolutions = _nj0;
2086 vinfos[1].jointtype = 1;
2087 vinfos[1].foffset = j1;
2088 vinfos[1].indices[0] = _ij1[0];
2089 vinfos[1].indices[1] = _ij1[1];
2090 vinfos[1].maxsolutions = _nj1;
2091 vinfos[2].jointtype = 1;
2092 vinfos[2].foffset = j2;
2093 vinfos[2].indices[0] = _ij2[0];
2094 vinfos[2].indices[1] = _ij2[1];
2095 vinfos[2].maxsolutions = _nj2;
2096 vinfos[3].jointtype = 1;
2097 vinfos[3].foffset = j3;
2098 vinfos[3].indices[0] = _ij3[0];
2099 vinfos[3].indices[1] = _ij3[1];
2100 vinfos[3].maxsolutions = _nj3;
2101 vinfos[4].jointtype = 1;
2102 vinfos[4].foffset = j4;
2103 vinfos[4].indices[0] = _ij4[0];
2104 vinfos[4].indices[1] = _ij4[1];
2105 vinfos[4].maxsolutions = _nj4;
2106 vinfos[5].jointtype = 1;
2107 vinfos[5].foffset = j5;
2108 vinfos[5].indices[0] = _ij5[0];
2109 vinfos[5].indices[1] = _ij5[1];
2110 vinfos[5].maxsolutions = _nj5;
2111 std::vector<int> vfree(0);
2112 solutions.AddSolution(vinfos,vfree);
2113 }
2114 }
2115 }
2116 
2117 }
2118 
2119 }
2120 
2121 }
2122 } while(0);
2123 if( bgotonextstatement )
2124 {
2125 bool bgotonextstatement = true;
2126 do
2127 {
2128 CheckValue<IkReal> x151=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2129 if(!x151.valid){
2130 continue;
2131 }
2132 if((x151.value) < -0.00001)
2133 continue;
2134 IkReal gconst19=IKsqrt(x151.value);
2135 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
2136 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2137 {
2138 bgotonextstatement=false;
2139 {
2140 IkReal j5eval[1];
2141 new_r21=0;
2142 new_r20=0;
2143 new_r02=0;
2144 new_r12=0;
2145 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2146 continue;
2147 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))));
2148 cj3=gconst19;
2149 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
2150  continue;
2151 j3=((-1.0)*(IKacos(gconst19)));
2152 CheckValue<IkReal> x152=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2153 if(!x152.valid){
2154 continue;
2155 }
2156 if((x152.value) < -0.00001)
2157 continue;
2158 IkReal gconst19=IKsqrt(x152.value);
2159 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2160 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2161 {
2162 {
2163 IkReal j5array[1], cj5array[1], sj5array[1];
2164 bool j5valid[1]={false};
2165 _nj5 = 1;
2166 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2167 continue;
2168 CheckValue<IkReal> x153=IKPowWithIntegerCheck(gconst19,-1);
2169 if(!x153.valid){
2170 continue;
2171 }
2172 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x153.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))))+IKsqr((new_r11*(x153.value)))-1) <= IKFAST_SINCOS_THRESH )
2173  continue;
2174 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))), (new_r11*(x153.value)));
2175 sj5array[0]=IKsin(j5array[0]);
2176 cj5array[0]=IKcos(j5array[0]);
2177 if( j5array[0] > IKPI )
2178 {
2179  j5array[0]-=IK2PI;
2180 }
2181 else if( j5array[0] < -IKPI )
2182 { j5array[0]+=IK2PI;
2183 }
2184 j5valid[0] = true;
2185 for(int ij5 = 0; ij5 < 1; ++ij5)
2186 {
2187 if( !j5valid[ij5] )
2188 {
2189  continue;
2190 }
2191 _ij5[0] = ij5; _ij5[1] = -1;
2192 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2193 {
2194 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2195 {
2196  j5valid[iij5]=false; _ij5[1] = iij5; break;
2197 }
2198 }
2199 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2200 {
2201 IkReal evalcond[8];
2202 IkReal x154=IKcos(j5);
2203 IkReal x155=IKsin(j5);
2204 IkReal x156=((1.0)*gconst19);
2205 if((((1.0)+(((-1.0)*gconst19*x156)))) < -0.00001)
2206 continue;
2207 IkReal x157=IKsqrt(((1.0)+(((-1.0)*gconst19*x156))));
2208 IkReal x158=((1.0)*x157);
2209 evalcond[0]=x154;
2210 evalcond[1]=((-1.0)*x155);
2211 evalcond[2]=((((-1.0)*x154*x156))+new_r11);
2212 evalcond[3]=((((-1.0)*x155*x156))+new_r10);
2213 evalcond[4]=((((-1.0)*x154*x158))+new_r01);
2214 evalcond[5]=((((-1.0)*x155*x158))+new_r00);
2215 evalcond[6]=(x155+(((-1.0)*new_r10*x156))+(((-1.0)*new_r00*x158)));
2216 evalcond[7]=(x154+(((-1.0)*new_r11*x156))+(((-1.0)*new_r01*x158)));
2217 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 )
2218 {
2219 continue;
2220 }
2221 }
2222 
2223 {
2224 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2225 vinfos[0].jointtype = 1;
2226 vinfos[0].foffset = j0;
2227 vinfos[0].indices[0] = _ij0[0];
2228 vinfos[0].indices[1] = _ij0[1];
2229 vinfos[0].maxsolutions = _nj0;
2230 vinfos[1].jointtype = 1;
2231 vinfos[1].foffset = j1;
2232 vinfos[1].indices[0] = _ij1[0];
2233 vinfos[1].indices[1] = _ij1[1];
2234 vinfos[1].maxsolutions = _nj1;
2235 vinfos[2].jointtype = 1;
2236 vinfos[2].foffset = j2;
2237 vinfos[2].indices[0] = _ij2[0];
2238 vinfos[2].indices[1] = _ij2[1];
2239 vinfos[2].maxsolutions = _nj2;
2240 vinfos[3].jointtype = 1;
2241 vinfos[3].foffset = j3;
2242 vinfos[3].indices[0] = _ij3[0];
2243 vinfos[3].indices[1] = _ij3[1];
2244 vinfos[3].maxsolutions = _nj3;
2245 vinfos[4].jointtype = 1;
2246 vinfos[4].foffset = j4;
2247 vinfos[4].indices[0] = _ij4[0];
2248 vinfos[4].indices[1] = _ij4[1];
2249 vinfos[4].maxsolutions = _nj4;
2250 vinfos[5].jointtype = 1;
2251 vinfos[5].foffset = j5;
2252 vinfos[5].indices[0] = _ij5[0];
2253 vinfos[5].indices[1] = _ij5[1];
2254 vinfos[5].maxsolutions = _nj5;
2255 std::vector<int> vfree(0);
2256 solutions.AddSolution(vinfos,vfree);
2257 }
2258 }
2259 }
2260 
2261 } else
2262 {
2263 {
2264 IkReal j5array[1], cj5array[1], sj5array[1];
2265 bool j5valid[1]={false};
2266 _nj5 = 1;
2267 CheckValue<IkReal> x159=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2268 if(!x159.valid){
2269 continue;
2270 }
2271 CheckValue<IkReal> x160 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2272 if(!x160.valid){
2273 continue;
2274 }
2275 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x159.value)))+(x160.value));
2276 sj5array[0]=IKsin(j5array[0]);
2277 cj5array[0]=IKcos(j5array[0]);
2278 if( j5array[0] > IKPI )
2279 {
2280  j5array[0]-=IK2PI;
2281 }
2282 else if( j5array[0] < -IKPI )
2283 { j5array[0]+=IK2PI;
2284 }
2285 j5valid[0] = true;
2286 for(int ij5 = 0; ij5 < 1; ++ij5)
2287 {
2288 if( !j5valid[ij5] )
2289 {
2290  continue;
2291 }
2292 _ij5[0] = ij5; _ij5[1] = -1;
2293 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2294 {
2295 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2296 {
2297  j5valid[iij5]=false; _ij5[1] = iij5; break;
2298 }
2299 }
2300 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2301 {
2302 IkReal evalcond[8];
2303 IkReal x161=IKcos(j5);
2304 IkReal x162=IKsin(j5);
2305 IkReal x163=((1.0)*gconst19);
2306 if((((1.0)+(((-1.0)*gconst19*x163)))) < -0.00001)
2307 continue;
2308 IkReal x164=IKsqrt(((1.0)+(((-1.0)*gconst19*x163))));
2309 IkReal x165=((1.0)*x164);
2310 evalcond[0]=x161;
2311 evalcond[1]=((-1.0)*x162);
2312 evalcond[2]=((((-1.0)*x161*x163))+new_r11);
2313 evalcond[3]=((((-1.0)*x162*x163))+new_r10);
2314 evalcond[4]=((((-1.0)*x161*x165))+new_r01);
2315 evalcond[5]=((((-1.0)*x162*x165))+new_r00);
2316 evalcond[6]=((((-1.0)*new_r00*x165))+(((-1.0)*new_r10*x163))+x162);
2317 evalcond[7]=((((-1.0)*new_r11*x163))+x161+(((-1.0)*new_r01*x165)));
2318 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 )
2319 {
2320 continue;
2321 }
2322 }
2323 
2324 {
2325 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2326 vinfos[0].jointtype = 1;
2327 vinfos[0].foffset = j0;
2328 vinfos[0].indices[0] = _ij0[0];
2329 vinfos[0].indices[1] = _ij0[1];
2330 vinfos[0].maxsolutions = _nj0;
2331 vinfos[1].jointtype = 1;
2332 vinfos[1].foffset = j1;
2333 vinfos[1].indices[0] = _ij1[0];
2334 vinfos[1].indices[1] = _ij1[1];
2335 vinfos[1].maxsolutions = _nj1;
2336 vinfos[2].jointtype = 1;
2337 vinfos[2].foffset = j2;
2338 vinfos[2].indices[0] = _ij2[0];
2339 vinfos[2].indices[1] = _ij2[1];
2340 vinfos[2].maxsolutions = _nj2;
2341 vinfos[3].jointtype = 1;
2342 vinfos[3].foffset = j3;
2343 vinfos[3].indices[0] = _ij3[0];
2344 vinfos[3].indices[1] = _ij3[1];
2345 vinfos[3].maxsolutions = _nj3;
2346 vinfos[4].jointtype = 1;
2347 vinfos[4].foffset = j4;
2348 vinfos[4].indices[0] = _ij4[0];
2349 vinfos[4].indices[1] = _ij4[1];
2350 vinfos[4].maxsolutions = _nj4;
2351 vinfos[5].jointtype = 1;
2352 vinfos[5].foffset = j5;
2353 vinfos[5].indices[0] = _ij5[0];
2354 vinfos[5].indices[1] = _ij5[1];
2355 vinfos[5].maxsolutions = _nj5;
2356 std::vector<int> vfree(0);
2357 solutions.AddSolution(vinfos,vfree);
2358 }
2359 }
2360 }
2361 
2362 }
2363 
2364 }
2365 
2366 }
2367 } while(0);
2368 if( bgotonextstatement )
2369 {
2370 bool bgotonextstatement = true;
2371 do
2372 {
2373 if( 1 )
2374 {
2375 bgotonextstatement=false;
2376 continue; // branch miss [j5]
2377 
2378 }
2379 } while(0);
2380 if( bgotonextstatement )
2381 {
2382 }
2383 }
2384 }
2385 }
2386 }
2387 }
2388 }
2389 }
2390 
2391 } else
2392 {
2393 {
2394 IkReal j5array[1], cj5array[1], sj5array[1];
2395 bool j5valid[1]={false};
2396 _nj5 = 1;
2397 IkReal x166=(new_r00*sj3);
2398 CheckValue<IkReal> x167=IKPowWithIntegerCheck(sj3,-1);
2399 if(!x167.valid){
2400 continue;
2401 }
2402 if( IKabs(((((-1.0)*x166))+((cj3*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x166))+((cj3*new_r10))))+IKsqr(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2403  continue;
2404 j5array[0]=IKatan2(((((-1.0)*x166))+((cj3*new_r10))), ((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2405 sj5array[0]=IKsin(j5array[0]);
2406 cj5array[0]=IKcos(j5array[0]);
2407 if( j5array[0] > IKPI )
2408 {
2409  j5array[0]-=IK2PI;
2410 }
2411 else if( j5array[0] < -IKPI )
2412 { j5array[0]+=IK2PI;
2413 }
2414 j5valid[0] = true;
2415 for(int ij5 = 0; ij5 < 1; ++ij5)
2416 {
2417 if( !j5valid[ij5] )
2418 {
2419  continue;
2420 }
2421 _ij5[0] = ij5; _ij5[1] = -1;
2422 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2423 {
2424 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2425 {
2426  j5valid[iij5]=false; _ij5[1] = iij5; break;
2427 }
2428 }
2429 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2430 {
2431 IkReal evalcond[10];
2432 IkReal x168=IKsin(j5);
2433 IkReal x169=IKcos(j5);
2434 IkReal x170=((1.0)*new_r11);
2435 IkReal x171=(new_r22*sj3);
2436 IkReal x172=((1.0)*new_r10);
2437 IkReal x173=((1.0)*cj3*new_r22);
2438 IkReal x174=(sj3*x168);
2439 IkReal x175=(new_r22*x168);
2440 IkReal x176=((1.0)*x169);
2441 IkReal x177=((1.0)*x168);
2442 evalcond[0]=(((new_r00*sj3))+x168+(((-1.0)*cj3*x172)));
2443 evalcond[1]=(((new_r01*sj3))+x169+(((-1.0)*cj3*x170)));
2444 evalcond[2]=(((new_r11*sj3))+x175+((cj3*new_r01)));
2445 evalcond[3]=(((sj3*x169))+((cj3*x175))+new_r01);
2446 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x176))+((cj3*new_r00)));
2447 evalcond[5]=(x174+new_r00+(((-1.0)*x169*x173)));
2448 evalcond[6]=(((x168*x171))+(((-1.0)*cj3*x176))+new_r11);
2449 evalcond[7]=(x169+(((-1.0)*x171*x172))+(((-1.0)*new_r00*x173)));
2450 evalcond[8]=((((-1.0)*cj3*x177))+(((-1.0)*x171*x176))+new_r10);
2451 evalcond[9]=((((-1.0)*x177))+(((-1.0)*x170*x171))+(((-1.0)*new_r01*x173)));
2452 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 )
2453 {
2454 continue;
2455 }
2456 }
2457 
2458 {
2459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2460 vinfos[0].jointtype = 1;
2461 vinfos[0].foffset = j0;
2462 vinfos[0].indices[0] = _ij0[0];
2463 vinfos[0].indices[1] = _ij0[1];
2464 vinfos[0].maxsolutions = _nj0;
2465 vinfos[1].jointtype = 1;
2466 vinfos[1].foffset = j1;
2467 vinfos[1].indices[0] = _ij1[0];
2468 vinfos[1].indices[1] = _ij1[1];
2469 vinfos[1].maxsolutions = _nj1;
2470 vinfos[2].jointtype = 1;
2471 vinfos[2].foffset = j2;
2472 vinfos[2].indices[0] = _ij2[0];
2473 vinfos[2].indices[1] = _ij2[1];
2474 vinfos[2].maxsolutions = _nj2;
2475 vinfos[3].jointtype = 1;
2476 vinfos[3].foffset = j3;
2477 vinfos[3].indices[0] = _ij3[0];
2478 vinfos[3].indices[1] = _ij3[1];
2479 vinfos[3].maxsolutions = _nj3;
2480 vinfos[4].jointtype = 1;
2481 vinfos[4].foffset = j4;
2482 vinfos[4].indices[0] = _ij4[0];
2483 vinfos[4].indices[1] = _ij4[1];
2484 vinfos[4].maxsolutions = _nj4;
2485 vinfos[5].jointtype = 1;
2486 vinfos[5].foffset = j5;
2487 vinfos[5].indices[0] = _ij5[0];
2488 vinfos[5].indices[1] = _ij5[1];
2489 vinfos[5].maxsolutions = _nj5;
2490 std::vector<int> vfree(0);
2491 solutions.AddSolution(vinfos,vfree);
2492 }
2493 }
2494 }
2495 
2496 }
2497 
2498 }
2499 
2500 } else
2501 {
2502 {
2503 IkReal j5array[1], cj5array[1], sj5array[1];
2504 bool j5valid[1]={false};
2505 _nj5 = 1;
2506 IkReal x178=((1.0)*new_r01);
2507 CheckValue<IkReal> x179=IKPowWithIntegerCheck(new_r22,-1);
2508 if(!x179.valid){
2509 continue;
2510 }
2511 if( IKabs(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r11))+(((-1.0)*sj3*x178)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))))+IKsqr((((cj3*new_r11))+(((-1.0)*sj3*x178))))-1) <= IKFAST_SINCOS_THRESH )
2512  continue;
2513 j5array[0]=IKatan2(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))), (((cj3*new_r11))+(((-1.0)*sj3*x178))));
2514 sj5array[0]=IKsin(j5array[0]);
2515 cj5array[0]=IKcos(j5array[0]);
2516 if( j5array[0] > IKPI )
2517 {
2518  j5array[0]-=IK2PI;
2519 }
2520 else if( j5array[0] < -IKPI )
2521 { j5array[0]+=IK2PI;
2522 }
2523 j5valid[0] = true;
2524 for(int ij5 = 0; ij5 < 1; ++ij5)
2525 {
2526 if( !j5valid[ij5] )
2527 {
2528  continue;
2529 }
2530 _ij5[0] = ij5; _ij5[1] = -1;
2531 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2532 {
2533 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2534 {
2535  j5valid[iij5]=false; _ij5[1] = iij5; break;
2536 }
2537 }
2538 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2539 {
2540 IkReal evalcond[10];
2541 IkReal x180=IKsin(j5);
2542 IkReal x181=IKcos(j5);
2543 IkReal x182=((1.0)*new_r11);
2544 IkReal x183=(new_r22*sj3);
2545 IkReal x184=((1.0)*new_r10);
2546 IkReal x185=((1.0)*cj3*new_r22);
2547 IkReal x186=(sj3*x180);
2548 IkReal x187=(new_r22*x180);
2549 IkReal x188=((1.0)*x181);
2550 IkReal x189=((1.0)*x180);
2551 evalcond[0]=(((new_r00*sj3))+x180+(((-1.0)*cj3*x184)));
2552 evalcond[1]=(((new_r01*sj3))+x181+(((-1.0)*cj3*x182)));
2553 evalcond[2]=(((new_r11*sj3))+x187+((cj3*new_r01)));
2554 evalcond[3]=(((sj3*x181))+((cj3*x187))+new_r01);
2555 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x188))+((cj3*new_r00)));
2556 evalcond[5]=(x186+new_r00+(((-1.0)*x181*x185)));
2557 evalcond[6]=(((x180*x183))+new_r11+(((-1.0)*cj3*x188)));
2558 evalcond[7]=(x181+(((-1.0)*new_r00*x185))+(((-1.0)*x183*x184)));
2559 evalcond[8]=(new_r10+(((-1.0)*x183*x188))+(((-1.0)*cj3*x189)));
2560 evalcond[9]=((((-1.0)*x189))+(((-1.0)*new_r01*x185))+(((-1.0)*x182*x183)));
2561 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 )
2562 {
2563 continue;
2564 }
2565 }
2566 
2567 {
2568 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2569 vinfos[0].jointtype = 1;
2570 vinfos[0].foffset = j0;
2571 vinfos[0].indices[0] = _ij0[0];
2572 vinfos[0].indices[1] = _ij0[1];
2573 vinfos[0].maxsolutions = _nj0;
2574 vinfos[1].jointtype = 1;
2575 vinfos[1].foffset = j1;
2576 vinfos[1].indices[0] = _ij1[0];
2577 vinfos[1].indices[1] = _ij1[1];
2578 vinfos[1].maxsolutions = _nj1;
2579 vinfos[2].jointtype = 1;
2580 vinfos[2].foffset = j2;
2581 vinfos[2].indices[0] = _ij2[0];
2582 vinfos[2].indices[1] = _ij2[1];
2583 vinfos[2].maxsolutions = _nj2;
2584 vinfos[3].jointtype = 1;
2585 vinfos[3].foffset = j3;
2586 vinfos[3].indices[0] = _ij3[0];
2587 vinfos[3].indices[1] = _ij3[1];
2588 vinfos[3].maxsolutions = _nj3;
2589 vinfos[4].jointtype = 1;
2590 vinfos[4].foffset = j4;
2591 vinfos[4].indices[0] = _ij4[0];
2592 vinfos[4].indices[1] = _ij4[1];
2593 vinfos[4].maxsolutions = _nj4;
2594 vinfos[5].jointtype = 1;
2595 vinfos[5].foffset = j5;
2596 vinfos[5].indices[0] = _ij5[0];
2597 vinfos[5].indices[1] = _ij5[1];
2598 vinfos[5].maxsolutions = _nj5;
2599 std::vector<int> vfree(0);
2600 solutions.AddSolution(vinfos,vfree);
2601 }
2602 }
2603 }
2604 
2605 }
2606 
2607 }
2608 
2609 } else
2610 {
2611 {
2612 IkReal j5array[1], cj5array[1], sj5array[1];
2613 bool j5valid[1]={false};
2614 _nj5 = 1;
2615 IkReal x190=cj3*cj3;
2616 IkReal x191=(cj3*new_r22);
2617 CheckValue<IkReal> x192=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x190*(new_r22*new_r22)))+x190)),-1);
2618 if(!x192.valid){
2619 continue;
2620 }
2621 CheckValue<IkReal> x193 = IKatan2WithCheck(IkReal((((new_r01*x191))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x191)))),IKFAST_ATAN2_MAGTHRESH);
2622 if(!x193.valid){
2623 continue;
2624 }
2625 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.value)))+(x193.value));
2626 sj5array[0]=IKsin(j5array[0]);
2627 cj5array[0]=IKcos(j5array[0]);
2628 if( j5array[0] > IKPI )
2629 {
2630  j5array[0]-=IK2PI;
2631 }
2632 else if( j5array[0] < -IKPI )
2633 { j5array[0]+=IK2PI;
2634 }
2635 j5valid[0] = true;
2636 for(int ij5 = 0; ij5 < 1; ++ij5)
2637 {
2638 if( !j5valid[ij5] )
2639 {
2640  continue;
2641 }
2642 _ij5[0] = ij5; _ij5[1] = -1;
2643 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2644 {
2645 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2646 {
2647  j5valid[iij5]=false; _ij5[1] = iij5; break;
2648 }
2649 }
2650 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2651 {
2652 IkReal evalcond[10];
2653 IkReal x194=IKsin(j5);
2654 IkReal x195=IKcos(j5);
2655 IkReal x196=((1.0)*new_r11);
2656 IkReal x197=(new_r22*sj3);
2657 IkReal x198=((1.0)*new_r10);
2658 IkReal x199=((1.0)*cj3*new_r22);
2659 IkReal x200=(sj3*x194);
2660 IkReal x201=(new_r22*x194);
2661 IkReal x202=((1.0)*x195);
2662 IkReal x203=((1.0)*x194);
2663 evalcond[0]=(((new_r00*sj3))+x194+(((-1.0)*cj3*x198)));
2664 evalcond[1]=(((new_r01*sj3))+x195+(((-1.0)*cj3*x196)));
2665 evalcond[2]=(((new_r11*sj3))+x201+((cj3*new_r01)));
2666 evalcond[3]=(((sj3*x195))+((cj3*x201))+new_r01);
2667 evalcond[4]=((((-1.0)*new_r22*x202))+((new_r10*sj3))+((cj3*new_r00)));
2668 evalcond[5]=((((-1.0)*x195*x199))+x200+new_r00);
2669 evalcond[6]=(((x194*x197))+new_r11+(((-1.0)*cj3*x202)));
2670 evalcond[7]=((((-1.0)*x197*x198))+x195+(((-1.0)*new_r00*x199)));
2671 evalcond[8]=((((-1.0)*x197*x202))+new_r10+(((-1.0)*cj3*x203)));
2672 evalcond[9]=((((-1.0)*x196*x197))+(((-1.0)*x203))+(((-1.0)*new_r01*x199)));
2673 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 )
2674 {
2675 continue;
2676 }
2677 }
2678 
2679 {
2680 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2681 vinfos[0].jointtype = 1;
2682 vinfos[0].foffset = j0;
2683 vinfos[0].indices[0] = _ij0[0];
2684 vinfos[0].indices[1] = _ij0[1];
2685 vinfos[0].maxsolutions = _nj0;
2686 vinfos[1].jointtype = 1;
2687 vinfos[1].foffset = j1;
2688 vinfos[1].indices[0] = _ij1[0];
2689 vinfos[1].indices[1] = _ij1[1];
2690 vinfos[1].maxsolutions = _nj1;
2691 vinfos[2].jointtype = 1;
2692 vinfos[2].foffset = j2;
2693 vinfos[2].indices[0] = _ij2[0];
2694 vinfos[2].indices[1] = _ij2[1];
2695 vinfos[2].maxsolutions = _nj2;
2696 vinfos[3].jointtype = 1;
2697 vinfos[3].foffset = j3;
2698 vinfos[3].indices[0] = _ij3[0];
2699 vinfos[3].indices[1] = _ij3[1];
2700 vinfos[3].maxsolutions = _nj3;
2701 vinfos[4].jointtype = 1;
2702 vinfos[4].foffset = j4;
2703 vinfos[4].indices[0] = _ij4[0];
2704 vinfos[4].indices[1] = _ij4[1];
2705 vinfos[4].maxsolutions = _nj4;
2706 vinfos[5].jointtype = 1;
2707 vinfos[5].foffset = j5;
2708 vinfos[5].indices[0] = _ij5[0];
2709 vinfos[5].indices[1] = _ij5[1];
2710 vinfos[5].maxsolutions = _nj5;
2711 std::vector<int> vfree(0);
2712 solutions.AddSolution(vinfos,vfree);
2713 }
2714 }
2715 }
2716 
2717 }
2718 
2719 }
2720  }
2721 
2722 }
2723 
2724 }
2725 
2726 }
2727 } while(0);
2728 if( bgotonextstatement )
2729 {
2730 bool bgotonextstatement = true;
2731 do
2732 {
2733 if( 1 )
2734 {
2735 bgotonextstatement=false;
2736 continue; // branch miss [j3, j5]
2737 
2738 }
2739 } while(0);
2740 if( bgotonextstatement )
2741 {
2742 }
2743 }
2744 }
2745 }
2746 }
2747 
2748 } else
2749 {
2750 {
2751 IkReal j3array[1], cj3array[1], sj3array[1];
2752 bool j3valid[1]={false};
2753 _nj3 = 1;
2754 CheckValue<IkReal> x205=IKPowWithIntegerCheck(sj4,-1);
2755 if(!x205.valid){
2756 continue;
2757 }
2758 IkReal x204=x205.value;
2759 CheckValue<IkReal> x206=IKPowWithIntegerCheck(new_r12,-1);
2760 if(!x206.valid){
2761 continue;
2762 }
2763 if( IKabs((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x204)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x204))-1) <= IKFAST_SINCOS_THRESH )
2764  continue;
2765 j3array[0]=IKatan2((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x204));
2766 sj3array[0]=IKsin(j3array[0]);
2767 cj3array[0]=IKcos(j3array[0]);
2768 if( j3array[0] > IKPI )
2769 {
2770  j3array[0]-=IK2PI;
2771 }
2772 else if( j3array[0] < -IKPI )
2773 { j3array[0]+=IK2PI;
2774 }
2775 j3valid[0] = true;
2776 for(int ij3 = 0; ij3 < 1; ++ij3)
2777 {
2778 if( !j3valid[ij3] )
2779 {
2780  continue;
2781 }
2782 _ij3[0] = ij3; _ij3[1] = -1;
2783 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2784 {
2785 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2786 {
2787  j3valid[iij3]=false; _ij3[1] = iij3; break;
2788 }
2789 }
2790 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2791 {
2792 IkReal evalcond[8];
2793 IkReal x207=IKcos(j3);
2794 IkReal x208=IKsin(j3);
2795 IkReal x209=((1.0)*cj4);
2796 IkReal x210=(sj4*x208);
2797 IkReal x211=(new_r12*x208);
2798 IkReal x212=(sj4*x207);
2799 IkReal x213=(new_r02*x207);
2800 evalcond[0]=(x212+new_r02);
2801 evalcond[1]=(x210+new_r12);
2802 evalcond[2]=((((-1.0)*new_r02*x208))+((new_r12*x207)));
2803 evalcond[3]=(sj4+x211+x213);
2804 evalcond[4]=((((-1.0)*new_r20*x209))+((new_r00*x212))+((new_r10*x210)));
2805 evalcond[5]=((((-1.0)*new_r21*x209))+((new_r01*x212))+((new_r11*x210)));
2806 evalcond[6]=((1.0)+(((-1.0)*new_r22*x209))+((new_r02*x212))+((new_r12*x210)));
2807 evalcond[7]=((((-1.0)*new_r22*sj4))+(((-1.0)*x209*x213))+(((-1.0)*x209*x211)));
2808 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 )
2809 {
2810 continue;
2811 }
2812 }
2813 
2814 {
2815 IkReal j5eval[3];
2816 j5eval[0]=sj4;
2817 j5eval[1]=IKsign(sj4);
2818 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2819 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2820 {
2821 {
2822 IkReal j5eval[2];
2823 j5eval[0]=sj4;
2824 j5eval[1]=sj3;
2825 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2826 {
2827 {
2828 IkReal j5eval[2];
2829 j5eval[0]=sj4;
2830 j5eval[1]=cj3;
2831 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2832 {
2833 {
2834 IkReal evalcond[5];
2835 bool bgotonextstatement = true;
2836 do
2837 {
2838 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2839 evalcond[1]=new_r02;
2840 evalcond[2]=new_r12;
2841 evalcond[3]=new_r21;
2842 evalcond[4]=new_r20;
2843 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 )
2844 {
2845 bgotonextstatement=false;
2846 {
2847 IkReal j5array[1], cj5array[1], sj5array[1];
2848 bool j5valid[1]={false};
2849 _nj5 = 1;
2850 IkReal x214=((1.0)*new_r01);
2851 if( IKabs(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x214))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x214))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2852  continue;
2853 j5array[0]=IKatan2(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x214))+((cj3*new_r00))));
2854 sj5array[0]=IKsin(j5array[0]);
2855 cj5array[0]=IKcos(j5array[0]);
2856 if( j5array[0] > IKPI )
2857 {
2858  j5array[0]-=IK2PI;
2859 }
2860 else if( j5array[0] < -IKPI )
2861 { j5array[0]+=IK2PI;
2862 }
2863 j5valid[0] = true;
2864 for(int ij5 = 0; ij5 < 1; ++ij5)
2865 {
2866 if( !j5valid[ij5] )
2867 {
2868  continue;
2869 }
2870 _ij5[0] = ij5; _ij5[1] = -1;
2871 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2872 {
2873 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2874 {
2875  j5valid[iij5]=false; _ij5[1] = iij5; break;
2876 }
2877 }
2878 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2879 {
2880 IkReal evalcond[8];
2881 IkReal x215=IKsin(j5);
2882 IkReal x216=IKcos(j5);
2883 IkReal x217=((1.0)*cj3);
2884 IkReal x218=(sj3*x215);
2885 IkReal x219=((1.0)*x216);
2886 IkReal x220=(x216*x217);
2887 evalcond[0]=(((new_r11*sj3))+x215+((cj3*new_r01)));
2888 evalcond[1]=(((new_r00*sj3))+(((-1.0)*new_r10*x217))+x215);
2889 evalcond[2]=((((-1.0)*new_r11*x217))+((new_r01*sj3))+x216);
2890 evalcond[3]=(((sj3*x216))+((cj3*x215))+new_r01);
2891 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x219)));
2892 evalcond[5]=(x218+new_r00+(((-1.0)*x220)));
2893 evalcond[6]=(x218+new_r11+(((-1.0)*x220)));
2894 evalcond[7]=((((-1.0)*sj3*x219))+new_r10+(((-1.0)*x215*x217)));
2895 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 )
2896 {
2897 continue;
2898 }
2899 }
2900 
2901 {
2902 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2903 vinfos[0].jointtype = 1;
2904 vinfos[0].foffset = j0;
2905 vinfos[0].indices[0] = _ij0[0];
2906 vinfos[0].indices[1] = _ij0[1];
2907 vinfos[0].maxsolutions = _nj0;
2908 vinfos[1].jointtype = 1;
2909 vinfos[1].foffset = j1;
2910 vinfos[1].indices[0] = _ij1[0];
2911 vinfos[1].indices[1] = _ij1[1];
2912 vinfos[1].maxsolutions = _nj1;
2913 vinfos[2].jointtype = 1;
2914 vinfos[2].foffset = j2;
2915 vinfos[2].indices[0] = _ij2[0];
2916 vinfos[2].indices[1] = _ij2[1];
2917 vinfos[2].maxsolutions = _nj2;
2918 vinfos[3].jointtype = 1;
2919 vinfos[3].foffset = j3;
2920 vinfos[3].indices[0] = _ij3[0];
2921 vinfos[3].indices[1] = _ij3[1];
2922 vinfos[3].maxsolutions = _nj3;
2923 vinfos[4].jointtype = 1;
2924 vinfos[4].foffset = j4;
2925 vinfos[4].indices[0] = _ij4[0];
2926 vinfos[4].indices[1] = _ij4[1];
2927 vinfos[4].maxsolutions = _nj4;
2928 vinfos[5].jointtype = 1;
2929 vinfos[5].foffset = j5;
2930 vinfos[5].indices[0] = _ij5[0];
2931 vinfos[5].indices[1] = _ij5[1];
2932 vinfos[5].maxsolutions = _nj5;
2933 std::vector<int> vfree(0);
2934 solutions.AddSolution(vinfos,vfree);
2935 }
2936 }
2937 }
2938 
2939 }
2940 } while(0);
2941 if( bgotonextstatement )
2942 {
2943 bool bgotonextstatement = true;
2944 do
2945 {
2946 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2947 evalcond[1]=new_r02;
2948 evalcond[2]=new_r12;
2949 evalcond[3]=new_r21;
2950 evalcond[4]=new_r20;
2951 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 )
2952 {
2953 bgotonextstatement=false;
2954 {
2955 IkReal j5array[1], cj5array[1], sj5array[1];
2956 bool j5valid[1]={false};
2957 _nj5 = 1;
2958 IkReal x221=((1.0)*sj3);
2959 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x221)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x221))))+IKsqr(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2960  continue;
2961 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x221))), ((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))));
2962 sj5array[0]=IKsin(j5array[0]);
2963 cj5array[0]=IKcos(j5array[0]);
2964 if( j5array[0] > IKPI )
2965 {
2966  j5array[0]-=IK2PI;
2967 }
2968 else if( j5array[0] < -IKPI )
2969 { j5array[0]+=IK2PI;
2970 }
2971 j5valid[0] = true;
2972 for(int ij5 = 0; ij5 < 1; ++ij5)
2973 {
2974 if( !j5valid[ij5] )
2975 {
2976  continue;
2977 }
2978 _ij5[0] = ij5; _ij5[1] = -1;
2979 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2980 {
2981 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2982 {
2983  j5valid[iij5]=false; _ij5[1] = iij5; break;
2984 }
2985 }
2986 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2987 {
2988 IkReal evalcond[8];
2989 IkReal x222=IKcos(j5);
2990 IkReal x223=IKsin(j5);
2991 IkReal x224=((1.0)*cj3);
2992 IkReal x225=(sj3*x222);
2993 IkReal x226=((1.0)*x223);
2994 IkReal x227=(x223*x224);
2995 evalcond[0]=(((new_r10*sj3))+x222+((cj3*new_r00)));
2996 evalcond[1]=((((-1.0)*new_r10*x224))+((new_r00*sj3))+x223);
2997 evalcond[2]=((((-1.0)*new_r11*x224))+((new_r01*sj3))+x222);
2998 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x226)));
2999 evalcond[4]=(((cj3*x222))+((sj3*x223))+new_r00);
3000 evalcond[5]=(x225+new_r01+(((-1.0)*x227)));
3001 evalcond[6]=(x225+new_r10+(((-1.0)*x227)));
3002 evalcond[7]=((((-1.0)*x222*x224))+(((-1.0)*sj3*x226))+new_r11);
3003 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 )
3004 {
3005 continue;
3006 }
3007 }
3008 
3009 {
3010 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3011 vinfos[0].jointtype = 1;
3012 vinfos[0].foffset = j0;
3013 vinfos[0].indices[0] = _ij0[0];
3014 vinfos[0].indices[1] = _ij0[1];
3015 vinfos[0].maxsolutions = _nj0;
3016 vinfos[1].jointtype = 1;
3017 vinfos[1].foffset = j1;
3018 vinfos[1].indices[0] = _ij1[0];
3019 vinfos[1].indices[1] = _ij1[1];
3020 vinfos[1].maxsolutions = _nj1;
3021 vinfos[2].jointtype = 1;
3022 vinfos[2].foffset = j2;
3023 vinfos[2].indices[0] = _ij2[0];
3024 vinfos[2].indices[1] = _ij2[1];
3025 vinfos[2].maxsolutions = _nj2;
3026 vinfos[3].jointtype = 1;
3027 vinfos[3].foffset = j3;
3028 vinfos[3].indices[0] = _ij3[0];
3029 vinfos[3].indices[1] = _ij3[1];
3030 vinfos[3].maxsolutions = _nj3;
3031 vinfos[4].jointtype = 1;
3032 vinfos[4].foffset = j4;
3033 vinfos[4].indices[0] = _ij4[0];
3034 vinfos[4].indices[1] = _ij4[1];
3035 vinfos[4].maxsolutions = _nj4;
3036 vinfos[5].jointtype = 1;
3037 vinfos[5].foffset = j5;
3038 vinfos[5].indices[0] = _ij5[0];
3039 vinfos[5].indices[1] = _ij5[1];
3040 vinfos[5].maxsolutions = _nj5;
3041 std::vector<int> vfree(0);
3042 solutions.AddSolution(vinfos,vfree);
3043 }
3044 }
3045 }
3046 
3047 }
3048 } while(0);
3049 if( bgotonextstatement )
3050 {
3051 bool bgotonextstatement = true;
3052 do
3053 {
3054 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3055 evalcond[1]=new_r02;
3056 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3057 {
3058 bgotonextstatement=false;
3059 {
3060 IkReal j5array[1], cj5array[1], sj5array[1];
3061 bool j5valid[1]={false};
3062 _nj5 = 1;
3063 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
3064  continue;
3065 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3066 sj5array[0]=IKsin(j5array[0]);
3067 cj5array[0]=IKcos(j5array[0]);
3068 if( j5array[0] > IKPI )
3069 {
3070  j5array[0]-=IK2PI;
3071 }
3072 else if( j5array[0] < -IKPI )
3073 { j5array[0]+=IK2PI;
3074 }
3075 j5valid[0] = true;
3076 for(int ij5 = 0; ij5 < 1; ++ij5)
3077 {
3078 if( !j5valid[ij5] )
3079 {
3080  continue;
3081 }
3082 _ij5[0] = ij5; _ij5[1] = -1;
3083 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3084 {
3085 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3086 {
3087  j5valid[iij5]=false; _ij5[1] = iij5; break;
3088 }
3089 }
3090 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3091 {
3092 IkReal evalcond[8];
3093 IkReal x228=IKsin(j5);
3094 IkReal x229=IKcos(j5);
3095 IkReal x230=((1.0)*cj4);
3096 IkReal x231=((1.0)*sj4);
3097 evalcond[0]=(x228+new_r00);
3098 evalcond[1]=(x229+new_r01);
3099 evalcond[2]=(((sj4*x228))+new_r21);
3100 evalcond[3]=(((cj4*x228))+new_r11);
3101 evalcond[4]=(new_r20+(((-1.0)*x229*x231)));
3102 evalcond[5]=(new_r10+(((-1.0)*x229*x230)));
3103 evalcond[6]=((((-1.0)*new_r20*x231))+x229+(((-1.0)*new_r10*x230)));
3104 evalcond[7]=((((-1.0)*new_r21*x231))+(((-1.0)*new_r11*x230))+(((-1.0)*x228)));
3105 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 )
3106 {
3107 continue;
3108 }
3109 }
3110 
3111 {
3112 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3113 vinfos[0].jointtype = 1;
3114 vinfos[0].foffset = j0;
3115 vinfos[0].indices[0] = _ij0[0];
3116 vinfos[0].indices[1] = _ij0[1];
3117 vinfos[0].maxsolutions = _nj0;
3118 vinfos[1].jointtype = 1;
3119 vinfos[1].foffset = j1;
3120 vinfos[1].indices[0] = _ij1[0];
3121 vinfos[1].indices[1] = _ij1[1];
3122 vinfos[1].maxsolutions = _nj1;
3123 vinfos[2].jointtype = 1;
3124 vinfos[2].foffset = j2;
3125 vinfos[2].indices[0] = _ij2[0];
3126 vinfos[2].indices[1] = _ij2[1];
3127 vinfos[2].maxsolutions = _nj2;
3128 vinfos[3].jointtype = 1;
3129 vinfos[3].foffset = j3;
3130 vinfos[3].indices[0] = _ij3[0];
3131 vinfos[3].indices[1] = _ij3[1];
3132 vinfos[3].maxsolutions = _nj3;
3133 vinfos[4].jointtype = 1;
3134 vinfos[4].foffset = j4;
3135 vinfos[4].indices[0] = _ij4[0];
3136 vinfos[4].indices[1] = _ij4[1];
3137 vinfos[4].maxsolutions = _nj4;
3138 vinfos[5].jointtype = 1;
3139 vinfos[5].foffset = j5;
3140 vinfos[5].indices[0] = _ij5[0];
3141 vinfos[5].indices[1] = _ij5[1];
3142 vinfos[5].maxsolutions = _nj5;
3143 std::vector<int> vfree(0);
3144 solutions.AddSolution(vinfos,vfree);
3145 }
3146 }
3147 }
3148 
3149 }
3150 } while(0);
3151 if( bgotonextstatement )
3152 {
3153 bool bgotonextstatement = true;
3154 do
3155 {
3156 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3157 evalcond[1]=new_r02;
3158 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3159 {
3160 bgotonextstatement=false;
3161 {
3162 IkReal j5array[1], cj5array[1], sj5array[1];
3163 bool j5valid[1]={false};
3164 _nj5 = 1;
3165 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3166  continue;
3167 j5array[0]=IKatan2(new_r00, new_r01);
3168 sj5array[0]=IKsin(j5array[0]);
3169 cj5array[0]=IKcos(j5array[0]);
3170 if( j5array[0] > IKPI )
3171 {
3172  j5array[0]-=IK2PI;
3173 }
3174 else if( j5array[0] < -IKPI )
3175 { j5array[0]+=IK2PI;
3176 }
3177 j5valid[0] = true;
3178 for(int ij5 = 0; ij5 < 1; ++ij5)
3179 {
3180 if( !j5valid[ij5] )
3181 {
3182  continue;
3183 }
3184 _ij5[0] = ij5; _ij5[1] = -1;
3185 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3186 {
3187 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3188 {
3189  j5valid[iij5]=false; _ij5[1] = iij5; break;
3190 }
3191 }
3192 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3193 {
3194 IkReal evalcond[8];
3195 IkReal x232=IKsin(j5);
3196 IkReal x233=IKcos(j5);
3197 IkReal x234=((1.0)*sj4);
3198 IkReal x235=((1.0)*x233);
3199 evalcond[0]=(((sj4*x232))+new_r21);
3200 evalcond[1]=(x232+(((-1.0)*new_r00)));
3201 evalcond[2]=(x233+(((-1.0)*new_r01)));
3202 evalcond[3]=((((-1.0)*x233*x234))+new_r20);
3203 evalcond[4]=(((cj4*x232))+(((-1.0)*new_r11)));
3204 evalcond[5]=((((-1.0)*cj4*x235))+(((-1.0)*new_r10)));
3205 evalcond[6]=((((-1.0)*new_r20*x234))+((cj4*new_r10))+x233);
3206 evalcond[7]=((((-1.0)*new_r21*x234))+((cj4*new_r11))+(((-1.0)*x232)));
3207 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 )
3208 {
3209 continue;
3210 }
3211 }
3212 
3213 {
3214 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3215 vinfos[0].jointtype = 1;
3216 vinfos[0].foffset = j0;
3217 vinfos[0].indices[0] = _ij0[0];
3218 vinfos[0].indices[1] = _ij0[1];
3219 vinfos[0].maxsolutions = _nj0;
3220 vinfos[1].jointtype = 1;
3221 vinfos[1].foffset = j1;
3222 vinfos[1].indices[0] = _ij1[0];
3223 vinfos[1].indices[1] = _ij1[1];
3224 vinfos[1].maxsolutions = _nj1;
3225 vinfos[2].jointtype = 1;
3226 vinfos[2].foffset = j2;
3227 vinfos[2].indices[0] = _ij2[0];
3228 vinfos[2].indices[1] = _ij2[1];
3229 vinfos[2].maxsolutions = _nj2;
3230 vinfos[3].jointtype = 1;
3231 vinfos[3].foffset = j3;
3232 vinfos[3].indices[0] = _ij3[0];
3233 vinfos[3].indices[1] = _ij3[1];
3234 vinfos[3].maxsolutions = _nj3;
3235 vinfos[4].jointtype = 1;
3236 vinfos[4].foffset = j4;
3237 vinfos[4].indices[0] = _ij4[0];
3238 vinfos[4].indices[1] = _ij4[1];
3239 vinfos[4].maxsolutions = _nj4;
3240 vinfos[5].jointtype = 1;
3241 vinfos[5].foffset = j5;
3242 vinfos[5].indices[0] = _ij5[0];
3243 vinfos[5].indices[1] = _ij5[1];
3244 vinfos[5].maxsolutions = _nj5;
3245 std::vector<int> vfree(0);
3246 solutions.AddSolution(vinfos,vfree);
3247 }
3248 }
3249 }
3250 
3251 }
3252 } while(0);
3253 if( bgotonextstatement )
3254 {
3255 bool bgotonextstatement = true;
3256 do
3257 {
3258 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3259 evalcond[1]=new_r12;
3260 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3261 {
3262 bgotonextstatement=false;
3263 {
3264 IkReal j5array[1], cj5array[1], sj5array[1];
3265 bool j5valid[1]={false};
3266 _nj5 = 1;
3267 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3268  continue;
3269 j5array[0]=IKatan2(new_r10, new_r11);
3270 sj5array[0]=IKsin(j5array[0]);
3271 cj5array[0]=IKcos(j5array[0]);
3272 if( j5array[0] > IKPI )
3273 {
3274  j5array[0]-=IK2PI;
3275 }
3276 else if( j5array[0] < -IKPI )
3277 { j5array[0]+=IK2PI;
3278 }
3279 j5valid[0] = true;
3280 for(int ij5 = 0; ij5 < 1; ++ij5)
3281 {
3282 if( !j5valid[ij5] )
3283 {
3284  continue;
3285 }
3286 _ij5[0] = ij5; _ij5[1] = -1;
3287 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3288 {
3289 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3290 {
3291  j5valid[iij5]=false; _ij5[1] = iij5; break;
3292 }
3293 }
3294 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3295 {
3296 IkReal evalcond[8];
3297 IkReal x236=IKcos(j5);
3298 IkReal x237=IKsin(j5);
3299 IkReal x238=((1.0)*cj4);
3300 IkReal x239=((1.0)*sj4);
3301 IkReal x240=((1.0)*x237);
3302 evalcond[0]=(((new_r02*x236))+new_r20);
3303 evalcond[1]=(x237+(((-1.0)*new_r10)));
3304 evalcond[2]=(x236+(((-1.0)*new_r11)));
3305 evalcond[3]=(((cj4*x237))+new_r01);
3306 evalcond[4]=((((-1.0)*new_r02*x240))+new_r21);
3307 evalcond[5]=((((-1.0)*x236*x238))+new_r00);
3308 evalcond[6]=((((-1.0)*new_r20*x239))+x236+(((-1.0)*new_r00*x238)));
3309 evalcond[7]=((((-1.0)*new_r21*x239))+(((-1.0)*x240))+(((-1.0)*new_r01*x238)));
3310 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 )
3311 {
3312 continue;
3313 }
3314 }
3315 
3316 {
3317 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3318 vinfos[0].jointtype = 1;
3319 vinfos[0].foffset = j0;
3320 vinfos[0].indices[0] = _ij0[0];
3321 vinfos[0].indices[1] = _ij0[1];
3322 vinfos[0].maxsolutions = _nj0;
3323 vinfos[1].jointtype = 1;
3324 vinfos[1].foffset = j1;
3325 vinfos[1].indices[0] = _ij1[0];
3326 vinfos[1].indices[1] = _ij1[1];
3327 vinfos[1].maxsolutions = _nj1;
3328 vinfos[2].jointtype = 1;
3329 vinfos[2].foffset = j2;
3330 vinfos[2].indices[0] = _ij2[0];
3331 vinfos[2].indices[1] = _ij2[1];
3332 vinfos[2].maxsolutions = _nj2;
3333 vinfos[3].jointtype = 1;
3334 vinfos[3].foffset = j3;
3335 vinfos[3].indices[0] = _ij3[0];
3336 vinfos[3].indices[1] = _ij3[1];
3337 vinfos[3].maxsolutions = _nj3;
3338 vinfos[4].jointtype = 1;
3339 vinfos[4].foffset = j4;
3340 vinfos[4].indices[0] = _ij4[0];
3341 vinfos[4].indices[1] = _ij4[1];
3342 vinfos[4].maxsolutions = _nj4;
3343 vinfos[5].jointtype = 1;
3344 vinfos[5].foffset = j5;
3345 vinfos[5].indices[0] = _ij5[0];
3346 vinfos[5].indices[1] = _ij5[1];
3347 vinfos[5].maxsolutions = _nj5;
3348 std::vector<int> vfree(0);
3349 solutions.AddSolution(vinfos,vfree);
3350 }
3351 }
3352 }
3353 
3354 }
3355 } while(0);
3356 if( bgotonextstatement )
3357 {
3358 bool bgotonextstatement = true;
3359 do
3360 {
3361 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3362 evalcond[1]=new_r12;
3363 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3364 {
3365 bgotonextstatement=false;
3366 {
3367 IkReal j5array[1], cj5array[1], sj5array[1];
3368 bool j5valid[1]={false};
3369 _nj5 = 1;
3370 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
3371  continue;
3372 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3373 sj5array[0]=IKsin(j5array[0]);
3374 cj5array[0]=IKcos(j5array[0]);
3375 if( j5array[0] > IKPI )
3376 {
3377  j5array[0]-=IK2PI;
3378 }
3379 else if( j5array[0] < -IKPI )
3380 { j5array[0]+=IK2PI;
3381 }
3382 j5valid[0] = true;
3383 for(int ij5 = 0; ij5 < 1; ++ij5)
3384 {
3385 if( !j5valid[ij5] )
3386 {
3387  continue;
3388 }
3389 _ij5[0] = ij5; _ij5[1] = -1;
3390 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3391 {
3392 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3393 {
3394  j5valid[iij5]=false; _ij5[1] = iij5; break;
3395 }
3396 }
3397 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3398 {
3399 IkReal evalcond[8];
3400 IkReal x241=IKsin(j5);
3401 IkReal x242=IKcos(j5);
3402 IkReal x243=((1.0)*sj4);
3403 IkReal x244=((1.0)*x242);
3404 evalcond[0]=(x241+new_r10);
3405 evalcond[1]=(x242+new_r11);
3406 evalcond[2]=(new_r21+((new_r02*x241)));
3407 evalcond[3]=((((-1.0)*new_r02*x244))+new_r20);
3408 evalcond[4]=(((cj4*x241))+(((-1.0)*new_r01)));
3409 evalcond[5]=((((-1.0)*cj4*x244))+(((-1.0)*new_r00)));
3410 evalcond[6]=(((cj4*new_r00))+x242+(((-1.0)*new_r20*x243)));
3411 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x241))+(((-1.0)*new_r21*x243)));
3412 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 )
3413 {
3414 continue;
3415 }
3416 }
3417 
3418 {
3419 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3420 vinfos[0].jointtype = 1;
3421 vinfos[0].foffset = j0;
3422 vinfos[0].indices[0] = _ij0[0];
3423 vinfos[0].indices[1] = _ij0[1];
3424 vinfos[0].maxsolutions = _nj0;
3425 vinfos[1].jointtype = 1;
3426 vinfos[1].foffset = j1;
3427 vinfos[1].indices[0] = _ij1[0];
3428 vinfos[1].indices[1] = _ij1[1];
3429 vinfos[1].maxsolutions = _nj1;
3430 vinfos[2].jointtype = 1;
3431 vinfos[2].foffset = j2;
3432 vinfos[2].indices[0] = _ij2[0];
3433 vinfos[2].indices[1] = _ij2[1];
3434 vinfos[2].maxsolutions = _nj2;
3435 vinfos[3].jointtype = 1;
3436 vinfos[3].foffset = j3;
3437 vinfos[3].indices[0] = _ij3[0];
3438 vinfos[3].indices[1] = _ij3[1];
3439 vinfos[3].maxsolutions = _nj3;
3440 vinfos[4].jointtype = 1;
3441 vinfos[4].foffset = j4;
3442 vinfos[4].indices[0] = _ij4[0];
3443 vinfos[4].indices[1] = _ij4[1];
3444 vinfos[4].maxsolutions = _nj4;
3445 vinfos[5].jointtype = 1;
3446 vinfos[5].foffset = j5;
3447 vinfos[5].indices[0] = _ij5[0];
3448 vinfos[5].indices[1] = _ij5[1];
3449 vinfos[5].maxsolutions = _nj5;
3450 std::vector<int> vfree(0);
3451 solutions.AddSolution(vinfos,vfree);
3452 }
3453 }
3454 }
3455 
3456 }
3457 } while(0);
3458 if( bgotonextstatement )
3459 {
3460 bool bgotonextstatement = true;
3461 do
3462 {
3463 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3464 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3465 {
3466 bgotonextstatement=false;
3467 {
3468 IkReal j5eval[1];
3469 new_r21=0;
3470 new_r20=0;
3471 new_r02=0;
3472 new_r12=0;
3473 j5eval[0]=1.0;
3474 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3475 {
3476 continue; // no branches [j5]
3477 
3478 } else
3479 {
3480 IkReal op[2+1], zeror[2];
3481 int numroots;
3482 op[0]=-1.0;
3483 op[1]=0;
3484 op[2]=1.0;
3485 polyroots2(op,zeror,numroots);
3486 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3487 int numsolutions = 0;
3488 for(int ij5 = 0; ij5 < numroots; ++ij5)
3489 {
3490 IkReal htj5 = zeror[ij5];
3491 tempj5array[0]=((2.0)*(atan(htj5)));
3492 for(int kj5 = 0; kj5 < 1; ++kj5)
3493 {
3494 j5array[numsolutions] = tempj5array[kj5];
3495 if( j5array[numsolutions] > IKPI )
3496 {
3497  j5array[numsolutions]-=IK2PI;
3498 }
3499 else if( j5array[numsolutions] < -IKPI )
3500 {
3501  j5array[numsolutions]+=IK2PI;
3502 }
3503 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3504 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3505 numsolutions++;
3506 }
3507 }
3508 bool j5valid[2]={true,true};
3509 _nj5 = 2;
3510 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3511  {
3512 if( !j5valid[ij5] )
3513 {
3514  continue;
3515 }
3516  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3517 htj5 = IKtan(j5/2);
3518 
3519 _ij5[0] = ij5; _ij5[1] = -1;
3520 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3521 {
3522 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3523 {
3524  j5valid[iij5]=false; _ij5[1] = iij5; break;
3525 }
3526 }
3527 {
3528 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3529 vinfos[0].jointtype = 1;
3530 vinfos[0].foffset = j0;
3531 vinfos[0].indices[0] = _ij0[0];
3532 vinfos[0].indices[1] = _ij0[1];
3533 vinfos[0].maxsolutions = _nj0;
3534 vinfos[1].jointtype = 1;
3535 vinfos[1].foffset = j1;
3536 vinfos[1].indices[0] = _ij1[0];
3537 vinfos[1].indices[1] = _ij1[1];
3538 vinfos[1].maxsolutions = _nj1;
3539 vinfos[2].jointtype = 1;
3540 vinfos[2].foffset = j2;
3541 vinfos[2].indices[0] = _ij2[0];
3542 vinfos[2].indices[1] = _ij2[1];
3543 vinfos[2].maxsolutions = _nj2;
3544 vinfos[3].jointtype = 1;
3545 vinfos[3].foffset = j3;
3546 vinfos[3].indices[0] = _ij3[0];
3547 vinfos[3].indices[1] = _ij3[1];
3548 vinfos[3].maxsolutions = _nj3;
3549 vinfos[4].jointtype = 1;
3550 vinfos[4].foffset = j4;
3551 vinfos[4].indices[0] = _ij4[0];
3552 vinfos[4].indices[1] = _ij4[1];
3553 vinfos[4].maxsolutions = _nj4;
3554 vinfos[5].jointtype = 1;
3555 vinfos[5].foffset = j5;
3556 vinfos[5].indices[0] = _ij5[0];
3557 vinfos[5].indices[1] = _ij5[1];
3558 vinfos[5].maxsolutions = _nj5;
3559 std::vector<int> vfree(0);
3560 solutions.AddSolution(vinfos,vfree);
3561 }
3562  }
3563 
3564 }
3565 
3566 }
3567 
3568 }
3569 } while(0);
3570 if( bgotonextstatement )
3571 {
3572 bool bgotonextstatement = true;
3573 do
3574 {
3575 if( 1 )
3576 {
3577 bgotonextstatement=false;
3578 continue; // branch miss [j5]
3579 
3580 }
3581 } while(0);
3582 if( bgotonextstatement )
3583 {
3584 }
3585 }
3586 }
3587 }
3588 }
3589 }
3590 }
3591 }
3592 }
3593 
3594 } else
3595 {
3596 {
3597 IkReal j5array[1], cj5array[1], sj5array[1];
3598 bool j5valid[1]={false};
3599 _nj5 = 1;
3600 CheckValue<IkReal> x246=IKPowWithIntegerCheck(sj4,-1);
3601 if(!x246.valid){
3602 continue;
3603 }
3604 IkReal x245=x246.value;
3605 CheckValue<IkReal> x247=IKPowWithIntegerCheck(cj3,-1);
3606 if(!x247.valid){
3607 continue;
3608 }
3609 if( IKabs(((-1.0)*new_r21*x245)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x245))+IKsqr((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3610  continue;
3611 j5array[0]=IKatan2(((-1.0)*new_r21*x245), (x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3612 sj5array[0]=IKsin(j5array[0]);
3613 cj5array[0]=IKcos(j5array[0]);
3614 if( j5array[0] > IKPI )
3615 {
3616  j5array[0]-=IK2PI;
3617 }
3618 else if( j5array[0] < -IKPI )
3619 { j5array[0]+=IK2PI;
3620 }
3621 j5valid[0] = true;
3622 for(int ij5 = 0; ij5 < 1; ++ij5)
3623 {
3624 if( !j5valid[ij5] )
3625 {
3626  continue;
3627 }
3628 _ij5[0] = ij5; _ij5[1] = -1;
3629 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3630 {
3631 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3632 {
3633  j5valid[iij5]=false; _ij5[1] = iij5; break;
3634 }
3635 }
3636 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3637 {
3638 IkReal evalcond[12];
3639 IkReal x248=IKsin(j5);
3640 IkReal x249=IKcos(j5);
3641 IkReal x250=(cj3*new_r00);
3642 IkReal x251=(cj3*cj4);
3643 IkReal x252=((1.0)*cj3);
3644 IkReal x253=(new_r11*sj3);
3645 IkReal x254=((1.0)*cj4);
3646 IkReal x255=(new_r10*sj3);
3647 IkReal x256=((1.0)*sj4);
3648 IkReal x257=(sj3*x248);
3649 IkReal x258=((1.0)*x249);
3650 IkReal x259=(sj3*x249);
3651 evalcond[0]=(new_r21+((sj4*x248)));
3652 evalcond[1]=((((-1.0)*x249*x256))+new_r20);
3653 evalcond[2]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x248);
3654 evalcond[3]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x249);
3655 evalcond[4]=(((cj4*x248))+x253+((cj3*new_r01)));
3656 evalcond[5]=(((x248*x251))+x259+new_r01);
3657 evalcond[6]=(x255+x250+(((-1.0)*x249*x254)));
3658 evalcond[7]=((((-1.0)*x251*x258))+x257+new_r00);
3659 evalcond[8]=((((-1.0)*x249*x252))+new_r11+((cj4*x257)));
3660 evalcond[9]=((((-1.0)*x254*x259))+(((-1.0)*x248*x252))+new_r10);
3661 evalcond[10]=((((-1.0)*x250*x254))+(((-1.0)*x254*x255))+x249+(((-1.0)*new_r20*x256)));
3662 evalcond[11]=((((-1.0)*new_r21*x256))+(((-1.0)*x248))+(((-1.0)*x253*x254))+(((-1.0)*new_r01*x251)));
3663 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 )
3664 {
3665 continue;
3666 }
3667 }
3668 
3669 {
3670 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3671 vinfos[0].jointtype = 1;
3672 vinfos[0].foffset = j0;
3673 vinfos[0].indices[0] = _ij0[0];
3674 vinfos[0].indices[1] = _ij0[1];
3675 vinfos[0].maxsolutions = _nj0;
3676 vinfos[1].jointtype = 1;
3677 vinfos[1].foffset = j1;
3678 vinfos[1].indices[0] = _ij1[0];
3679 vinfos[1].indices[1] = _ij1[1];
3680 vinfos[1].maxsolutions = _nj1;
3681 vinfos[2].jointtype = 1;
3682 vinfos[2].foffset = j2;
3683 vinfos[2].indices[0] = _ij2[0];
3684 vinfos[2].indices[1] = _ij2[1];
3685 vinfos[2].maxsolutions = _nj2;
3686 vinfos[3].jointtype = 1;
3687 vinfos[3].foffset = j3;
3688 vinfos[3].indices[0] = _ij3[0];
3689 vinfos[3].indices[1] = _ij3[1];
3690 vinfos[3].maxsolutions = _nj3;
3691 vinfos[4].jointtype = 1;
3692 vinfos[4].foffset = j4;
3693 vinfos[4].indices[0] = _ij4[0];
3694 vinfos[4].indices[1] = _ij4[1];
3695 vinfos[4].maxsolutions = _nj4;
3696 vinfos[5].jointtype = 1;
3697 vinfos[5].foffset = j5;
3698 vinfos[5].indices[0] = _ij5[0];
3699 vinfos[5].indices[1] = _ij5[1];
3700 vinfos[5].maxsolutions = _nj5;
3701 std::vector<int> vfree(0);
3702 solutions.AddSolution(vinfos,vfree);
3703 }
3704 }
3705 }
3706 
3707 }
3708 
3709 }
3710 
3711 } else
3712 {
3713 {
3714 IkReal j5array[1], cj5array[1], sj5array[1];
3715 bool j5valid[1]={false};
3716 _nj5 = 1;
3717 CheckValue<IkReal> x261=IKPowWithIntegerCheck(sj4,-1);
3718 if(!x261.valid){
3719 continue;
3720 }
3721 IkReal x260=x261.value;
3722 CheckValue<IkReal> x262=IKPowWithIntegerCheck(sj3,-1);
3723 if(!x262.valid){
3724 continue;
3725 }
3726 if( IKabs(((-1.0)*new_r21*x260)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x260))+IKsqr((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3727  continue;
3728 j5array[0]=IKatan2(((-1.0)*new_r21*x260), (x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3729 sj5array[0]=IKsin(j5array[0]);
3730 cj5array[0]=IKcos(j5array[0]);
3731 if( j5array[0] > IKPI )
3732 {
3733  j5array[0]-=IK2PI;
3734 }
3735 else if( j5array[0] < -IKPI )
3736 { j5array[0]+=IK2PI;
3737 }
3738 j5valid[0] = true;
3739 for(int ij5 = 0; ij5 < 1; ++ij5)
3740 {
3741 if( !j5valid[ij5] )
3742 {
3743  continue;
3744 }
3745 _ij5[0] = ij5; _ij5[1] = -1;
3746 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3747 {
3748 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3749 {
3750  j5valid[iij5]=false; _ij5[1] = iij5; break;
3751 }
3752 }
3753 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3754 {
3755 IkReal evalcond[12];
3756 IkReal x263=IKsin(j5);
3757 IkReal x264=IKcos(j5);
3758 IkReal x265=(cj3*new_r00);
3759 IkReal x266=(cj3*cj4);
3760 IkReal x267=((1.0)*cj3);
3761 IkReal x268=(new_r11*sj3);
3762 IkReal x269=((1.0)*cj4);
3763 IkReal x270=(new_r10*sj3);
3764 IkReal x271=((1.0)*sj4);
3765 IkReal x272=(sj3*x263);
3766 IkReal x273=((1.0)*x264);
3767 IkReal x274=(sj3*x264);
3768 evalcond[0]=(new_r21+((sj4*x263)));
3769 evalcond[1]=(new_r20+(((-1.0)*x264*x271)));
3770 evalcond[2]=(((new_r00*sj3))+x263+(((-1.0)*new_r10*x267)));
3771 evalcond[3]=(((new_r01*sj3))+x264+(((-1.0)*new_r11*x267)));
3772 evalcond[4]=(((cj4*x263))+x268+((cj3*new_r01)));
3773 evalcond[5]=(((x263*x266))+x274+new_r01);
3774 evalcond[6]=(x265+x270+(((-1.0)*x264*x269)));
3775 evalcond[7]=(x272+(((-1.0)*x266*x273))+new_r00);
3776 evalcond[8]=(((cj4*x272))+new_r11+(((-1.0)*x264*x267)));
3777 evalcond[9]=((((-1.0)*x263*x267))+(((-1.0)*x269*x274))+new_r10);
3778 evalcond[10]=((((-1.0)*x269*x270))+x264+(((-1.0)*new_r20*x271))+(((-1.0)*x265*x269)));
3779 evalcond[11]=((((-1.0)*x263))+(((-1.0)*new_r01*x266))+(((-1.0)*new_r21*x271))+(((-1.0)*x268*x269)));
3780 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 )
3781 {
3782 continue;
3783 }
3784 }
3785 
3786 {
3787 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3788 vinfos[0].jointtype = 1;
3789 vinfos[0].foffset = j0;
3790 vinfos[0].indices[0] = _ij0[0];
3791 vinfos[0].indices[1] = _ij0[1];
3792 vinfos[0].maxsolutions = _nj0;
3793 vinfos[1].jointtype = 1;
3794 vinfos[1].foffset = j1;
3795 vinfos[1].indices[0] = _ij1[0];
3796 vinfos[1].indices[1] = _ij1[1];
3797 vinfos[1].maxsolutions = _nj1;
3798 vinfos[2].jointtype = 1;
3799 vinfos[2].foffset = j2;
3800 vinfos[2].indices[0] = _ij2[0];
3801 vinfos[2].indices[1] = _ij2[1];
3802 vinfos[2].maxsolutions = _nj2;
3803 vinfos[3].jointtype = 1;
3804 vinfos[3].foffset = j3;
3805 vinfos[3].indices[0] = _ij3[0];
3806 vinfos[3].indices[1] = _ij3[1];
3807 vinfos[3].maxsolutions = _nj3;
3808 vinfos[4].jointtype = 1;
3809 vinfos[4].foffset = j4;
3810 vinfos[4].indices[0] = _ij4[0];
3811 vinfos[4].indices[1] = _ij4[1];
3812 vinfos[4].maxsolutions = _nj4;
3813 vinfos[5].jointtype = 1;
3814 vinfos[5].foffset = j5;
3815 vinfos[5].indices[0] = _ij5[0];
3816 vinfos[5].indices[1] = _ij5[1];
3817 vinfos[5].maxsolutions = _nj5;
3818 std::vector<int> vfree(0);
3819 solutions.AddSolution(vinfos,vfree);
3820 }
3821 }
3822 }
3823 
3824 }
3825 
3826 }
3827 
3828 } else
3829 {
3830 {
3831 IkReal j5array[1], cj5array[1], sj5array[1];
3832 bool j5valid[1]={false};
3833 _nj5 = 1;
3834 CheckValue<IkReal> x275 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3835 if(!x275.valid){
3836 continue;
3837 }
3838 CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(sj4),-1);
3839 if(!x276.valid){
3840 continue;
3841 }
3842 j5array[0]=((-1.5707963267949)+(x275.value)+(((1.5707963267949)*(x276.value))));
3843 sj5array[0]=IKsin(j5array[0]);
3844 cj5array[0]=IKcos(j5array[0]);
3845 if( j5array[0] > IKPI )
3846 {
3847  j5array[0]-=IK2PI;
3848 }
3849 else if( j5array[0] < -IKPI )
3850 { j5array[0]+=IK2PI;
3851 }
3852 j5valid[0] = true;
3853 for(int ij5 = 0; ij5 < 1; ++ij5)
3854 {
3855 if( !j5valid[ij5] )
3856 {
3857  continue;
3858 }
3859 _ij5[0] = ij5; _ij5[1] = -1;
3860 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3861 {
3862 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3863 {
3864  j5valid[iij5]=false; _ij5[1] = iij5; break;
3865 }
3866 }
3867 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3868 {
3869 IkReal evalcond[12];
3870 IkReal x277=IKsin(j5);
3871 IkReal x278=IKcos(j5);
3872 IkReal x279=(cj3*new_r00);
3873 IkReal x280=(cj3*cj4);
3874 IkReal x281=((1.0)*cj3);
3875 IkReal x282=(new_r11*sj3);
3876 IkReal x283=((1.0)*cj4);
3877 IkReal x284=(new_r10*sj3);
3878 IkReal x285=((1.0)*sj4);
3879 IkReal x286=(sj3*x277);
3880 IkReal x287=((1.0)*x278);
3881 IkReal x288=(sj3*x278);
3882 evalcond[0]=(new_r21+((sj4*x277)));
3883 evalcond[1]=(new_r20+(((-1.0)*x278*x285)));
3884 evalcond[2]=(((new_r00*sj3))+x277+(((-1.0)*new_r10*x281)));
3885 evalcond[3]=(((new_r01*sj3))+x278+(((-1.0)*new_r11*x281)));
3886 evalcond[4]=(((cj4*x277))+x282+((cj3*new_r01)));
3887 evalcond[5]=(x288+new_r01+((x277*x280)));
3888 evalcond[6]=(x279+x284+(((-1.0)*x278*x283)));
3889 evalcond[7]=((((-1.0)*x280*x287))+x286+new_r00);
3890 evalcond[8]=(new_r11+((cj4*x286))+(((-1.0)*x278*x281)));
3891 evalcond[9]=((((-1.0)*x277*x281))+new_r10+(((-1.0)*x283*x288)));
3892 evalcond[10]=(x278+(((-1.0)*new_r20*x285))+(((-1.0)*x279*x283))+(((-1.0)*x283*x284)));
3893 evalcond[11]=((((-1.0)*x277))+(((-1.0)*x282*x283))+(((-1.0)*new_r01*x280))+(((-1.0)*new_r21*x285)));
3894 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 )
3895 {
3896 continue;
3897 }
3898 }
3899 
3900 {
3901 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3902 vinfos[0].jointtype = 1;
3903 vinfos[0].foffset = j0;
3904 vinfos[0].indices[0] = _ij0[0];
3905 vinfos[0].indices[1] = _ij0[1];
3906 vinfos[0].maxsolutions = _nj0;
3907 vinfos[1].jointtype = 1;
3908 vinfos[1].foffset = j1;
3909 vinfos[1].indices[0] = _ij1[0];
3910 vinfos[1].indices[1] = _ij1[1];
3911 vinfos[1].maxsolutions = _nj1;
3912 vinfos[2].jointtype = 1;
3913 vinfos[2].foffset = j2;
3914 vinfos[2].indices[0] = _ij2[0];
3915 vinfos[2].indices[1] = _ij2[1];
3916 vinfos[2].maxsolutions = _nj2;
3917 vinfos[3].jointtype = 1;
3918 vinfos[3].foffset = j3;
3919 vinfos[3].indices[0] = _ij3[0];
3920 vinfos[3].indices[1] = _ij3[1];
3921 vinfos[3].maxsolutions = _nj3;
3922 vinfos[4].jointtype = 1;
3923 vinfos[4].foffset = j4;
3924 vinfos[4].indices[0] = _ij4[0];
3925 vinfos[4].indices[1] = _ij4[1];
3926 vinfos[4].maxsolutions = _nj4;
3927 vinfos[5].jointtype = 1;
3928 vinfos[5].foffset = j5;
3929 vinfos[5].indices[0] = _ij5[0];
3930 vinfos[5].indices[1] = _ij5[1];
3931 vinfos[5].maxsolutions = _nj5;
3932 std::vector<int> vfree(0);
3933 solutions.AddSolution(vinfos,vfree);
3934 }
3935 }
3936 }
3937 
3938 }
3939 
3940 }
3941 }
3942 }
3943 
3944 }
3945 
3946 }
3947 
3948 } else
3949 {
3950 {
3951 IkReal j3array[1], cj3array[1], sj3array[1];
3952 bool j3valid[1]={false};
3953 _nj3 = 1;
3954 CheckValue<IkReal> x289=IKPowWithIntegerCheck(IKsign(sj4),-1);
3955 if(!x289.valid){
3956 continue;
3957 }
3958 CheckValue<IkReal> x290 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3959 if(!x290.valid){
3960 continue;
3961 }
3962 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x289.value)))+(x290.value));
3963 sj3array[0]=IKsin(j3array[0]);
3964 cj3array[0]=IKcos(j3array[0]);
3965 if( j3array[0] > IKPI )
3966 {
3967  j3array[0]-=IK2PI;
3968 }
3969 else if( j3array[0] < -IKPI )
3970 { j3array[0]+=IK2PI;
3971 }
3972 j3valid[0] = true;
3973 for(int ij3 = 0; ij3 < 1; ++ij3)
3974 {
3975 if( !j3valid[ij3] )
3976 {
3977  continue;
3978 }
3979 _ij3[0] = ij3; _ij3[1] = -1;
3980 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3981 {
3982 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3983 {
3984  j3valid[iij3]=false; _ij3[1] = iij3; break;
3985 }
3986 }
3987 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3988 {
3989 IkReal evalcond[8];
3990 IkReal x291=IKcos(j3);
3991 IkReal x292=IKsin(j3);
3992 IkReal x293=((1.0)*cj4);
3993 IkReal x294=(sj4*x292);
3994 IkReal x295=(new_r12*x292);
3995 IkReal x296=(sj4*x291);
3996 IkReal x297=(new_r02*x291);
3997 evalcond[0]=(x296+new_r02);
3998 evalcond[1]=(x294+new_r12);
3999 evalcond[2]=(((new_r12*x291))+(((-1.0)*new_r02*x292)));
4000 evalcond[3]=(sj4+x295+x297);
4001 evalcond[4]=((((-1.0)*new_r20*x293))+((new_r10*x294))+((new_r00*x296)));
4002 evalcond[5]=((((-1.0)*new_r21*x293))+((new_r11*x294))+((new_r01*x296)));
4003 evalcond[6]=((1.0)+((new_r02*x296))+((new_r12*x294))+(((-1.0)*new_r22*x293)));
4004 evalcond[7]=((((-1.0)*x293*x297))+(((-1.0)*x293*x295))+(((-1.0)*new_r22*sj4)));
4005 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 )
4006 {
4007 continue;
4008 }
4009 }
4010 
4011 {
4012 IkReal j5eval[3];
4013 j5eval[0]=sj4;
4014 j5eval[1]=IKsign(sj4);
4015 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4016 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4017 {
4018 {
4019 IkReal j5eval[2];
4020 j5eval[0]=sj4;
4021 j5eval[1]=sj3;
4022 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4023 {
4024 {
4025 IkReal j5eval[2];
4026 j5eval[0]=sj4;
4027 j5eval[1]=cj3;
4028 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4029 {
4030 {
4031 IkReal evalcond[5];
4032 bool bgotonextstatement = true;
4033 do
4034 {
4035 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4036 evalcond[1]=new_r02;
4037 evalcond[2]=new_r12;
4038 evalcond[3]=new_r21;
4039 evalcond[4]=new_r20;
4040 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 )
4041 {
4042 bgotonextstatement=false;
4043 {
4044 IkReal j5array[1], cj5array[1], sj5array[1];
4045 bool j5valid[1]={false};
4046 _nj5 = 1;
4047 IkReal x298=((1.0)*new_r01);
4048 if( IKabs(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x298))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x298))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
4049  continue;
4050 j5array[0]=IKatan2(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x298))+((cj3*new_r00))));
4051 sj5array[0]=IKsin(j5array[0]);
4052 cj5array[0]=IKcos(j5array[0]);
4053 if( j5array[0] > IKPI )
4054 {
4055  j5array[0]-=IK2PI;
4056 }
4057 else if( j5array[0] < -IKPI )
4058 { j5array[0]+=IK2PI;
4059 }
4060 j5valid[0] = true;
4061 for(int ij5 = 0; ij5 < 1; ++ij5)
4062 {
4063 if( !j5valid[ij5] )
4064 {
4065  continue;
4066 }
4067 _ij5[0] = ij5; _ij5[1] = -1;
4068 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4069 {
4070 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4071 {
4072  j5valid[iij5]=false; _ij5[1] = iij5; break;
4073 }
4074 }
4075 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4076 {
4077 IkReal evalcond[8];
4078 IkReal x299=IKsin(j5);
4079 IkReal x300=IKcos(j5);
4080 IkReal x301=((1.0)*cj3);
4081 IkReal x302=(sj3*x299);
4082 IkReal x303=((1.0)*x300);
4083 IkReal x304=(x300*x301);
4084 evalcond[0]=(((new_r11*sj3))+x299+((cj3*new_r01)));
4085 evalcond[1]=(((new_r00*sj3))+x299+(((-1.0)*new_r10*x301)));
4086 evalcond[2]=(((new_r01*sj3))+x300+(((-1.0)*new_r11*x301)));
4087 evalcond[3]=(((cj3*x299))+((sj3*x300))+new_r01);
4088 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x303)));
4089 evalcond[5]=(x302+new_r00+(((-1.0)*x304)));
4090 evalcond[6]=(x302+new_r11+(((-1.0)*x304)));
4091 evalcond[7]=((((-1.0)*sj3*x303))+(((-1.0)*x299*x301))+new_r10);
4092 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 )
4093 {
4094 continue;
4095 }
4096 }
4097 
4098 {
4099 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4100 vinfos[0].jointtype = 1;
4101 vinfos[0].foffset = j0;
4102 vinfos[0].indices[0] = _ij0[0];
4103 vinfos[0].indices[1] = _ij0[1];
4104 vinfos[0].maxsolutions = _nj0;
4105 vinfos[1].jointtype = 1;
4106 vinfos[1].foffset = j1;
4107 vinfos[1].indices[0] = _ij1[0];
4108 vinfos[1].indices[1] = _ij1[1];
4109 vinfos[1].maxsolutions = _nj1;
4110 vinfos[2].jointtype = 1;
4111 vinfos[2].foffset = j2;
4112 vinfos[2].indices[0] = _ij2[0];
4113 vinfos[2].indices[1] = _ij2[1];
4114 vinfos[2].maxsolutions = _nj2;
4115 vinfos[3].jointtype = 1;
4116 vinfos[3].foffset = j3;
4117 vinfos[3].indices[0] = _ij3[0];
4118 vinfos[3].indices[1] = _ij3[1];
4119 vinfos[3].maxsolutions = _nj3;
4120 vinfos[4].jointtype = 1;
4121 vinfos[4].foffset = j4;
4122 vinfos[4].indices[0] = _ij4[0];
4123 vinfos[4].indices[1] = _ij4[1];
4124 vinfos[4].maxsolutions = _nj4;
4125 vinfos[5].jointtype = 1;
4126 vinfos[5].foffset = j5;
4127 vinfos[5].indices[0] = _ij5[0];
4128 vinfos[5].indices[1] = _ij5[1];
4129 vinfos[5].maxsolutions = _nj5;
4130 std::vector<int> vfree(0);
4131 solutions.AddSolution(vinfos,vfree);
4132 }
4133 }
4134 }
4135 
4136 }
4137 } while(0);
4138 if( bgotonextstatement )
4139 {
4140 bool bgotonextstatement = true;
4141 do
4142 {
4143 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4144 evalcond[1]=new_r02;
4145 evalcond[2]=new_r12;
4146 evalcond[3]=new_r21;
4147 evalcond[4]=new_r20;
4148 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 )
4149 {
4150 bgotonextstatement=false;
4151 {
4152 IkReal j5array[1], cj5array[1], sj5array[1];
4153 bool j5valid[1]={false};
4154 _nj5 = 1;
4155 IkReal x305=((1.0)*sj3);
4156 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x305))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))))-1) <= IKFAST_SINCOS_THRESH )
4157  continue;
4158 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x305))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))));
4159 sj5array[0]=IKsin(j5array[0]);
4160 cj5array[0]=IKcos(j5array[0]);
4161 if( j5array[0] > IKPI )
4162 {
4163  j5array[0]-=IK2PI;
4164 }
4165 else if( j5array[0] < -IKPI )
4166 { j5array[0]+=IK2PI;
4167 }
4168 j5valid[0] = true;
4169 for(int ij5 = 0; ij5 < 1; ++ij5)
4170 {
4171 if( !j5valid[ij5] )
4172 {
4173  continue;
4174 }
4175 _ij5[0] = ij5; _ij5[1] = -1;
4176 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4177 {
4178 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4179 {
4180  j5valid[iij5]=false; _ij5[1] = iij5; break;
4181 }
4182 }
4183 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4184 {
4185 IkReal evalcond[8];
4186 IkReal x306=IKcos(j5);
4187 IkReal x307=IKsin(j5);
4188 IkReal x308=((1.0)*cj3);
4189 IkReal x309=(sj3*x306);
4190 IkReal x310=((1.0)*x307);
4191 IkReal x311=(x307*x308);
4192 evalcond[0]=(((new_r10*sj3))+x306+((cj3*new_r00)));
4193 evalcond[1]=(((new_r00*sj3))+x307+(((-1.0)*new_r10*x308)));
4194 evalcond[2]=(((new_r01*sj3))+x306+(((-1.0)*new_r11*x308)));
4195 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x310))+((cj3*new_r01)));
4196 evalcond[4]=(((sj3*x307))+((cj3*x306))+new_r00);
4197 evalcond[5]=(x309+(((-1.0)*x311))+new_r01);
4198 evalcond[6]=(x309+(((-1.0)*x311))+new_r10);
4199 evalcond[7]=((((-1.0)*sj3*x310))+(((-1.0)*x306*x308))+new_r11);
4200 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 )
4201 {
4202 continue;
4203 }
4204 }
4205 
4206 {
4207 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4208 vinfos[0].jointtype = 1;
4209 vinfos[0].foffset = j0;
4210 vinfos[0].indices[0] = _ij0[0];
4211 vinfos[0].indices[1] = _ij0[1];
4212 vinfos[0].maxsolutions = _nj0;
4213 vinfos[1].jointtype = 1;
4214 vinfos[1].foffset = j1;
4215 vinfos[1].indices[0] = _ij1[0];
4216 vinfos[1].indices[1] = _ij1[1];
4217 vinfos[1].maxsolutions = _nj1;
4218 vinfos[2].jointtype = 1;
4219 vinfos[2].foffset = j2;
4220 vinfos[2].indices[0] = _ij2[0];
4221 vinfos[2].indices[1] = _ij2[1];
4222 vinfos[2].maxsolutions = _nj2;
4223 vinfos[3].jointtype = 1;
4224 vinfos[3].foffset = j3;
4225 vinfos[3].indices[0] = _ij3[0];
4226 vinfos[3].indices[1] = _ij3[1];
4227 vinfos[3].maxsolutions = _nj3;
4228 vinfos[4].jointtype = 1;
4229 vinfos[4].foffset = j4;
4230 vinfos[4].indices[0] = _ij4[0];
4231 vinfos[4].indices[1] = _ij4[1];
4232 vinfos[4].maxsolutions = _nj4;
4233 vinfos[5].jointtype = 1;
4234 vinfos[5].foffset = j5;
4235 vinfos[5].indices[0] = _ij5[0];
4236 vinfos[5].indices[1] = _ij5[1];
4237 vinfos[5].maxsolutions = _nj5;
4238 std::vector<int> vfree(0);
4239 solutions.AddSolution(vinfos,vfree);
4240 }
4241 }
4242 }
4243 
4244 }
4245 } while(0);
4246 if( bgotonextstatement )
4247 {
4248 bool bgotonextstatement = true;
4249 do
4250 {
4251 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4252 evalcond[1]=new_r02;
4253 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4254 {
4255 bgotonextstatement=false;
4256 {
4257 IkReal j5array[1], cj5array[1], sj5array[1];
4258 bool j5valid[1]={false};
4259 _nj5 = 1;
4260 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
4261  continue;
4262 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4263 sj5array[0]=IKsin(j5array[0]);
4264 cj5array[0]=IKcos(j5array[0]);
4265 if( j5array[0] > IKPI )
4266 {
4267  j5array[0]-=IK2PI;
4268 }
4269 else if( j5array[0] < -IKPI )
4270 { j5array[0]+=IK2PI;
4271 }
4272 j5valid[0] = true;
4273 for(int ij5 = 0; ij5 < 1; ++ij5)
4274 {
4275 if( !j5valid[ij5] )
4276 {
4277  continue;
4278 }
4279 _ij5[0] = ij5; _ij5[1] = -1;
4280 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4281 {
4282 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4283 {
4284  j5valid[iij5]=false; _ij5[1] = iij5; break;
4285 }
4286 }
4287 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4288 {
4289 IkReal evalcond[8];
4290 IkReal x312=IKsin(j5);
4291 IkReal x313=IKcos(j5);
4292 IkReal x314=((1.0)*cj4);
4293 IkReal x315=((1.0)*sj4);
4294 evalcond[0]=(x312+new_r00);
4295 evalcond[1]=(x313+new_r01);
4296 evalcond[2]=(((sj4*x312))+new_r21);
4297 evalcond[3]=(((cj4*x312))+new_r11);
4298 evalcond[4]=((((-1.0)*x313*x315))+new_r20);
4299 evalcond[5]=((((-1.0)*x313*x314))+new_r10);
4300 evalcond[6]=((((-1.0)*new_r20*x315))+x313+(((-1.0)*new_r10*x314)));
4301 evalcond[7]=((((-1.0)*new_r21*x315))+(((-1.0)*new_r11*x314))+(((-1.0)*x312)));
4302 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 )
4303 {
4304 continue;
4305 }
4306 }
4307 
4308 {
4309 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4310 vinfos[0].jointtype = 1;
4311 vinfos[0].foffset = j0;
4312 vinfos[0].indices[0] = _ij0[0];
4313 vinfos[0].indices[1] = _ij0[1];
4314 vinfos[0].maxsolutions = _nj0;
4315 vinfos[1].jointtype = 1;
4316 vinfos[1].foffset = j1;
4317 vinfos[1].indices[0] = _ij1[0];
4318 vinfos[1].indices[1] = _ij1[1];
4319 vinfos[1].maxsolutions = _nj1;
4320 vinfos[2].jointtype = 1;
4321 vinfos[2].foffset = j2;
4322 vinfos[2].indices[0] = _ij2[0];
4323 vinfos[2].indices[1] = _ij2[1];
4324 vinfos[2].maxsolutions = _nj2;
4325 vinfos[3].jointtype = 1;
4326 vinfos[3].foffset = j3;
4327 vinfos[3].indices[0] = _ij3[0];
4328 vinfos[3].indices[1] = _ij3[1];
4329 vinfos[3].maxsolutions = _nj3;
4330 vinfos[4].jointtype = 1;
4331 vinfos[4].foffset = j4;
4332 vinfos[4].indices[0] = _ij4[0];
4333 vinfos[4].indices[1] = _ij4[1];
4334 vinfos[4].maxsolutions = _nj4;
4335 vinfos[5].jointtype = 1;
4336 vinfos[5].foffset = j5;
4337 vinfos[5].indices[0] = _ij5[0];
4338 vinfos[5].indices[1] = _ij5[1];
4339 vinfos[5].maxsolutions = _nj5;
4340 std::vector<int> vfree(0);
4341 solutions.AddSolution(vinfos,vfree);
4342 }
4343 }
4344 }
4345 
4346 }
4347 } while(0);
4348 if( bgotonextstatement )
4349 {
4350 bool bgotonextstatement = true;
4351 do
4352 {
4353 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4354 evalcond[1]=new_r02;
4355 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4356 {
4357 bgotonextstatement=false;
4358 {
4359 IkReal j5array[1], cj5array[1], sj5array[1];
4360 bool j5valid[1]={false};
4361 _nj5 = 1;
4362 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4363  continue;
4364 j5array[0]=IKatan2(new_r00, new_r01);
4365 sj5array[0]=IKsin(j5array[0]);
4366 cj5array[0]=IKcos(j5array[0]);
4367 if( j5array[0] > IKPI )
4368 {
4369  j5array[0]-=IK2PI;
4370 }
4371 else if( j5array[0] < -IKPI )
4372 { j5array[0]+=IK2PI;
4373 }
4374 j5valid[0] = true;
4375 for(int ij5 = 0; ij5 < 1; ++ij5)
4376 {
4377 if( !j5valid[ij5] )
4378 {
4379  continue;
4380 }
4381 _ij5[0] = ij5; _ij5[1] = -1;
4382 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4383 {
4384 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4385 {
4386  j5valid[iij5]=false; _ij5[1] = iij5; break;
4387 }
4388 }
4389 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4390 {
4391 IkReal evalcond[8];
4392 IkReal x316=IKsin(j5);
4393 IkReal x317=IKcos(j5);
4394 IkReal x318=((1.0)*sj4);
4395 IkReal x319=((1.0)*x317);
4396 evalcond[0]=(((sj4*x316))+new_r21);
4397 evalcond[1]=(x316+(((-1.0)*new_r00)));
4398 evalcond[2]=(x317+(((-1.0)*new_r01)));
4399 evalcond[3]=(new_r20+(((-1.0)*x317*x318)));
4400 evalcond[4]=(((cj4*x316))+(((-1.0)*new_r11)));
4401 evalcond[5]=((((-1.0)*cj4*x319))+(((-1.0)*new_r10)));
4402 evalcond[6]=((((-1.0)*new_r20*x318))+((cj4*new_r10))+x317);
4403 evalcond[7]=((((-1.0)*new_r21*x318))+((cj4*new_r11))+(((-1.0)*x316)));
4404 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 )
4405 {
4406 continue;
4407 }
4408 }
4409 
4410 {
4411 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4412 vinfos[0].jointtype = 1;
4413 vinfos[0].foffset = j0;
4414 vinfos[0].indices[0] = _ij0[0];
4415 vinfos[0].indices[1] = _ij0[1];
4416 vinfos[0].maxsolutions = _nj0;
4417 vinfos[1].jointtype = 1;
4418 vinfos[1].foffset = j1;
4419 vinfos[1].indices[0] = _ij1[0];
4420 vinfos[1].indices[1] = _ij1[1];
4421 vinfos[1].maxsolutions = _nj1;
4422 vinfos[2].jointtype = 1;
4423 vinfos[2].foffset = j2;
4424 vinfos[2].indices[0] = _ij2[0];
4425 vinfos[2].indices[1] = _ij2[1];
4426 vinfos[2].maxsolutions = _nj2;
4427 vinfos[3].jointtype = 1;
4428 vinfos[3].foffset = j3;
4429 vinfos[3].indices[0] = _ij3[0];
4430 vinfos[3].indices[1] = _ij3[1];
4431 vinfos[3].maxsolutions = _nj3;
4432 vinfos[4].jointtype = 1;
4433 vinfos[4].foffset = j4;
4434 vinfos[4].indices[0] = _ij4[0];
4435 vinfos[4].indices[1] = _ij4[1];
4436 vinfos[4].maxsolutions = _nj4;
4437 vinfos[5].jointtype = 1;
4438 vinfos[5].foffset = j5;
4439 vinfos[5].indices[0] = _ij5[0];
4440 vinfos[5].indices[1] = _ij5[1];
4441 vinfos[5].maxsolutions = _nj5;
4442 std::vector<int> vfree(0);
4443 solutions.AddSolution(vinfos,vfree);
4444 }
4445 }
4446 }
4447 
4448 }
4449 } while(0);
4450 if( bgotonextstatement )
4451 {
4452 bool bgotonextstatement = true;
4453 do
4454 {
4455 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4456 evalcond[1]=new_r12;
4457 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4458 {
4459 bgotonextstatement=false;
4460 {
4461 IkReal j5array[1], cj5array[1], sj5array[1];
4462 bool j5valid[1]={false};
4463 _nj5 = 1;
4464 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4465  continue;
4466 j5array[0]=IKatan2(new_r10, new_r11);
4467 sj5array[0]=IKsin(j5array[0]);
4468 cj5array[0]=IKcos(j5array[0]);
4469 if( j5array[0] > IKPI )
4470 {
4471  j5array[0]-=IK2PI;
4472 }
4473 else if( j5array[0] < -IKPI )
4474 { j5array[0]+=IK2PI;
4475 }
4476 j5valid[0] = true;
4477 for(int ij5 = 0; ij5 < 1; ++ij5)
4478 {
4479 if( !j5valid[ij5] )
4480 {
4481  continue;
4482 }
4483 _ij5[0] = ij5; _ij5[1] = -1;
4484 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4485 {
4486 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4487 {
4488  j5valid[iij5]=false; _ij5[1] = iij5; break;
4489 }
4490 }
4491 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4492 {
4493 IkReal evalcond[8];
4494 IkReal x320=IKcos(j5);
4495 IkReal x321=IKsin(j5);
4496 IkReal x322=((1.0)*cj4);
4497 IkReal x323=((1.0)*sj4);
4498 IkReal x324=((1.0)*x321);
4499 evalcond[0]=(new_r20+((new_r02*x320)));
4500 evalcond[1]=(x321+(((-1.0)*new_r10)));
4501 evalcond[2]=(x320+(((-1.0)*new_r11)));
4502 evalcond[3]=(((cj4*x321))+new_r01);
4503 evalcond[4]=((((-1.0)*new_r02*x324))+new_r21);
4504 evalcond[5]=((((-1.0)*x320*x322))+new_r00);
4505 evalcond[6]=(x320+(((-1.0)*new_r00*x322))+(((-1.0)*new_r20*x323)));
4506 evalcond[7]=((((-1.0)*x324))+(((-1.0)*new_r01*x322))+(((-1.0)*new_r21*x323)));
4507 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 )
4508 {
4509 continue;
4510 }
4511 }
4512 
4513 {
4514 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4515 vinfos[0].jointtype = 1;
4516 vinfos[0].foffset = j0;
4517 vinfos[0].indices[0] = _ij0[0];
4518 vinfos[0].indices[1] = _ij0[1];
4519 vinfos[0].maxsolutions = _nj0;
4520 vinfos[1].jointtype = 1;
4521 vinfos[1].foffset = j1;
4522 vinfos[1].indices[0] = _ij1[0];
4523 vinfos[1].indices[1] = _ij1[1];
4524 vinfos[1].maxsolutions = _nj1;
4525 vinfos[2].jointtype = 1;
4526 vinfos[2].foffset = j2;
4527 vinfos[2].indices[0] = _ij2[0];
4528 vinfos[2].indices[1] = _ij2[1];
4529 vinfos[2].maxsolutions = _nj2;
4530 vinfos[3].jointtype = 1;
4531 vinfos[3].foffset = j3;
4532 vinfos[3].indices[0] = _ij3[0];
4533 vinfos[3].indices[1] = _ij3[1];
4534 vinfos[3].maxsolutions = _nj3;
4535 vinfos[4].jointtype = 1;
4536 vinfos[4].foffset = j4;
4537 vinfos[4].indices[0] = _ij4[0];
4538 vinfos[4].indices[1] = _ij4[1];
4539 vinfos[4].maxsolutions = _nj4;
4540 vinfos[5].jointtype = 1;
4541 vinfos[5].foffset = j5;
4542 vinfos[5].indices[0] = _ij5[0];
4543 vinfos[5].indices[1] = _ij5[1];
4544 vinfos[5].maxsolutions = _nj5;
4545 std::vector<int> vfree(0);
4546 solutions.AddSolution(vinfos,vfree);
4547 }
4548 }
4549 }
4550 
4551 }
4552 } while(0);
4553 if( bgotonextstatement )
4554 {
4555 bool bgotonextstatement = true;
4556 do
4557 {
4558 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4559 evalcond[1]=new_r12;
4560 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4561 {
4562 bgotonextstatement=false;
4563 {
4564 IkReal j5array[1], cj5array[1], sj5array[1];
4565 bool j5valid[1]={false};
4566 _nj5 = 1;
4567 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
4568  continue;
4569 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4570 sj5array[0]=IKsin(j5array[0]);
4571 cj5array[0]=IKcos(j5array[0]);
4572 if( j5array[0] > IKPI )
4573 {
4574  j5array[0]-=IK2PI;
4575 }
4576 else if( j5array[0] < -IKPI )
4577 { j5array[0]+=IK2PI;
4578 }
4579 j5valid[0] = true;
4580 for(int ij5 = 0; ij5 < 1; ++ij5)
4581 {
4582 if( !j5valid[ij5] )
4583 {
4584  continue;
4585 }
4586 _ij5[0] = ij5; _ij5[1] = -1;
4587 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4588 {
4589 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4590 {
4591  j5valid[iij5]=false; _ij5[1] = iij5; break;
4592 }
4593 }
4594 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4595 {
4596 IkReal evalcond[8];
4597 IkReal x325=IKsin(j5);
4598 IkReal x326=IKcos(j5);
4599 IkReal x327=((1.0)*sj4);
4600 IkReal x328=((1.0)*x326);
4601 evalcond[0]=(x325+new_r10);
4602 evalcond[1]=(x326+new_r11);
4603 evalcond[2]=(new_r21+((new_r02*x325)));
4604 evalcond[3]=((((-1.0)*new_r02*x328))+new_r20);
4605 evalcond[4]=(((cj4*x325))+(((-1.0)*new_r01)));
4606 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x328)));
4607 evalcond[6]=(((cj4*new_r00))+x326+(((-1.0)*new_r20*x327)));
4608 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x325))+(((-1.0)*new_r21*x327)));
4609 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 )
4610 {
4611 continue;
4612 }
4613 }
4614 
4615 {
4616 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4617 vinfos[0].jointtype = 1;
4618 vinfos[0].foffset = j0;
4619 vinfos[0].indices[0] = _ij0[0];
4620 vinfos[0].indices[1] = _ij0[1];
4621 vinfos[0].maxsolutions = _nj0;
4622 vinfos[1].jointtype = 1;
4623 vinfos[1].foffset = j1;
4624 vinfos[1].indices[0] = _ij1[0];
4625 vinfos[1].indices[1] = _ij1[1];
4626 vinfos[1].maxsolutions = _nj1;
4627 vinfos[2].jointtype = 1;
4628 vinfos[2].foffset = j2;
4629 vinfos[2].indices[0] = _ij2[0];
4630 vinfos[2].indices[1] = _ij2[1];
4631 vinfos[2].maxsolutions = _nj2;
4632 vinfos[3].jointtype = 1;
4633 vinfos[3].foffset = j3;
4634 vinfos[3].indices[0] = _ij3[0];
4635 vinfos[3].indices[1] = _ij3[1];
4636 vinfos[3].maxsolutions = _nj3;
4637 vinfos[4].jointtype = 1;
4638 vinfos[4].foffset = j4;
4639 vinfos[4].indices[0] = _ij4[0];
4640 vinfos[4].indices[1] = _ij4[1];
4641 vinfos[4].maxsolutions = _nj4;
4642 vinfos[5].jointtype = 1;
4643 vinfos[5].foffset = j5;
4644 vinfos[5].indices[0] = _ij5[0];
4645 vinfos[5].indices[1] = _ij5[1];
4646 vinfos[5].maxsolutions = _nj5;
4647 std::vector<int> vfree(0);
4648 solutions.AddSolution(vinfos,vfree);
4649 }
4650 }
4651 }
4652 
4653 }
4654 } while(0);
4655 if( bgotonextstatement )
4656 {
4657 bool bgotonextstatement = true;
4658 do
4659 {
4660 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4661 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4662 {
4663 bgotonextstatement=false;
4664 {
4665 IkReal j5eval[1];
4666 new_r21=0;
4667 new_r20=0;
4668 new_r02=0;
4669 new_r12=0;
4670 j5eval[0]=1.0;
4671 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4672 {
4673 continue; // no branches [j5]
4674 
4675 } else
4676 {
4677 IkReal op[2+1], zeror[2];
4678 int numroots;
4679 op[0]=-1.0;
4680 op[1]=0;
4681 op[2]=1.0;
4682 polyroots2(op,zeror,numroots);
4683 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4684 int numsolutions = 0;
4685 for(int ij5 = 0; ij5 < numroots; ++ij5)
4686 {
4687 IkReal htj5 = zeror[ij5];
4688 tempj5array[0]=((2.0)*(atan(htj5)));
4689 for(int kj5 = 0; kj5 < 1; ++kj5)
4690 {
4691 j5array[numsolutions] = tempj5array[kj5];
4692 if( j5array[numsolutions] > IKPI )
4693 {
4694  j5array[numsolutions]-=IK2PI;
4695 }
4696 else if( j5array[numsolutions] < -IKPI )
4697 {
4698  j5array[numsolutions]+=IK2PI;
4699 }
4700 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4701 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4702 numsolutions++;
4703 }
4704 }
4705 bool j5valid[2]={true,true};
4706 _nj5 = 2;
4707 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4708  {
4709 if( !j5valid[ij5] )
4710 {
4711  continue;
4712 }
4713  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4714 htj5 = IKtan(j5/2);
4715 
4716 _ij5[0] = ij5; _ij5[1] = -1;
4717 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4718 {
4719 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4720 {
4721  j5valid[iij5]=false; _ij5[1] = iij5; break;
4722 }
4723 }
4724 {
4725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4726 vinfos[0].jointtype = 1;
4727 vinfos[0].foffset = j0;
4728 vinfos[0].indices[0] = _ij0[0];
4729 vinfos[0].indices[1] = _ij0[1];
4730 vinfos[0].maxsolutions = _nj0;
4731 vinfos[1].jointtype = 1;
4732 vinfos[1].foffset = j1;
4733 vinfos[1].indices[0] = _ij1[0];
4734 vinfos[1].indices[1] = _ij1[1];
4735 vinfos[1].maxsolutions = _nj1;
4736 vinfos[2].jointtype = 1;
4737 vinfos[2].foffset = j2;
4738 vinfos[2].indices[0] = _ij2[0];
4739 vinfos[2].indices[1] = _ij2[1];
4740 vinfos[2].maxsolutions = _nj2;
4741 vinfos[3].jointtype = 1;
4742 vinfos[3].foffset = j3;
4743 vinfos[3].indices[0] = _ij3[0];
4744 vinfos[3].indices[1] = _ij3[1];
4745 vinfos[3].maxsolutions = _nj3;
4746 vinfos[4].jointtype = 1;
4747 vinfos[4].foffset = j4;
4748 vinfos[4].indices[0] = _ij4[0];
4749 vinfos[4].indices[1] = _ij4[1];
4750 vinfos[4].maxsolutions = _nj4;
4751 vinfos[5].jointtype = 1;
4752 vinfos[5].foffset = j5;
4753 vinfos[5].indices[0] = _ij5[0];
4754 vinfos[5].indices[1] = _ij5[1];
4755 vinfos[5].maxsolutions = _nj5;
4756 std::vector<int> vfree(0);
4757 solutions.AddSolution(vinfos,vfree);
4758 }
4759  }
4760 
4761 }
4762 
4763 }
4764 
4765 }
4766 } while(0);
4767 if( bgotonextstatement )
4768 {
4769 bool bgotonextstatement = true;
4770 do
4771 {
4772 if( 1 )
4773 {
4774 bgotonextstatement=false;
4775 continue; // branch miss [j5]
4776 
4777 }
4778 } while(0);
4779 if( bgotonextstatement )
4780 {
4781 }
4782 }
4783 }
4784 }
4785 }
4786 }
4787 }
4788 }
4789 }
4790 
4791 } else
4792 {
4793 {
4794 IkReal j5array[1], cj5array[1], sj5array[1];
4795 bool j5valid[1]={false};
4796 _nj5 = 1;
4797 CheckValue<IkReal> x330=IKPowWithIntegerCheck(sj4,-1);
4798 if(!x330.valid){
4799 continue;
4800 }
4801 IkReal x329=x330.value;
4802 CheckValue<IkReal> x331=IKPowWithIntegerCheck(cj3,-1);
4803 if(!x331.valid){
4804 continue;
4805 }
4806 if( IKabs(((-1.0)*new_r21*x329)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x329))+IKsqr((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4807  continue;
4808 j5array[0]=IKatan2(((-1.0)*new_r21*x329), (x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4809 sj5array[0]=IKsin(j5array[0]);
4810 cj5array[0]=IKcos(j5array[0]);
4811 if( j5array[0] > IKPI )
4812 {
4813  j5array[0]-=IK2PI;
4814 }
4815 else if( j5array[0] < -IKPI )
4816 { j5array[0]+=IK2PI;
4817 }
4818 j5valid[0] = true;
4819 for(int ij5 = 0; ij5 < 1; ++ij5)
4820 {
4821 if( !j5valid[ij5] )
4822 {
4823  continue;
4824 }
4825 _ij5[0] = ij5; _ij5[1] = -1;
4826 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4827 {
4828 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4829 {
4830  j5valid[iij5]=false; _ij5[1] = iij5; break;
4831 }
4832 }
4833 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4834 {
4835 IkReal evalcond[12];
4836 IkReal x332=IKsin(j5);
4837 IkReal x333=IKcos(j5);
4838 IkReal x334=(cj3*new_r00);
4839 IkReal x335=(cj3*cj4);
4840 IkReal x336=((1.0)*cj3);
4841 IkReal x337=(new_r11*sj3);
4842 IkReal x338=((1.0)*cj4);
4843 IkReal x339=(new_r10*sj3);
4844 IkReal x340=((1.0)*sj4);
4845 IkReal x341=(sj3*x332);
4846 IkReal x342=((1.0)*x333);
4847 IkReal x343=(sj3*x333);
4848 evalcond[0]=(((sj4*x332))+new_r21);
4849 evalcond[1]=(new_r20+(((-1.0)*x333*x340)));
4850 evalcond[2]=((((-1.0)*new_r10*x336))+((new_r00*sj3))+x332);
4851 evalcond[3]=((((-1.0)*new_r11*x336))+((new_r01*sj3))+x333);
4852 evalcond[4]=(x337+((cj4*x332))+((cj3*new_r01)));
4853 evalcond[5]=(((x332*x335))+x343+new_r01);
4854 evalcond[6]=((((-1.0)*x333*x338))+x339+x334);
4855 evalcond[7]=((((-1.0)*x335*x342))+x341+new_r00);
4856 evalcond[8]=((((-1.0)*x333*x336))+((cj4*x341))+new_r11);
4857 evalcond[9]=((((-1.0)*x332*x336))+new_r10+(((-1.0)*x338*x343)));
4858 evalcond[10]=((((-1.0)*x338*x339))+x333+(((-1.0)*new_r20*x340))+(((-1.0)*x334*x338)));
4859 evalcond[11]=((((-1.0)*x337*x338))+(((-1.0)*new_r01*x335))+(((-1.0)*x332))+(((-1.0)*new_r21*x340)));
4860 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 )
4861 {
4862 continue;
4863 }
4864 }
4865 
4866 {
4867 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4868 vinfos[0].jointtype = 1;
4869 vinfos[0].foffset = j0;
4870 vinfos[0].indices[0] = _ij0[0];
4871 vinfos[0].indices[1] = _ij0[1];
4872 vinfos[0].maxsolutions = _nj0;
4873 vinfos[1].jointtype = 1;
4874 vinfos[1].foffset = j1;
4875 vinfos[1].indices[0] = _ij1[0];
4876 vinfos[1].indices[1] = _ij1[1];
4877 vinfos[1].maxsolutions = _nj1;
4878 vinfos[2].jointtype = 1;
4879 vinfos[2].foffset = j2;
4880 vinfos[2].indices[0] = _ij2[0];
4881 vinfos[2].indices[1] = _ij2[1];
4882 vinfos[2].maxsolutions = _nj2;
4883 vinfos[3].jointtype = 1;
4884 vinfos[3].foffset = j3;
4885 vinfos[3].indices[0] = _ij3[0];
4886 vinfos[3].indices[1] = _ij3[1];
4887 vinfos[3].maxsolutions = _nj3;
4888 vinfos[4].jointtype = 1;
4889 vinfos[4].foffset = j4;
4890 vinfos[4].indices[0] = _ij4[0];
4891 vinfos[4].indices[1] = _ij4[1];
4892 vinfos[4].maxsolutions = _nj4;
4893 vinfos[5].jointtype = 1;
4894 vinfos[5].foffset = j5;
4895 vinfos[5].indices[0] = _ij5[0];
4896 vinfos[5].indices[1] = _ij5[1];
4897 vinfos[5].maxsolutions = _nj5;
4898 std::vector<int> vfree(0);
4899 solutions.AddSolution(vinfos,vfree);
4900 }
4901 }
4902 }
4903 
4904 }
4905 
4906 }
4907 
4908 } else
4909 {
4910 {
4911 IkReal j5array[1], cj5array[1], sj5array[1];
4912 bool j5valid[1]={false};
4913 _nj5 = 1;
4914 CheckValue<IkReal> x345=IKPowWithIntegerCheck(sj4,-1);
4915 if(!x345.valid){
4916 continue;
4917 }
4918 IkReal x344=x345.value;
4919 CheckValue<IkReal> x346=IKPowWithIntegerCheck(sj3,-1);
4920 if(!x346.valid){
4921 continue;
4922 }
4923 if( IKabs(((-1.0)*new_r21*x344)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x344))+IKsqr((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4924  continue;
4925 j5array[0]=IKatan2(((-1.0)*new_r21*x344), (x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4926 sj5array[0]=IKsin(j5array[0]);
4927 cj5array[0]=IKcos(j5array[0]);
4928 if( j5array[0] > IKPI )
4929 {
4930  j5array[0]-=IK2PI;
4931 }
4932 else if( j5array[0] < -IKPI )
4933 { j5array[0]+=IK2PI;
4934 }
4935 j5valid[0] = true;
4936 for(int ij5 = 0; ij5 < 1; ++ij5)
4937 {
4938 if( !j5valid[ij5] )
4939 {
4940  continue;
4941 }
4942 _ij5[0] = ij5; _ij5[1] = -1;
4943 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4944 {
4945 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4946 {
4947  j5valid[iij5]=false; _ij5[1] = iij5; break;
4948 }
4949 }
4950 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4951 {
4952 IkReal evalcond[12];
4953 IkReal x347=IKsin(j5);
4954 IkReal x348=IKcos(j5);
4955 IkReal x349=(cj3*new_r00);
4956 IkReal x350=(cj3*cj4);
4957 IkReal x351=((1.0)*cj3);
4958 IkReal x352=(new_r11*sj3);
4959 IkReal x353=((1.0)*cj4);
4960 IkReal x354=(new_r10*sj3);
4961 IkReal x355=((1.0)*sj4);
4962 IkReal x356=(sj3*x347);
4963 IkReal x357=((1.0)*x348);
4964 IkReal x358=(sj3*x348);
4965 evalcond[0]=(((sj4*x347))+new_r21);
4966 evalcond[1]=((((-1.0)*x348*x355))+new_r20);
4967 evalcond[2]=(((new_r00*sj3))+x347+(((-1.0)*new_r10*x351)));
4968 evalcond[3]=(((new_r01*sj3))+x348+(((-1.0)*new_r11*x351)));
4969 evalcond[4]=(x352+((cj4*x347))+((cj3*new_r01)));
4970 evalcond[5]=(x358+((x347*x350))+new_r01);
4971 evalcond[6]=((((-1.0)*x348*x353))+x354+x349);
4972 evalcond[7]=(x356+new_r00+(((-1.0)*x350*x357)));
4973 evalcond[8]=((((-1.0)*x348*x351))+((cj4*x356))+new_r11);
4974 evalcond[9]=((((-1.0)*x347*x351))+(((-1.0)*x353*x358))+new_r10);
4975 evalcond[10]=((((-1.0)*x349*x353))+x348+(((-1.0)*x353*x354))+(((-1.0)*new_r20*x355)));
4976 evalcond[11]=((((-1.0)*new_r01*x350))+(((-1.0)*new_r21*x355))+(((-1.0)*x352*x353))+(((-1.0)*x347)));
4977 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 )
4978 {
4979 continue;
4980 }
4981 }
4982 
4983 {
4984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4985 vinfos[0].jointtype = 1;
4986 vinfos[0].foffset = j0;
4987 vinfos[0].indices[0] = _ij0[0];
4988 vinfos[0].indices[1] = _ij0[1];
4989 vinfos[0].maxsolutions = _nj0;
4990 vinfos[1].jointtype = 1;
4991 vinfos[1].foffset = j1;
4992 vinfos[1].indices[0] = _ij1[0];
4993 vinfos[1].indices[1] = _ij1[1];
4994 vinfos[1].maxsolutions = _nj1;
4995 vinfos[2].jointtype = 1;
4996 vinfos[2].foffset = j2;
4997 vinfos[2].indices[0] = _ij2[0];
4998 vinfos[2].indices[1] = _ij2[1];
4999 vinfos[2].maxsolutions = _nj2;
5000 vinfos[3].jointtype = 1;
5001 vinfos[3].foffset = j3;
5002 vinfos[3].indices[0] = _ij3[0];
5003 vinfos[3].indices[1] = _ij3[1];
5004 vinfos[3].maxsolutions = _nj3;
5005 vinfos[4].jointtype = 1;
5006 vinfos[4].foffset = j4;
5007 vinfos[4].indices[0] = _ij4[0];
5008 vinfos[4].indices[1] = _ij4[1];
5009 vinfos[4].maxsolutions = _nj4;
5010 vinfos[5].jointtype = 1;
5011 vinfos[5].foffset = j5;
5012 vinfos[5].indices[0] = _ij5[0];
5013 vinfos[5].indices[1] = _ij5[1];
5014 vinfos[5].maxsolutions = _nj5;
5015 std::vector<int> vfree(0);
5016 solutions.AddSolution(vinfos,vfree);
5017 }
5018 }
5019 }
5020 
5021 }
5022 
5023 }
5024 
5025 } else
5026 {
5027 {
5028 IkReal j5array[1], cj5array[1], sj5array[1];
5029 bool j5valid[1]={false};
5030 _nj5 = 1;
5031 CheckValue<IkReal> x359 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5032 if(!x359.valid){
5033 continue;
5034 }
5035 CheckValue<IkReal> x360=IKPowWithIntegerCheck(IKsign(sj4),-1);
5036 if(!x360.valid){
5037 continue;
5038 }
5039 j5array[0]=((-1.5707963267949)+(x359.value)+(((1.5707963267949)*(x360.value))));
5040 sj5array[0]=IKsin(j5array[0]);
5041 cj5array[0]=IKcos(j5array[0]);
5042 if( j5array[0] > IKPI )
5043 {
5044  j5array[0]-=IK2PI;
5045 }
5046 else if( j5array[0] < -IKPI )
5047 { j5array[0]+=IK2PI;
5048 }
5049 j5valid[0] = true;
5050 for(int ij5 = 0; ij5 < 1; ++ij5)
5051 {
5052 if( !j5valid[ij5] )
5053 {
5054  continue;
5055 }
5056 _ij5[0] = ij5; _ij5[1] = -1;
5057 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5058 {
5059 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5060 {
5061  j5valid[iij5]=false; _ij5[1] = iij5; break;
5062 }
5063 }
5064 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5065 {
5066 IkReal evalcond[12];
5067 IkReal x361=IKsin(j5);
5068 IkReal x362=IKcos(j5);
5069 IkReal x363=(cj3*new_r00);
5070 IkReal x364=(cj3*cj4);
5071 IkReal x365=((1.0)*cj3);
5072 IkReal x366=(new_r11*sj3);
5073 IkReal x367=((1.0)*cj4);
5074 IkReal x368=(new_r10*sj3);
5075 IkReal x369=((1.0)*sj4);
5076 IkReal x370=(sj3*x361);
5077 IkReal x371=((1.0)*x362);
5078 IkReal x372=(sj3*x362);
5079 evalcond[0]=(((sj4*x361))+new_r21);
5080 evalcond[1]=((((-1.0)*x362*x369))+new_r20);
5081 evalcond[2]=(((new_r00*sj3))+x361+(((-1.0)*new_r10*x365)));
5082 evalcond[3]=(((new_r01*sj3))+x362+(((-1.0)*new_r11*x365)));
5083 evalcond[4]=(((cj4*x361))+x366+((cj3*new_r01)));
5084 evalcond[5]=(((x361*x364))+x372+new_r01);
5085 evalcond[6]=((((-1.0)*x362*x367))+x368+x363);
5086 evalcond[7]=(x370+new_r00+(((-1.0)*x364*x371)));
5087 evalcond[8]=((((-1.0)*x362*x365))+((cj4*x370))+new_r11);
5088 evalcond[9]=((((-1.0)*x361*x365))+(((-1.0)*x367*x372))+new_r10);
5089 evalcond[10]=((((-1.0)*x363*x367))+(((-1.0)*new_r20*x369))+(((-1.0)*x367*x368))+x362);
5090 evalcond[11]=((((-1.0)*x361))+(((-1.0)*x366*x367))+(((-1.0)*new_r01*x364))+(((-1.0)*new_r21*x369)));
5091 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 )
5092 {
5093 continue;
5094 }
5095 }
5096 
5097 {
5098 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5099 vinfos[0].jointtype = 1;
5100 vinfos[0].foffset = j0;
5101 vinfos[0].indices[0] = _ij0[0];
5102 vinfos[0].indices[1] = _ij0[1];
5103 vinfos[0].maxsolutions = _nj0;
5104 vinfos[1].jointtype = 1;
5105 vinfos[1].foffset = j1;
5106 vinfos[1].indices[0] = _ij1[0];
5107 vinfos[1].indices[1] = _ij1[1];
5108 vinfos[1].maxsolutions = _nj1;
5109 vinfos[2].jointtype = 1;
5110 vinfos[2].foffset = j2;
5111 vinfos[2].indices[0] = _ij2[0];
5112 vinfos[2].indices[1] = _ij2[1];
5113 vinfos[2].maxsolutions = _nj2;
5114 vinfos[3].jointtype = 1;
5115 vinfos[3].foffset = j3;
5116 vinfos[3].indices[0] = _ij3[0];
5117 vinfos[3].indices[1] = _ij3[1];
5118 vinfos[3].maxsolutions = _nj3;
5119 vinfos[4].jointtype = 1;
5120 vinfos[4].foffset = j4;
5121 vinfos[4].indices[0] = _ij4[0];
5122 vinfos[4].indices[1] = _ij4[1];
5123 vinfos[4].maxsolutions = _nj4;
5124 vinfos[5].jointtype = 1;
5125 vinfos[5].foffset = j5;
5126 vinfos[5].indices[0] = _ij5[0];
5127 vinfos[5].indices[1] = _ij5[1];
5128 vinfos[5].maxsolutions = _nj5;
5129 std::vector<int> vfree(0);
5130 solutions.AddSolution(vinfos,vfree);
5131 }
5132 }
5133 }
5134 
5135 }
5136 
5137 }
5138 }
5139 }
5140 
5141 }
5142 
5143 }
5144 
5145 } else
5146 {
5147 {
5148 IkReal j5array[1], cj5array[1], sj5array[1];
5149 bool j5valid[1]={false};
5150 _nj5 = 1;
5151 CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5152 if(!x373.valid){
5153 continue;
5154 }
5155 CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5156 if(!x374.valid){
5157 continue;
5158 }
5159 j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5160 sj5array[0]=IKsin(j5array[0]);
5161 cj5array[0]=IKcos(j5array[0]);
5162 if( j5array[0] > IKPI )
5163 {
5164  j5array[0]-=IK2PI;
5165 }
5166 else if( j5array[0] < -IKPI )
5167 { j5array[0]+=IK2PI;
5168 }
5169 j5valid[0] = true;
5170 for(int ij5 = 0; ij5 < 1; ++ij5)
5171 {
5172 if( !j5valid[ij5] )
5173 {
5174  continue;
5175 }
5176 _ij5[0] = ij5; _ij5[1] = -1;
5177 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5178 {
5179 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5180 {
5181  j5valid[iij5]=false; _ij5[1] = iij5; break;
5182 }
5183 }
5184 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5185 {
5186 IkReal evalcond[2];
5187 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5188 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5189 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5190 {
5191 continue;
5192 }
5193 }
5194 
5195 {
5196 IkReal j3eval[3];
5197 j3eval[0]=sj4;
5198 j3eval[1]=IKsign(sj4);
5199 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5200 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5201 {
5202 {
5203 IkReal j3eval[2];
5204 j3eval[0]=new_r12;
5205 j3eval[1]=sj4;
5206 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5207 {
5208 {
5209 IkReal evalcond[5];
5210 bool bgotonextstatement = true;
5211 do
5212 {
5213 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5214 evalcond[1]=new_r02;
5215 evalcond[2]=new_r12;
5216 evalcond[3]=new_r21;
5217 evalcond[4]=new_r20;
5218 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 )
5219 {
5220 bgotonextstatement=false;
5221 {
5222 IkReal j3eval[3];
5223 sj4=0;
5224 cj4=1.0;
5225 j4=0;
5226 IkReal x375=((1.0)*new_r11);
5227 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5228 j3eval[0]=x376;
5229 j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5230 j3eval[2]=IKsign(x376);
5231 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5232 {
5233 {
5234 IkReal j3eval[3];
5235 sj4=0;
5236 cj4=1.0;
5237 j4=0;
5238 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5239 j3eval[0]=x377;
5240 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5241 j3eval[2]=IKsign(x377);
5242 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5243 {
5244 {
5245 IkReal j3eval[3];
5246 sj4=0;
5247 cj4=1.0;
5248 j4=0;
5249 IkReal x378=((1.0)*new_r11);
5250 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5251 j3eval[0]=x379;
5252 j3eval[1]=IKsign(x379);
5253 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5254 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5255 {
5256 {
5257 IkReal evalcond[1];
5258 bool bgotonextstatement = true;
5259 do
5260 {
5261 IkReal x380=((-1.0)*new_r01);
5262 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5263 if(IKabs(x382)==0){
5264 continue;
5265 }
5266 IkReal x381=pow(x382,-0.5);
5267 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5268 if(!x383.valid){
5269 continue;
5270 }
5271 IkReal gconst0=((-1.0)*(x383.value));
5272 IkReal gconst1=(new_r11*x381);
5273 IkReal gconst2=(x380*x381);
5274 CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5275 if(!x384.valid){
5276 continue;
5277 }
5278 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5279 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5280 {
5281 bgotonextstatement=false;
5282 {
5283 IkReal j3eval[3];
5284 IkReal x385=((-1.0)*new_r01);
5285 CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5286 if(!x388.valid){
5287 continue;
5288 }
5289 IkReal x386=((-1.0)*(x388.value));
5290 IkReal x387=x381;
5291 sj4=0;
5292 cj4=1.0;
5293 j4=0;
5294 sj5=gconst1;
5295 cj5=gconst2;
5296 j5=x386;
5297 IkReal gconst0=x386;
5298 IkReal gconst1=(new_r11*x387);
5299 IkReal gconst2=(x385*x387);
5300 IkReal x389=new_r11*new_r11;
5301 IkReal x390=(new_r10*new_r11);
5302 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5303 IkReal x392=x381;
5304 IkReal x393=(new_r11*x392);
5305 j3eval[0]=x391;
5306 j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5307 j3eval[2]=IKsign(x391);
5308 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5309 {
5310 {
5311 IkReal j3eval[1];
5312 IkReal x394=((-1.0)*new_r01);
5313 CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5314 if(!x397.valid){
5315 continue;
5316 }
5317 IkReal x395=((-1.0)*(x397.value));
5318 IkReal x396=x381;
5319 sj4=0;
5320 cj4=1.0;
5321 j4=0;
5322 sj5=gconst1;
5323 cj5=gconst2;
5324 j5=x395;
5325 IkReal gconst0=x395;
5326 IkReal gconst1=(new_r11*x396);
5327 IkReal gconst2=(x394*x396);
5328 IkReal x398=new_r11*new_r11;
5329 CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5330 if(!x401.valid){
5331 continue;
5332 }
5333 IkReal x399=x401.value;
5334 IkReal x400=(x398*x399);
5335 j3eval[0]=((IKabs((((new_r01*new_r10))+x400)))+(IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5336 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5337 {
5338 {
5339 IkReal j3eval[1];
5340 IkReal x402=((-1.0)*new_r01);
5341 CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5342 if(!x405.valid){
5343 continue;
5344 }
5345 IkReal x403=((-1.0)*(x405.value));
5346 IkReal x404=x381;
5347 sj4=0;
5348 cj4=1.0;
5349 j4=0;
5350 sj5=gconst1;
5351 cj5=gconst2;
5352 j5=x403;
5353 IkReal gconst0=x403;
5354 IkReal gconst1=(new_r11*x404);
5355 IkReal gconst2=(x402*x404);
5356 IkReal x406=new_r01*new_r01;
5357 IkReal x407=new_r11*new_r11;
5358 CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5359 if(!x414.valid){
5360 continue;
5361 }
5362 IkReal x408=x414.value;
5363 IkReal x409=(x407*x408);
5364 CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5365 if(!x415.valid){
5366 continue;
5367 }
5368 IkReal x410=x415.value;
5369 IkReal x411=((1.0)*x410);
5370 IkReal x412=(new_r11*x411);
5371 IkReal x413=(new_r01*x411);
5372 j3eval[0]=((IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5373 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5374 {
5375 {
5376 IkReal evalcond[3];
5377 bool bgotonextstatement = true;
5378 do
5379 {
5380 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5381 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5382 {
5383 bgotonextstatement=false;
5384 {
5385 IkReal j3array[2], cj3array[2], sj3array[2];
5386 bool j3valid[2]={false};
5387 _nj3 = 2;
5388 CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst2,-1);
5389 if(!x416.valid){
5390 continue;
5391 }
5392 sj3array[0]=(new_r10*(x416.value));
5393 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5394 {
5395  j3valid[0] = j3valid[1] = true;
5396  j3array[0] = IKasin(sj3array[0]);
5397  cj3array[0] = IKcos(j3array[0]);
5398  sj3array[1] = sj3array[0];
5399  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5400  cj3array[1] = -cj3array[0];
5401 }
5402 else if( isnan(sj3array[0]) )
5403 {
5404  // probably any value will work
5405  j3valid[0] = true;
5406  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5407 }
5408 for(int ij3 = 0; ij3 < 2; ++ij3)
5409 {
5410 if( !j3valid[ij3] )
5411 {
5412  continue;
5413 }
5414 _ij3[0] = ij3; _ij3[1] = -1;
5415 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5416 {
5417 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5418 {
5419  j3valid[iij3]=false; _ij3[1] = iij3; break;
5420 }
5421 }
5422 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5423 {
5424 IkReal evalcond[6];
5425 IkReal x417=IKcos(j3);
5426 IkReal x418=IKsin(j3);
5427 IkReal x419=((-1.0)*x417);
5428 evalcond[0]=(new_r01*x417);
5429 evalcond[1]=(new_r10*x419);
5430 evalcond[2]=(gconst2*x419);
5431 evalcond[3]=(gconst2+((new_r01*x418)));
5432 evalcond[4]=(((gconst2*x418))+new_r01);
5433 evalcond[5]=((((-1.0)*gconst2))+((new_r10*x418)));
5434 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 )
5435 {
5436 continue;
5437 }
5438 }
5439 
5440 {
5441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5442 vinfos[0].jointtype = 1;
5443 vinfos[0].foffset = j0;
5444 vinfos[0].indices[0] = _ij0[0];
5445 vinfos[0].indices[1] = _ij0[1];
5446 vinfos[0].maxsolutions = _nj0;
5447 vinfos[1].jointtype = 1;
5448 vinfos[1].foffset = j1;
5449 vinfos[1].indices[0] = _ij1[0];
5450 vinfos[1].indices[1] = _ij1[1];
5451 vinfos[1].maxsolutions = _nj1;
5452 vinfos[2].jointtype = 1;
5453 vinfos[2].foffset = j2;
5454 vinfos[2].indices[0] = _ij2[0];
5455 vinfos[2].indices[1] = _ij2[1];
5456 vinfos[2].maxsolutions = _nj2;
5457 vinfos[3].jointtype = 1;
5458 vinfos[3].foffset = j3;
5459 vinfos[3].indices[0] = _ij3[0];
5460 vinfos[3].indices[1] = _ij3[1];
5461 vinfos[3].maxsolutions = _nj3;
5462 vinfos[4].jointtype = 1;
5463 vinfos[4].foffset = j4;
5464 vinfos[4].indices[0] = _ij4[0];
5465 vinfos[4].indices[1] = _ij4[1];
5466 vinfos[4].maxsolutions = _nj4;
5467 vinfos[5].jointtype = 1;
5468 vinfos[5].foffset = j5;
5469 vinfos[5].indices[0] = _ij5[0];
5470 vinfos[5].indices[1] = _ij5[1];
5471 vinfos[5].maxsolutions = _nj5;
5472 std::vector<int> vfree(0);
5473 solutions.AddSolution(vinfos,vfree);
5474 }
5475 }
5476 }
5477 
5478 }
5479 } while(0);
5480 if( bgotonextstatement )
5481 {
5482 bool bgotonextstatement = true;
5483 do
5484 {
5485 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5486 evalcond[1]=gconst1;
5487 evalcond[2]=gconst2;
5488 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
5489 {
5490 bgotonextstatement=false;
5491 {
5492 IkReal j3eval[3];
5493 IkReal x420=((-1.0)*new_r01);
5494 CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5495 if(!x422.valid){
5496 continue;
5497 }
5498 IkReal x421=((-1.0)*(x422.value));
5499 sj4=0;
5500 cj4=1.0;
5501 j4=0;
5502 sj5=gconst1;
5503 cj5=gconst2;
5504 j5=x421;
5505 new_r00=0;
5506 new_r10=0;
5507 new_r21=0;
5508 new_r22=0;
5509 IkReal gconst0=x421;
5510 IkReal gconst1=new_r11;
5511 IkReal gconst2=x420;
5512 j3eval[0]=-1.0;
5513 j3eval[1]=-1.0;
5514 j3eval[2]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5515 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5516 {
5517 {
5518 IkReal j3eval[3];
5519 IkReal x423=((-1.0)*new_r01);
5520 CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5521 if(!x425.valid){
5522 continue;
5523 }
5524 IkReal x424=((-1.0)*(x425.value));
5525 sj4=0;
5526 cj4=1.0;
5527 j4=0;
5528 sj5=gconst1;
5529 cj5=gconst2;
5530 j5=x424;
5531 new_r00=0;
5532 new_r10=0;
5533 new_r21=0;
5534 new_r22=0;
5535 IkReal gconst0=x424;
5536 IkReal gconst1=new_r11;
5537 IkReal gconst2=x423;
5538 j3eval[0]=-1.0;
5539 j3eval[1]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5540 j3eval[2]=-1.0;
5541 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5542 {
5543 {
5544 IkReal j3eval[3];
5545 IkReal x426=((-1.0)*new_r01);
5546 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5547 if(!x428.valid){
5548 continue;
5549 }
5550 IkReal x427=((-1.0)*(x428.value));
5551 sj4=0;
5552 cj4=1.0;
5553 j4=0;
5554 sj5=gconst1;
5555 cj5=gconst2;
5556 j5=x427;
5557 new_r00=0;
5558 new_r10=0;
5559 new_r21=0;
5560 new_r22=0;
5561 IkReal gconst0=x427;
5562 IkReal gconst1=new_r11;
5563 IkReal gconst2=x426;
5564 j3eval[0]=1.0;
5565 j3eval[1]=1.0;
5566 j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5567 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5568 {
5569 continue; // 3 cases reached
5570 
5571 } else
5572 {
5573 {
5574 IkReal j3array[1], cj3array[1], sj3array[1];
5575 bool j3valid[1]={false};
5576 _nj3 = 1;
5577 IkReal x429=((1.0)*new_r01);
5578 CheckValue<IkReal> x430 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x429))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
5579 if(!x430.valid){
5580 continue;
5581 }
5582 CheckValue<IkReal> x431=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x429)))),-1);
5583 if(!x431.valid){
5584 continue;
5585 }
5586 j3array[0]=((-1.5707963267949)+(x430.value)+(((1.5707963267949)*(x431.value))));
5587 sj3array[0]=IKsin(j3array[0]);
5588 cj3array[0]=IKcos(j3array[0]);
5589 if( j3array[0] > IKPI )
5590 {
5591  j3array[0]-=IK2PI;
5592 }
5593 else if( j3array[0] < -IKPI )
5594 { j3array[0]+=IK2PI;
5595 }
5596 j3valid[0] = true;
5597 for(int ij3 = 0; ij3 < 1; ++ij3)
5598 {
5599 if( !j3valid[ij3] )
5600 {
5601  continue;
5602 }
5603 _ij3[0] = ij3; _ij3[1] = -1;
5604 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5605 {
5606 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5607 {
5608  j3valid[iij3]=false; _ij3[1] = iij3; break;
5609 }
5610 }
5611 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5612 {
5613 IkReal evalcond[6];
5614 IkReal x432=IKcos(j3);
5615 IkReal x433=IKsin(j3);
5616 IkReal x434=(gconst1*x433);
5617 IkReal x435=(gconst2*x433);
5618 IkReal x436=((1.0)*x432);
5619 IkReal x437=(gconst2*x436);
5620 evalcond[0]=(((new_r01*x432))+gconst1+((new_r11*x433)));
5621 evalcond[1]=(((gconst1*x432))+x435+new_r01);
5622 evalcond[2]=((((-1.0)*x437))+x434);
5623 evalcond[3]=(((new_r01*x433))+gconst2+(((-1.0)*new_r11*x436)));
5624 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5625 evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst1*x436)));
5626 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 )
5627 {
5628 continue;
5629 }
5630 }
5631 
5632 {
5633 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5634 vinfos[0].jointtype = 1;
5635 vinfos[0].foffset = j0;
5636 vinfos[0].indices[0] = _ij0[0];
5637 vinfos[0].indices[1] = _ij0[1];
5638 vinfos[0].maxsolutions = _nj0;
5639 vinfos[1].jointtype = 1;
5640 vinfos[1].foffset = j1;
5641 vinfos[1].indices[0] = _ij1[0];
5642 vinfos[1].indices[1] = _ij1[1];
5643 vinfos[1].maxsolutions = _nj1;
5644 vinfos[2].jointtype = 1;
5645 vinfos[2].foffset = j2;
5646 vinfos[2].indices[0] = _ij2[0];
5647 vinfos[2].indices[1] = _ij2[1];
5648 vinfos[2].maxsolutions = _nj2;
5649 vinfos[3].jointtype = 1;
5650 vinfos[3].foffset = j3;
5651 vinfos[3].indices[0] = _ij3[0];
5652 vinfos[3].indices[1] = _ij3[1];
5653 vinfos[3].maxsolutions = _nj3;
5654 vinfos[4].jointtype = 1;
5655 vinfos[4].foffset = j4;
5656 vinfos[4].indices[0] = _ij4[0];
5657 vinfos[4].indices[1] = _ij4[1];
5658 vinfos[4].maxsolutions = _nj4;
5659 vinfos[5].jointtype = 1;
5660 vinfos[5].foffset = j5;
5661 vinfos[5].indices[0] = _ij5[0];
5662 vinfos[5].indices[1] = _ij5[1];
5663 vinfos[5].maxsolutions = _nj5;
5664 std::vector<int> vfree(0);
5665 solutions.AddSolution(vinfos,vfree);
5666 }
5667 }
5668 }
5669 
5670 }
5671 
5672 }
5673 
5674 } else
5675 {
5676 {
5677 IkReal j3array[1], cj3array[1], sj3array[1];
5678 bool j3valid[1]={false};
5679 _nj3 = 1;
5680 CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst2*gconst2)))+(((-1.0)*(gconst1*gconst1))))),-1);
5681 if(!x438.valid){
5682 continue;
5683 }
5684 CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst2*new_r01)),IkReal((gconst1*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5685 if(!x439.valid){
5686 continue;
5687 }
5688 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5689 sj3array[0]=IKsin(j3array[0]);
5690 cj3array[0]=IKcos(j3array[0]);
5691 if( j3array[0] > IKPI )
5692 {
5693  j3array[0]-=IK2PI;
5694 }
5695 else if( j3array[0] < -IKPI )
5696 { j3array[0]+=IK2PI;
5697 }
5698 j3valid[0] = true;
5699 for(int ij3 = 0; ij3 < 1; ++ij3)
5700 {
5701 if( !j3valid[ij3] )
5702 {
5703  continue;
5704 }
5705 _ij3[0] = ij3; _ij3[1] = -1;
5706 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5707 {
5708 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5709 {
5710  j3valid[iij3]=false; _ij3[1] = iij3; break;
5711 }
5712 }
5713 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5714 {
5715 IkReal evalcond[6];
5716 IkReal x440=IKcos(j3);
5717 IkReal x441=IKsin(j3);
5718 IkReal x442=(gconst1*x441);
5719 IkReal x443=(gconst2*x441);
5720 IkReal x444=((1.0)*x440);
5721 IkReal x445=(gconst2*x444);
5722 evalcond[0]=(((new_r01*x440))+gconst1+((new_r11*x441)));
5723 evalcond[1]=(((gconst1*x440))+x443+new_r01);
5724 evalcond[2]=((((-1.0)*x445))+x442);
5725 evalcond[3]=(((new_r01*x441))+gconst2+(((-1.0)*new_r11*x444)));
5726 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5727 evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst1*x444)));
5728 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 )
5729 {
5730 continue;
5731 }
5732 }
5733 
5734 {
5735 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5736 vinfos[0].jointtype = 1;
5737 vinfos[0].foffset = j0;
5738 vinfos[0].indices[0] = _ij0[0];
5739 vinfos[0].indices[1] = _ij0[1];
5740 vinfos[0].maxsolutions = _nj0;
5741 vinfos[1].jointtype = 1;
5742 vinfos[1].foffset = j1;
5743 vinfos[1].indices[0] = _ij1[0];
5744 vinfos[1].indices[1] = _ij1[1];
5745 vinfos[1].maxsolutions = _nj1;
5746 vinfos[2].jointtype = 1;
5747 vinfos[2].foffset = j2;
5748 vinfos[2].indices[0] = _ij2[0];
5749 vinfos[2].indices[1] = _ij2[1];
5750 vinfos[2].maxsolutions = _nj2;
5751 vinfos[3].jointtype = 1;
5752 vinfos[3].foffset = j3;
5753 vinfos[3].indices[0] = _ij3[0];
5754 vinfos[3].indices[1] = _ij3[1];
5755 vinfos[3].maxsolutions = _nj3;
5756 vinfos[4].jointtype = 1;
5757 vinfos[4].foffset = j4;
5758 vinfos[4].indices[0] = _ij4[0];
5759 vinfos[4].indices[1] = _ij4[1];
5760 vinfos[4].maxsolutions = _nj4;
5761 vinfos[5].jointtype = 1;
5762 vinfos[5].foffset = j5;
5763 vinfos[5].indices[0] = _ij5[0];
5764 vinfos[5].indices[1] = _ij5[1];
5765 vinfos[5].maxsolutions = _nj5;
5766 std::vector<int> vfree(0);
5767 solutions.AddSolution(vinfos,vfree);
5768 }
5769 }
5770 }
5771 
5772 }
5773 
5774 }
5775 
5776 } else
5777 {
5778 {
5779 IkReal j3array[1], cj3array[1], sj3array[1];
5780 bool j3valid[1]={false};
5781 _nj3 = 1;
5782 CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(gconst1*gconst1),IkReal(((-1.0)*gconst1*gconst2)),IKFAST_ATAN2_MAGTHRESH);
5783 if(!x446.valid){
5784 continue;
5785 }
5786 CheckValue<IkReal> x447=IKPowWithIntegerCheck(IKsign((((gconst2*new_r01))+(((-1.0)*gconst1*new_r11)))),-1);
5787 if(!x447.valid){
5788 continue;
5789 }
5790 j3array[0]=((-1.5707963267949)+(x446.value)+(((1.5707963267949)*(x447.value))));
5791 sj3array[0]=IKsin(j3array[0]);
5792 cj3array[0]=IKcos(j3array[0]);
5793 if( j3array[0] > IKPI )
5794 {
5795  j3array[0]-=IK2PI;
5796 }
5797 else if( j3array[0] < -IKPI )
5798 { j3array[0]+=IK2PI;
5799 }
5800 j3valid[0] = true;
5801 for(int ij3 = 0; ij3 < 1; ++ij3)
5802 {
5803 if( !j3valid[ij3] )
5804 {
5805  continue;
5806 }
5807 _ij3[0] = ij3; _ij3[1] = -1;
5808 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5809 {
5810 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5811 {
5812  j3valid[iij3]=false; _ij3[1] = iij3; break;
5813 }
5814 }
5815 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5816 {
5817 IkReal evalcond[6];
5818 IkReal x448=IKcos(j3);
5819 IkReal x449=IKsin(j3);
5820 IkReal x450=(gconst1*x449);
5821 IkReal x451=(gconst2*x449);
5822 IkReal x452=((1.0)*x448);
5823 IkReal x453=(gconst2*x452);
5824 evalcond[0]=(((new_r01*x448))+gconst1+((new_r11*x449)));
5825 evalcond[1]=(((gconst1*x448))+x451+new_r01);
5826 evalcond[2]=((((-1.0)*x453))+x450);
5827 evalcond[3]=(((new_r01*x449))+gconst2+(((-1.0)*new_r11*x452)));
5828 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5829 evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst1*x452)));
5830 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 )
5831 {
5832 continue;
5833 }
5834 }
5835 
5836 {
5837 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5838 vinfos[0].jointtype = 1;
5839 vinfos[0].foffset = j0;
5840 vinfos[0].indices[0] = _ij0[0];
5841 vinfos[0].indices[1] = _ij0[1];
5842 vinfos[0].maxsolutions = _nj0;
5843 vinfos[1].jointtype = 1;
5844 vinfos[1].foffset = j1;
5845 vinfos[1].indices[0] = _ij1[0];
5846 vinfos[1].indices[1] = _ij1[1];
5847 vinfos[1].maxsolutions = _nj1;
5848 vinfos[2].jointtype = 1;
5849 vinfos[2].foffset = j2;
5850 vinfos[2].indices[0] = _ij2[0];
5851 vinfos[2].indices[1] = _ij2[1];
5852 vinfos[2].maxsolutions = _nj2;
5853 vinfos[3].jointtype = 1;
5854 vinfos[3].foffset = j3;
5855 vinfos[3].indices[0] = _ij3[0];
5856 vinfos[3].indices[1] = _ij3[1];
5857 vinfos[3].maxsolutions = _nj3;
5858 vinfos[4].jointtype = 1;
5859 vinfos[4].foffset = j4;
5860 vinfos[4].indices[0] = _ij4[0];
5861 vinfos[4].indices[1] = _ij4[1];
5862 vinfos[4].maxsolutions = _nj4;
5863 vinfos[5].jointtype = 1;
5864 vinfos[5].foffset = j5;
5865 vinfos[5].indices[0] = _ij5[0];
5866 vinfos[5].indices[1] = _ij5[1];
5867 vinfos[5].maxsolutions = _nj5;
5868 std::vector<int> vfree(0);
5869 solutions.AddSolution(vinfos,vfree);
5870 }
5871 }
5872 }
5873 
5874 }
5875 
5876 }
5877 
5878 }
5879 } while(0);
5880 if( bgotonextstatement )
5881 {
5882 bool bgotonextstatement = true;
5883 do
5884 {
5885 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5886 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5887 {
5888 bgotonextstatement=false;
5889 {
5890 IkReal j3eval[1];
5891 CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5892 if(!x455.valid){
5893 continue;
5894 }
5895 IkReal x454=((-1.0)*(x455.value));
5896 sj4=0;
5897 cj4=1.0;
5898 j4=0;
5899 sj5=gconst1;
5900 cj5=gconst2;
5901 j5=x454;
5902 new_r01=0;
5903 new_r10=0;
5904 IkReal gconst0=x454;
5905 IkReal x456 = new_r11*new_r11;
5906 if(IKabs(x456)==0){
5907 continue;
5908 }
5909 IkReal gconst1=(new_r11*(pow(x456,-0.5)));
5910 IkReal gconst2=0;
5911 j3eval[0]=new_r00;
5912 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5913 {
5914 {
5915 IkReal j3eval[1];
5916 CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5917 if(!x458.valid){
5918 continue;
5919 }
5920 IkReal x457=((-1.0)*(x458.value));
5921 sj4=0;
5922 cj4=1.0;
5923 j4=0;
5924 sj5=gconst1;
5925 cj5=gconst2;
5926 j5=x457;
5927 new_r01=0;
5928 new_r10=0;
5929 IkReal gconst0=x457;
5930 IkReal x459 = new_r11*new_r11;
5931 if(IKabs(x459)==0){
5932 continue;
5933 }
5934 IkReal gconst1=(new_r11*(pow(x459,-0.5)));
5935 IkReal gconst2=0;
5936 j3eval[0]=new_r11;
5937 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5938 {
5939 {
5940 IkReal j3array[2], cj3array[2], sj3array[2];
5941 bool j3valid[2]={false};
5942 _nj3 = 2;
5943 CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst1,-1);
5944 if(!x460.valid){
5945 continue;
5946 }
5947 sj3array[0]=((-1.0)*new_r00*(x460.value));
5948 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5949 {
5950  j3valid[0] = j3valid[1] = true;
5951  j3array[0] = IKasin(sj3array[0]);
5952  cj3array[0] = IKcos(j3array[0]);
5953  sj3array[1] = sj3array[0];
5954  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5955  cj3array[1] = -cj3array[0];
5956 }
5957 else if( isnan(sj3array[0]) )
5958 {
5959  // probably any value will work
5960  j3valid[0] = true;
5961  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5962 }
5963 for(int ij3 = 0; ij3 < 2; ++ij3)
5964 {
5965 if( !j3valid[ij3] )
5966 {
5967  continue;
5968 }
5969 _ij3[0] = ij3; _ij3[1] = -1;
5970 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5971 {
5972 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5973 {
5974  j3valid[iij3]=false; _ij3[1] = iij3; break;
5975 }
5976 }
5977 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5978 {
5979 IkReal evalcond[6];
5980 IkReal x461=IKcos(j3);
5981 IkReal x462=IKsin(j3);
5982 evalcond[0]=(gconst1*x461);
5983 evalcond[1]=(new_r00*x461);
5984 evalcond[2]=((-1.0)*new_r11*x461);
5985 evalcond[3]=(((new_r00*x462))+gconst1);
5986 evalcond[4]=(((new_r11*x462))+gconst1);
5987 evalcond[5]=(((gconst1*x462))+new_r11);
5988 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 )
5989 {
5990 continue;
5991 }
5992 }
5993 
5994 {
5995 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5996 vinfos[0].jointtype = 1;
5997 vinfos[0].foffset = j0;
5998 vinfos[0].indices[0] = _ij0[0];
5999 vinfos[0].indices[1] = _ij0[1];
6000 vinfos[0].maxsolutions = _nj0;
6001 vinfos[1].jointtype = 1;
6002 vinfos[1].foffset = j1;
6003 vinfos[1].indices[0] = _ij1[0];
6004 vinfos[1].indices[1] = _ij1[1];
6005 vinfos[1].maxsolutions = _nj1;
6006 vinfos[2].jointtype = 1;
6007 vinfos[2].foffset = j2;
6008 vinfos[2].indices[0] = _ij2[0];
6009 vinfos[2].indices[1] = _ij2[1];
6010 vinfos[2].maxsolutions = _nj2;
6011 vinfos[3].jointtype = 1;
6012 vinfos[3].foffset = j3;
6013 vinfos[3].indices[0] = _ij3[0];
6014 vinfos[3].indices[1] = _ij3[1];
6015 vinfos[3].maxsolutions = _nj3;
6016 vinfos[4].jointtype = 1;
6017 vinfos[4].foffset = j4;
6018 vinfos[4].indices[0] = _ij4[0];
6019 vinfos[4].indices[1] = _ij4[1];
6020 vinfos[4].maxsolutions = _nj4;
6021 vinfos[5].jointtype = 1;
6022 vinfos[5].foffset = j5;
6023 vinfos[5].indices[0] = _ij5[0];
6024 vinfos[5].indices[1] = _ij5[1];
6025 vinfos[5].maxsolutions = _nj5;
6026 std::vector<int> vfree(0);
6027 solutions.AddSolution(vinfos,vfree);
6028 }
6029 }
6030 }
6031 
6032 } else
6033 {
6034 {
6035 IkReal j3array[2], cj3array[2], sj3array[2];
6036 bool j3valid[2]={false};
6037 _nj3 = 2;
6038 CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
6039 if(!x463.valid){
6040 continue;
6041 }
6042 sj3array[0]=((-1.0)*gconst1*(x463.value));
6043 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6044 {
6045  j3valid[0] = j3valid[1] = true;
6046  j3array[0] = IKasin(sj3array[0]);
6047  cj3array[0] = IKcos(j3array[0]);
6048  sj3array[1] = sj3array[0];
6049  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6050  cj3array[1] = -cj3array[0];
6051 }
6052 else if( isnan(sj3array[0]) )
6053 {
6054  // probably any value will work
6055  j3valid[0] = true;
6056  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6057 }
6058 for(int ij3 = 0; ij3 < 2; ++ij3)
6059 {
6060 if( !j3valid[ij3] )
6061 {
6062  continue;
6063 }
6064 _ij3[0] = ij3; _ij3[1] = -1;
6065 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6066 {
6067 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6068 {
6069  j3valid[iij3]=false; _ij3[1] = iij3; break;
6070 }
6071 }
6072 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6073 {
6074 IkReal evalcond[6];
6075 IkReal x464=IKcos(j3);
6076 IkReal x465=IKsin(j3);
6077 IkReal x466=(gconst1*x465);
6078 evalcond[0]=(gconst1*x464);
6079 evalcond[1]=(new_r00*x464);
6080 evalcond[2]=((-1.0)*new_r11*x464);
6081 evalcond[3]=(((new_r00*x465))+gconst1);
6082 evalcond[4]=(x466+new_r00);
6083 evalcond[5]=(x466+new_r11);
6084 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 )
6085 {
6086 continue;
6087 }
6088 }
6089 
6090 {
6091 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6092 vinfos[0].jointtype = 1;
6093 vinfos[0].foffset = j0;
6094 vinfos[0].indices[0] = _ij0[0];
6095 vinfos[0].indices[1] = _ij0[1];
6096 vinfos[0].maxsolutions = _nj0;
6097 vinfos[1].jointtype = 1;
6098 vinfos[1].foffset = j1;
6099 vinfos[1].indices[0] = _ij1[0];
6100 vinfos[1].indices[1] = _ij1[1];
6101 vinfos[1].maxsolutions = _nj1;
6102 vinfos[2].jointtype = 1;
6103 vinfos[2].foffset = j2;
6104 vinfos[2].indices[0] = _ij2[0];
6105 vinfos[2].indices[1] = _ij2[1];
6106 vinfos[2].maxsolutions = _nj2;
6107 vinfos[3].jointtype = 1;
6108 vinfos[3].foffset = j3;
6109 vinfos[3].indices[0] = _ij3[0];
6110 vinfos[3].indices[1] = _ij3[1];
6111 vinfos[3].maxsolutions = _nj3;
6112 vinfos[4].jointtype = 1;
6113 vinfos[4].foffset = j4;
6114 vinfos[4].indices[0] = _ij4[0];
6115 vinfos[4].indices[1] = _ij4[1];
6116 vinfos[4].maxsolutions = _nj4;
6117 vinfos[5].jointtype = 1;
6118 vinfos[5].foffset = j5;
6119 vinfos[5].indices[0] = _ij5[0];
6120 vinfos[5].indices[1] = _ij5[1];
6121 vinfos[5].maxsolutions = _nj5;
6122 std::vector<int> vfree(0);
6123 solutions.AddSolution(vinfos,vfree);
6124 }
6125 }
6126 }
6127 
6128 }
6129 
6130 }
6131 
6132 } else
6133 {
6134 {
6135 IkReal j3array[2], cj3array[2], sj3array[2];
6136 bool j3valid[2]={false};
6137 _nj3 = 2;
6138 CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6139 if(!x467.valid){
6140 continue;
6141 }
6142 sj3array[0]=((-1.0)*gconst1*(x467.value));
6143 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6144 {
6145  j3valid[0] = j3valid[1] = true;
6146  j3array[0] = IKasin(sj3array[0]);
6147  cj3array[0] = IKcos(j3array[0]);
6148  sj3array[1] = sj3array[0];
6149  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6150  cj3array[1] = -cj3array[0];
6151 }
6152 else if( isnan(sj3array[0]) )
6153 {
6154  // probably any value will work
6155  j3valid[0] = true;
6156  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6157 }
6158 for(int ij3 = 0; ij3 < 2; ++ij3)
6159 {
6160 if( !j3valid[ij3] )
6161 {
6162  continue;
6163 }
6164 _ij3[0] = ij3; _ij3[1] = -1;
6165 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6166 {
6167 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6168 {
6169  j3valid[iij3]=false; _ij3[1] = iij3; break;
6170 }
6171 }
6172 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6173 {
6174 IkReal evalcond[6];
6175 IkReal x468=IKcos(j3);
6176 IkReal x469=IKsin(j3);
6177 IkReal x470=(gconst1*x469);
6178 evalcond[0]=(gconst1*x468);
6179 evalcond[1]=(new_r00*x468);
6180 evalcond[2]=((-1.0)*new_r11*x468);
6181 evalcond[3]=(((new_r11*x469))+gconst1);
6182 evalcond[4]=(x470+new_r00);
6183 evalcond[5]=(x470+new_r11);
6184 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 )
6185 {
6186 continue;
6187 }
6188 }
6189 
6190 {
6191 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6192 vinfos[0].jointtype = 1;
6193 vinfos[0].foffset = j0;
6194 vinfos[0].indices[0] = _ij0[0];
6195 vinfos[0].indices[1] = _ij0[1];
6196 vinfos[0].maxsolutions = _nj0;
6197 vinfos[1].jointtype = 1;
6198 vinfos[1].foffset = j1;
6199 vinfos[1].indices[0] = _ij1[0];
6200 vinfos[1].indices[1] = _ij1[1];
6201 vinfos[1].maxsolutions = _nj1;
6202 vinfos[2].jointtype = 1;
6203 vinfos[2].foffset = j2;
6204 vinfos[2].indices[0] = _ij2[0];
6205 vinfos[2].indices[1] = _ij2[1];
6206 vinfos[2].maxsolutions = _nj2;
6207 vinfos[3].jointtype = 1;
6208 vinfos[3].foffset = j3;
6209 vinfos[3].indices[0] = _ij3[0];
6210 vinfos[3].indices[1] = _ij3[1];
6211 vinfos[3].maxsolutions = _nj3;
6212 vinfos[4].jointtype = 1;
6213 vinfos[4].foffset = j4;
6214 vinfos[4].indices[0] = _ij4[0];
6215 vinfos[4].indices[1] = _ij4[1];
6216 vinfos[4].maxsolutions = _nj4;
6217 vinfos[5].jointtype = 1;
6218 vinfos[5].foffset = j5;
6219 vinfos[5].indices[0] = _ij5[0];
6220 vinfos[5].indices[1] = _ij5[1];
6221 vinfos[5].maxsolutions = _nj5;
6222 std::vector<int> vfree(0);
6223 solutions.AddSolution(vinfos,vfree);
6224 }
6225 }
6226 }
6227 
6228 }
6229 
6230 }
6231 
6232 }
6233 } while(0);
6234 if( bgotonextstatement )
6235 {
6236 bool bgotonextstatement = true;
6237 do
6238 {
6239 evalcond[0]=IKabs(new_r11);
6240 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6241 {
6242 bgotonextstatement=false;
6243 {
6244 IkReal j3eval[1];
6245 IkReal x471=((-1.0)*new_r01);
6246 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6247 if(!x473.valid){
6248 continue;
6249 }
6250 IkReal x472=((-1.0)*(x473.value));
6251 sj4=0;
6252 cj4=1.0;
6253 j4=0;
6254 sj5=gconst1;
6255 cj5=gconst2;
6256 j5=x472;
6257 new_r11=0;
6258 IkReal gconst0=x472;
6259 IkReal gconst1=0;
6260 IkReal x474 = new_r01*new_r01;
6261 if(IKabs(x474)==0){
6262 continue;
6263 }
6264 IkReal gconst2=(x471*(pow(x474,-0.5)));
6265 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6266 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6267 {
6268 {
6269 IkReal j3eval[1];
6270 IkReal x475=((-1.0)*new_r01);
6271 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6272 if(!x477.valid){
6273 continue;
6274 }
6275 IkReal x476=((-1.0)*(x477.value));
6276 sj4=0;
6277 cj4=1.0;
6278 j4=0;
6279 sj5=gconst1;
6280 cj5=gconst2;
6281 j5=x476;
6282 new_r11=0;
6283 IkReal gconst0=x476;
6284 IkReal gconst1=0;
6285 IkReal x478 = new_r01*new_r01;
6286 if(IKabs(x478)==0){
6287 continue;
6288 }
6289 IkReal gconst2=(x475*(pow(x478,-0.5)));
6290 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6291 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6292 {
6293 {
6294 IkReal j3eval[1];
6295 IkReal x479=((-1.0)*new_r01);
6296 CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal(0),IkReal(x479),IKFAST_ATAN2_MAGTHRESH);
6297 if(!x481.valid){
6298 continue;
6299 }
6300 IkReal x480=((-1.0)*(x481.value));
6301 sj4=0;
6302 cj4=1.0;
6303 j4=0;
6304 sj5=gconst1;
6305 cj5=gconst2;
6306 j5=x480;
6307 new_r11=0;
6308 IkReal gconst0=x480;
6309 IkReal gconst1=0;
6310 IkReal x482 = new_r01*new_r01;
6311 if(IKabs(x482)==0){
6312 continue;
6313 }
6314 IkReal gconst2=(x479*(pow(x482,-0.5)));
6315 j3eval[0]=new_r01;
6316 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6317 {
6318 continue; // 3 cases reached
6319 
6320 } else
6321 {
6322 {
6323 IkReal j3array[1], cj3array[1], sj3array[1];
6324 bool j3valid[1]={false};
6325 _nj3 = 1;
6326 CheckValue<IkReal> x483=IKPowWithIntegerCheck(new_r01,-1);
6327 if(!x483.valid){
6328 continue;
6329 }
6330 CheckValue<IkReal> x484=IKPowWithIntegerCheck(gconst2,-1);
6331 if(!x484.valid){
6332 continue;
6333 }
6334 if( IKabs(((-1.0)*gconst2*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x484.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst2*(x483.value)))+IKsqr((new_r00*(x484.value)))-1) <= IKFAST_SINCOS_THRESH )
6335  continue;
6336 j3array[0]=IKatan2(((-1.0)*gconst2*(x483.value)), (new_r00*(x484.value)));
6337 sj3array[0]=IKsin(j3array[0]);
6338 cj3array[0]=IKcos(j3array[0]);
6339 if( j3array[0] > IKPI )
6340 {
6341  j3array[0]-=IK2PI;
6342 }
6343 else if( j3array[0] < -IKPI )
6344 { j3array[0]+=IK2PI;
6345 }
6346 j3valid[0] = true;
6347 for(int ij3 = 0; ij3 < 1; ++ij3)
6348 {
6349 if( !j3valid[ij3] )
6350 {
6351  continue;
6352 }
6353 _ij3[0] = ij3; _ij3[1] = -1;
6354 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6355 {
6356 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6357 {
6358  j3valid[iij3]=false; _ij3[1] = iij3; break;
6359 }
6360 }
6361 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6362 {
6363 IkReal evalcond[8];
6364 IkReal x485=IKcos(j3);
6365 IkReal x486=IKsin(j3);
6366 IkReal x487=((1.0)*gconst2);
6367 IkReal x488=(gconst2*x486);
6368 evalcond[0]=(new_r01*x485);
6369 evalcond[1]=((-1.0)*gconst2*x485);
6370 evalcond[2]=(gconst2+((new_r01*x486)));
6371 evalcond[3]=(x488+new_r01);
6372 evalcond[4]=(new_r00+(((-1.0)*x485*x487)));
6373 evalcond[5]=((((-1.0)*x486*x487))+new_r10);
6374 evalcond[6]=((((-1.0)*new_r10*x485))+((new_r00*x486)));
6375 evalcond[7]=((((-1.0)*x487))+((new_r10*x486))+((new_r00*x485)));
6376 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 )
6377 {
6378 continue;
6379 }
6380 }
6381 
6382 {
6383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6384 vinfos[0].jointtype = 1;
6385 vinfos[0].foffset = j0;
6386 vinfos[0].indices[0] = _ij0[0];
6387 vinfos[0].indices[1] = _ij0[1];
6388 vinfos[0].maxsolutions = _nj0;
6389 vinfos[1].jointtype = 1;
6390 vinfos[1].foffset = j1;
6391 vinfos[1].indices[0] = _ij1[0];
6392 vinfos[1].indices[1] = _ij1[1];
6393 vinfos[1].maxsolutions = _nj1;
6394 vinfos[2].jointtype = 1;
6395 vinfos[2].foffset = j2;
6396 vinfos[2].indices[0] = _ij2[0];
6397 vinfos[2].indices[1] = _ij2[1];
6398 vinfos[2].maxsolutions = _nj2;
6399 vinfos[3].jointtype = 1;
6400 vinfos[3].foffset = j3;
6401 vinfos[3].indices[0] = _ij3[0];
6402 vinfos[3].indices[1] = _ij3[1];
6403 vinfos[3].maxsolutions = _nj3;
6404 vinfos[4].jointtype = 1;
6405 vinfos[4].foffset = j4;
6406 vinfos[4].indices[0] = _ij4[0];
6407 vinfos[4].indices[1] = _ij4[1];
6408 vinfos[4].maxsolutions = _nj4;
6409 vinfos[5].jointtype = 1;
6410 vinfos[5].foffset = j5;
6411 vinfos[5].indices[0] = _ij5[0];
6412 vinfos[5].indices[1] = _ij5[1];
6413 vinfos[5].maxsolutions = _nj5;
6414 std::vector<int> vfree(0);
6415 solutions.AddSolution(vinfos,vfree);
6416 }
6417 }
6418 }
6419 
6420 }
6421 
6422 }
6423 
6424 } else
6425 {
6426 {
6427 IkReal j3array[1], cj3array[1], sj3array[1];
6428 bool j3valid[1]={false};
6429 _nj3 = 1;
6430 CheckValue<IkReal> x489 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6431 if(!x489.valid){
6432 continue;
6433 }
6434 CheckValue<IkReal> x490=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6435 if(!x490.valid){
6436 continue;
6437 }
6438 j3array[0]=((-1.5707963267949)+(x489.value)+(((1.5707963267949)*(x490.value))));
6439 sj3array[0]=IKsin(j3array[0]);
6440 cj3array[0]=IKcos(j3array[0]);
6441 if( j3array[0] > IKPI )
6442 {
6443  j3array[0]-=IK2PI;
6444 }
6445 else if( j3array[0] < -IKPI )
6446 { j3array[0]+=IK2PI;
6447 }
6448 j3valid[0] = true;
6449 for(int ij3 = 0; ij3 < 1; ++ij3)
6450 {
6451 if( !j3valid[ij3] )
6452 {
6453  continue;
6454 }
6455 _ij3[0] = ij3; _ij3[1] = -1;
6456 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6457 {
6458 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6459 {
6460  j3valid[iij3]=false; _ij3[1] = iij3; break;
6461 }
6462 }
6463 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6464 {
6465 IkReal evalcond[8];
6466 IkReal x491=IKcos(j3);
6467 IkReal x492=IKsin(j3);
6468 IkReal x493=((1.0)*gconst2);
6469 IkReal x494=(gconst2*x492);
6470 evalcond[0]=(new_r01*x491);
6471 evalcond[1]=((-1.0)*gconst2*x491);
6472 evalcond[2]=(gconst2+((new_r01*x492)));
6473 evalcond[3]=(x494+new_r01);
6474 evalcond[4]=((((-1.0)*x491*x493))+new_r00);
6475 evalcond[5]=(new_r10+(((-1.0)*x492*x493)));
6476 evalcond[6]=((((-1.0)*new_r10*x491))+((new_r00*x492)));
6477 evalcond[7]=((((-1.0)*x493))+((new_r10*x492))+((new_r00*x491)));
6478 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 )
6479 {
6480 continue;
6481 }
6482 }
6483 
6484 {
6485 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6486 vinfos[0].jointtype = 1;
6487 vinfos[0].foffset = j0;
6488 vinfos[0].indices[0] = _ij0[0];
6489 vinfos[0].indices[1] = _ij0[1];
6490 vinfos[0].maxsolutions = _nj0;
6491 vinfos[1].jointtype = 1;
6492 vinfos[1].foffset = j1;
6493 vinfos[1].indices[0] = _ij1[0];
6494 vinfos[1].indices[1] = _ij1[1];
6495 vinfos[1].maxsolutions = _nj1;
6496 vinfos[2].jointtype = 1;
6497 vinfos[2].foffset = j2;
6498 vinfos[2].indices[0] = _ij2[0];
6499 vinfos[2].indices[1] = _ij2[1];
6500 vinfos[2].maxsolutions = _nj2;
6501 vinfos[3].jointtype = 1;
6502 vinfos[3].foffset = j3;
6503 vinfos[3].indices[0] = _ij3[0];
6504 vinfos[3].indices[1] = _ij3[1];
6505 vinfos[3].maxsolutions = _nj3;
6506 vinfos[4].jointtype = 1;
6507 vinfos[4].foffset = j4;
6508 vinfos[4].indices[0] = _ij4[0];
6509 vinfos[4].indices[1] = _ij4[1];
6510 vinfos[4].maxsolutions = _nj4;
6511 vinfos[5].jointtype = 1;
6512 vinfos[5].foffset = j5;
6513 vinfos[5].indices[0] = _ij5[0];
6514 vinfos[5].indices[1] = _ij5[1];
6515 vinfos[5].maxsolutions = _nj5;
6516 std::vector<int> vfree(0);
6517 solutions.AddSolution(vinfos,vfree);
6518 }
6519 }
6520 }
6521 
6522 }
6523 
6524 }
6525 
6526 } else
6527 {
6528 {
6529 IkReal j3array[1], cj3array[1], sj3array[1];
6530 bool j3valid[1]={false};
6531 _nj3 = 1;
6532 CheckValue<IkReal> x495=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6533 if(!x495.valid){
6534 continue;
6535 }
6536 CheckValue<IkReal> x496 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6537 if(!x496.valid){
6538 continue;
6539 }
6540 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x495.value)))+(x496.value));
6541 sj3array[0]=IKsin(j3array[0]);
6542 cj3array[0]=IKcos(j3array[0]);
6543 if( j3array[0] > IKPI )
6544 {
6545  j3array[0]-=IK2PI;
6546 }
6547 else if( j3array[0] < -IKPI )
6548 { j3array[0]+=IK2PI;
6549 }
6550 j3valid[0] = true;
6551 for(int ij3 = 0; ij3 < 1; ++ij3)
6552 {
6553 if( !j3valid[ij3] )
6554 {
6555  continue;
6556 }
6557 _ij3[0] = ij3; _ij3[1] = -1;
6558 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6559 {
6560 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6561 {
6562  j3valid[iij3]=false; _ij3[1] = iij3; break;
6563 }
6564 }
6565 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6566 {
6567 IkReal evalcond[8];
6568 IkReal x497=IKcos(j3);
6569 IkReal x498=IKsin(j3);
6570 IkReal x499=((1.0)*gconst2);
6571 IkReal x500=(gconst2*x498);
6572 evalcond[0]=(new_r01*x497);
6573 evalcond[1]=((-1.0)*gconst2*x497);
6574 evalcond[2]=(gconst2+((new_r01*x498)));
6575 evalcond[3]=(x500+new_r01);
6576 evalcond[4]=((((-1.0)*x497*x499))+new_r00);
6577 evalcond[5]=((((-1.0)*x498*x499))+new_r10);
6578 evalcond[6]=((((-1.0)*new_r10*x497))+((new_r00*x498)));
6579 evalcond[7]=((((-1.0)*x499))+((new_r10*x498))+((new_r00*x497)));
6580 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 )
6581 {
6582 continue;
6583 }
6584 }
6585 
6586 {
6587 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6588 vinfos[0].jointtype = 1;
6589 vinfos[0].foffset = j0;
6590 vinfos[0].indices[0] = _ij0[0];
6591 vinfos[0].indices[1] = _ij0[1];
6592 vinfos[0].maxsolutions = _nj0;
6593 vinfos[1].jointtype = 1;
6594 vinfos[1].foffset = j1;
6595 vinfos[1].indices[0] = _ij1[0];
6596 vinfos[1].indices[1] = _ij1[1];
6597 vinfos[1].maxsolutions = _nj1;
6598 vinfos[2].jointtype = 1;
6599 vinfos[2].foffset = j2;
6600 vinfos[2].indices[0] = _ij2[0];
6601 vinfos[2].indices[1] = _ij2[1];
6602 vinfos[2].maxsolutions = _nj2;
6603 vinfos[3].jointtype = 1;
6604 vinfos[3].foffset = j3;
6605 vinfos[3].indices[0] = _ij3[0];
6606 vinfos[3].indices[1] = _ij3[1];
6607 vinfos[3].maxsolutions = _nj3;
6608 vinfos[4].jointtype = 1;
6609 vinfos[4].foffset = j4;
6610 vinfos[4].indices[0] = _ij4[0];
6611 vinfos[4].indices[1] = _ij4[1];
6612 vinfos[4].maxsolutions = _nj4;
6613 vinfos[5].jointtype = 1;
6614 vinfos[5].foffset = j5;
6615 vinfos[5].indices[0] = _ij5[0];
6616 vinfos[5].indices[1] = _ij5[1];
6617 vinfos[5].maxsolutions = _nj5;
6618 std::vector<int> vfree(0);
6619 solutions.AddSolution(vinfos,vfree);
6620 }
6621 }
6622 }
6623 
6624 }
6625 
6626 }
6627 
6628 }
6629 } while(0);
6630 if( bgotonextstatement )
6631 {
6632 bool bgotonextstatement = true;
6633 do
6634 {
6635 if( 1 )
6636 {
6637 bgotonextstatement=false;
6638 continue; // branch miss [j3]
6639 
6640 }
6641 } while(0);
6642 if( bgotonextstatement )
6643 {
6644 }
6645 }
6646 }
6647 }
6648 }
6649 }
6650 
6651 } else
6652 {
6653 {
6654 IkReal j3array[1], cj3array[1], sj3array[1];
6655 bool j3valid[1]={false};
6656 _nj3 = 1;
6657 IkReal x501=((1.0)*new_r01);
6658 CheckValue<IkReal> x502 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x501))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
6659 if(!x502.valid){
6660 continue;
6661 }
6662 CheckValue<IkReal> x503=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x501)))),-1);
6663 if(!x503.valid){
6664 continue;
6665 }
6666 j3array[0]=((-1.5707963267949)+(x502.value)+(((1.5707963267949)*(x503.value))));
6667 sj3array[0]=IKsin(j3array[0]);
6668 cj3array[0]=IKcos(j3array[0]);
6669 if( j3array[0] > IKPI )
6670 {
6671  j3array[0]-=IK2PI;
6672 }
6673 else if( j3array[0] < -IKPI )
6674 { j3array[0]+=IK2PI;
6675 }
6676 j3valid[0] = true;
6677 for(int ij3 = 0; ij3 < 1; ++ij3)
6678 {
6679 if( !j3valid[ij3] )
6680 {
6681  continue;
6682 }
6683 _ij3[0] = ij3; _ij3[1] = -1;
6684 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6685 {
6686 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6687 {
6688  j3valid[iij3]=false; _ij3[1] = iij3; break;
6689 }
6690 }
6691 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6692 {
6693 IkReal evalcond[8];
6694 IkReal x504=IKcos(j3);
6695 IkReal x505=IKsin(j3);
6696 IkReal x506=((1.0)*gconst2);
6697 IkReal x507=(gconst1*x505);
6698 IkReal x508=(gconst2*x505);
6699 IkReal x509=(gconst1*x504);
6700 IkReal x510=((1.0)*x504);
6701 IkReal x511=(x504*x506);
6702 evalcond[0]=(gconst1+((new_r11*x505))+((new_r01*x504)));
6703 evalcond[1]=(x508+x509+new_r01);
6704 evalcond[2]=((((-1.0)*new_r10*x510))+gconst1+((new_r00*x505)));
6705 evalcond[3]=((((-1.0)*new_r11*x510))+gconst2+((new_r01*x505)));
6706 evalcond[4]=(x507+new_r00+(((-1.0)*x511)));
6707 evalcond[5]=(x507+new_r11+(((-1.0)*x511)));
6708 evalcond[6]=((((-1.0)*x506))+((new_r10*x505))+((new_r00*x504)));
6709 evalcond[7]=((((-1.0)*x505*x506))+new_r10+(((-1.0)*x509)));
6710 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 )
6711 {
6712 continue;
6713 }
6714 }
6715 
6716 {
6717 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6718 vinfos[0].jointtype = 1;
6719 vinfos[0].foffset = j0;
6720 vinfos[0].indices[0] = _ij0[0];
6721 vinfos[0].indices[1] = _ij0[1];
6722 vinfos[0].maxsolutions = _nj0;
6723 vinfos[1].jointtype = 1;
6724 vinfos[1].foffset = j1;
6725 vinfos[1].indices[0] = _ij1[0];
6726 vinfos[1].indices[1] = _ij1[1];
6727 vinfos[1].maxsolutions = _nj1;
6728 vinfos[2].jointtype = 1;
6729 vinfos[2].foffset = j2;
6730 vinfos[2].indices[0] = _ij2[0];
6731 vinfos[2].indices[1] = _ij2[1];
6732 vinfos[2].maxsolutions = _nj2;
6733 vinfos[3].jointtype = 1;
6734 vinfos[3].foffset = j3;
6735 vinfos[3].indices[0] = _ij3[0];
6736 vinfos[3].indices[1] = _ij3[1];
6737 vinfos[3].maxsolutions = _nj3;
6738 vinfos[4].jointtype = 1;
6739 vinfos[4].foffset = j4;
6740 vinfos[4].indices[0] = _ij4[0];
6741 vinfos[4].indices[1] = _ij4[1];
6742 vinfos[4].maxsolutions = _nj4;
6743 vinfos[5].jointtype = 1;
6744 vinfos[5].foffset = j5;
6745 vinfos[5].indices[0] = _ij5[0];
6746 vinfos[5].indices[1] = _ij5[1];
6747 vinfos[5].maxsolutions = _nj5;
6748 std::vector<int> vfree(0);
6749 solutions.AddSolution(vinfos,vfree);
6750 }
6751 }
6752 }
6753 
6754 }
6755 
6756 }
6757 
6758 } else
6759 {
6760 {
6761 IkReal j3array[1], cj3array[1], sj3array[1];
6762 bool j3valid[1]={false};
6763 _nj3 = 1;
6764 IkReal x512=((1.0)*gconst2);
6765 CheckValue<IkReal> x513 = IKatan2WithCheck(IkReal(((gconst1*gconst1)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst1*x512)))),IKFAST_ATAN2_MAGTHRESH);
6766 if(!x513.valid){
6767 continue;
6768 }
6769 CheckValue<IkReal> x514=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x512))+(((-1.0)*gconst1*new_r00)))),-1);
6770 if(!x514.valid){
6771 continue;
6772 }
6773 j3array[0]=((-1.5707963267949)+(x513.value)+(((1.5707963267949)*(x514.value))));
6774 sj3array[0]=IKsin(j3array[0]);
6775 cj3array[0]=IKcos(j3array[0]);
6776 if( j3array[0] > IKPI )
6777 {
6778  j3array[0]-=IK2PI;
6779 }
6780 else if( j3array[0] < -IKPI )
6781 { j3array[0]+=IK2PI;
6782 }
6783 j3valid[0] = true;
6784 for(int ij3 = 0; ij3 < 1; ++ij3)
6785 {
6786 if( !j3valid[ij3] )
6787 {
6788  continue;
6789 }
6790 _ij3[0] = ij3; _ij3[1] = -1;
6791 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6792 {
6793 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6794 {
6795  j3valid[iij3]=false; _ij3[1] = iij3; break;
6796 }
6797 }
6798 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6799 {
6800 IkReal evalcond[8];
6801 IkReal x515=IKcos(j3);
6802 IkReal x516=IKsin(j3);
6803 IkReal x517=((1.0)*gconst2);
6804 IkReal x518=(gconst1*x516);
6805 IkReal x519=(gconst2*x516);
6806 IkReal x520=(gconst1*x515);
6807 IkReal x521=((1.0)*x515);
6808 IkReal x522=(x515*x517);
6809 evalcond[0]=(((new_r01*x515))+((new_r11*x516))+gconst1);
6810 evalcond[1]=(x520+x519+new_r01);
6811 evalcond[2]=(((new_r00*x516))+gconst1+(((-1.0)*new_r10*x521)));
6812 evalcond[3]=(((new_r01*x516))+gconst2+(((-1.0)*new_r11*x521)));
6813 evalcond[4]=((((-1.0)*x522))+x518+new_r00);
6814 evalcond[5]=((((-1.0)*x522))+x518+new_r11);
6815 evalcond[6]=(((new_r00*x515))+((new_r10*x516))+(((-1.0)*x517)));
6816 evalcond[7]=((((-1.0)*x520))+(((-1.0)*x516*x517))+new_r10);
6817 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 )
6818 {
6819 continue;
6820 }
6821 }
6822 
6823 {
6824 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6825 vinfos[0].jointtype = 1;
6826 vinfos[0].foffset = j0;
6827 vinfos[0].indices[0] = _ij0[0];
6828 vinfos[0].indices[1] = _ij0[1];
6829 vinfos[0].maxsolutions = _nj0;
6830 vinfos[1].jointtype = 1;
6831 vinfos[1].foffset = j1;
6832 vinfos[1].indices[0] = _ij1[0];
6833 vinfos[1].indices[1] = _ij1[1];
6834 vinfos[1].maxsolutions = _nj1;
6835 vinfos[2].jointtype = 1;
6836 vinfos[2].foffset = j2;
6837 vinfos[2].indices[0] = _ij2[0];
6838 vinfos[2].indices[1] = _ij2[1];
6839 vinfos[2].maxsolutions = _nj2;
6840 vinfos[3].jointtype = 1;
6841 vinfos[3].foffset = j3;
6842 vinfos[3].indices[0] = _ij3[0];
6843 vinfos[3].indices[1] = _ij3[1];
6844 vinfos[3].maxsolutions = _nj3;
6845 vinfos[4].jointtype = 1;
6846 vinfos[4].foffset = j4;
6847 vinfos[4].indices[0] = _ij4[0];
6848 vinfos[4].indices[1] = _ij4[1];
6849 vinfos[4].maxsolutions = _nj4;
6850 vinfos[5].jointtype = 1;
6851 vinfos[5].foffset = j5;
6852 vinfos[5].indices[0] = _ij5[0];
6853 vinfos[5].indices[1] = _ij5[1];
6854 vinfos[5].maxsolutions = _nj5;
6855 std::vector<int> vfree(0);
6856 solutions.AddSolution(vinfos,vfree);
6857 }
6858 }
6859 }
6860 
6861 }
6862 
6863 }
6864 
6865 } else
6866 {
6867 {
6868 IkReal j3array[1], cj3array[1], sj3array[1];
6869 bool j3valid[1]={false};
6870 _nj3 = 1;
6871 IkReal x523=((1.0)*new_r11);
6872 CheckValue<IkReal> x524 = IKatan2WithCheck(IkReal((((gconst1*new_r10))+((gconst1*new_r01)))),IkReal((((gconst1*new_r00))+(((-1.0)*gconst1*x523)))),IKFAST_ATAN2_MAGTHRESH);
6873 if(!x524.valid){
6874 continue;
6875 }
6876 CheckValue<IkReal> x525=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x523))+(((-1.0)*new_r00*new_r01)))),-1);
6877 if(!x525.valid){
6878 continue;
6879 }
6880 j3array[0]=((-1.5707963267949)+(x524.value)+(((1.5707963267949)*(x525.value))));
6881 sj3array[0]=IKsin(j3array[0]);
6882 cj3array[0]=IKcos(j3array[0]);
6883 if( j3array[0] > IKPI )
6884 {
6885  j3array[0]-=IK2PI;
6886 }
6887 else if( j3array[0] < -IKPI )
6888 { j3array[0]+=IK2PI;
6889 }
6890 j3valid[0] = true;
6891 for(int ij3 = 0; ij3 < 1; ++ij3)
6892 {
6893 if( !j3valid[ij3] )
6894 {
6895  continue;
6896 }
6897 _ij3[0] = ij3; _ij3[1] = -1;
6898 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6899 {
6900 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6901 {
6902  j3valid[iij3]=false; _ij3[1] = iij3; break;
6903 }
6904 }
6905 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6906 {
6907 IkReal evalcond[8];
6908 IkReal x526=IKcos(j3);
6909 IkReal x527=IKsin(j3);
6910 IkReal x528=((1.0)*gconst2);
6911 IkReal x529=(gconst1*x527);
6912 IkReal x530=(gconst2*x527);
6913 IkReal x531=(gconst1*x526);
6914 IkReal x532=((1.0)*x526);
6915 IkReal x533=(x526*x528);
6916 evalcond[0]=(((new_r01*x526))+gconst1+((new_r11*x527)));
6917 evalcond[1]=(x531+x530+new_r01);
6918 evalcond[2]=(gconst1+(((-1.0)*new_r10*x532))+((new_r00*x527)));
6919 evalcond[3]=(((new_r01*x527))+gconst2+(((-1.0)*new_r11*x532)));
6920 evalcond[4]=((((-1.0)*x533))+x529+new_r00);
6921 evalcond[5]=((((-1.0)*x533))+x529+new_r11);
6922 evalcond[6]=((((-1.0)*x528))+((new_r10*x527))+((new_r00*x526)));
6923 evalcond[7]=((((-1.0)*x527*x528))+(((-1.0)*x531))+new_r10);
6924 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 )
6925 {
6926 continue;
6927 }
6928 }
6929 
6930 {
6931 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6932 vinfos[0].jointtype = 1;
6933 vinfos[0].foffset = j0;
6934 vinfos[0].indices[0] = _ij0[0];
6935 vinfos[0].indices[1] = _ij0[1];
6936 vinfos[0].maxsolutions = _nj0;
6937 vinfos[1].jointtype = 1;
6938 vinfos[1].foffset = j1;
6939 vinfos[1].indices[0] = _ij1[0];
6940 vinfos[1].indices[1] = _ij1[1];
6941 vinfos[1].maxsolutions = _nj1;
6942 vinfos[2].jointtype = 1;
6943 vinfos[2].foffset = j2;
6944 vinfos[2].indices[0] = _ij2[0];
6945 vinfos[2].indices[1] = _ij2[1];
6946 vinfos[2].maxsolutions = _nj2;
6947 vinfos[3].jointtype = 1;
6948 vinfos[3].foffset = j3;
6949 vinfos[3].indices[0] = _ij3[0];
6950 vinfos[3].indices[1] = _ij3[1];
6951 vinfos[3].maxsolutions = _nj3;
6952 vinfos[4].jointtype = 1;
6953 vinfos[4].foffset = j4;
6954 vinfos[4].indices[0] = _ij4[0];
6955 vinfos[4].indices[1] = _ij4[1];
6956 vinfos[4].maxsolutions = _nj4;
6957 vinfos[5].jointtype = 1;
6958 vinfos[5].foffset = j5;
6959 vinfos[5].indices[0] = _ij5[0];
6960 vinfos[5].indices[1] = _ij5[1];
6961 vinfos[5].maxsolutions = _nj5;
6962 std::vector<int> vfree(0);
6963 solutions.AddSolution(vinfos,vfree);
6964 }
6965 }
6966 }
6967 
6968 }
6969 
6970 }
6971 
6972 }
6973 } while(0);
6974 if( bgotonextstatement )
6975 {
6976 bool bgotonextstatement = true;
6977 do
6978 {
6979 IkReal x534=((-1.0)*new_r11);
6980 IkReal x536 = ((new_r01*new_r01)+(new_r11*new_r11));
6981 if(IKabs(x536)==0){
6982 continue;
6983 }
6984 IkReal x535=pow(x536,-0.5);
6985 CheckValue<IkReal> x537 = IKatan2WithCheck(IkReal(x534),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6986 if(!x537.valid){
6987 continue;
6988 }
6989 IkReal gconst3=((3.14159265358979)+(((-1.0)*(x537.value))));
6990 IkReal gconst4=(x534*x535);
6991 IkReal gconst5=((1.0)*new_r01*x535);
6992 CheckValue<IkReal> x538 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6993 if(!x538.valid){
6994 continue;
6995 }
6996 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x538.value)+j5)))), 6.28318530717959)));
6997 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6998 {
6999 bgotonextstatement=false;
7000 {
7001 IkReal j3eval[3];
7002 IkReal x539=((-1.0)*new_r11);
7003 CheckValue<IkReal> x542 = IKatan2WithCheck(IkReal(x539),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7004 if(!x542.valid){
7005 continue;
7006 }
7007 IkReal x540=((1.0)*(x542.value));
7008 IkReal x541=x535;
7009 sj4=0;
7010 cj4=1.0;
7011 j4=0;
7012 sj5=gconst4;
7013 cj5=gconst5;
7014 j5=((3.14159265)+(((-1.0)*x540)));
7015 IkReal gconst3=((3.14159265358979)+(((-1.0)*x540)));
7016 IkReal gconst4=(x539*x541);
7017 IkReal gconst5=((1.0)*new_r01*x541);
7018 IkReal x543=new_r11*new_r11;
7019 IkReal x544=((1.0)*new_r01);
7020 IkReal x545=((1.0)*new_r10);
7021 IkReal x546=((((-1.0)*new_r00*x544))+(((-1.0)*new_r11*x545)));
7022 IkReal x547=x535;
7023 IkReal x548=(new_r11*x547);
7024 j3eval[0]=x546;
7025 j3eval[1]=((IKabs((((x543*x547))+(((-1.0)*new_r00*x548)))))+(IKabs(((((-1.0)*x544*x548))+(((-1.0)*x545*x548))))));
7026 j3eval[2]=IKsign(x546);
7027 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7028 {
7029 {
7030 IkReal j3eval[1];
7031 IkReal x549=((-1.0)*new_r11);
7032 CheckValue<IkReal> x552 = IKatan2WithCheck(IkReal(x549),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7033 if(!x552.valid){
7034 continue;
7035 }
7036 IkReal x550=((1.0)*(x552.value));
7037 IkReal x551=x535;
7038 sj4=0;
7039 cj4=1.0;
7040 j4=0;
7041 sj5=gconst4;
7042 cj5=gconst5;
7043 j5=((3.14159265)+(((-1.0)*x550)));
7044 IkReal gconst3=((3.14159265358979)+(((-1.0)*x550)));
7045 IkReal gconst4=(x549*x551);
7046 IkReal gconst5=((1.0)*new_r01*x551);
7047 IkReal x553=new_r11*new_r11;
7048 IkReal x554=new_r01*new_r01*new_r01;
7049 CheckValue<IkReal> x558=IKPowWithIntegerCheck(((new_r01*new_r01)+x553),-1);
7050 if(!x558.valid){
7051 continue;
7052 }
7053 IkReal x555=x558.value;
7054 IkReal x556=(x553*x555);
7055 IkReal x557=(x554*x555);
7056 j3eval[0]=((IKabs((((new_r10*x557))+x556+((new_r01*new_r10*x556)))))+(IKabs((((new_r00*new_r01*x556))+((new_r01*new_r11*x555))+((new_r00*x557))))));
7057 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7058 {
7059 {
7060 IkReal j3eval[1];
7061 IkReal x559=((-1.0)*new_r11);
7062 CheckValue<IkReal> x562 = IKatan2WithCheck(IkReal(x559),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7063 if(!x562.valid){
7064 continue;
7065 }
7066 IkReal x560=((1.0)*(x562.value));
7067 IkReal x561=x535;
7068 sj4=0;
7069 cj4=1.0;
7070 j4=0;
7071 sj5=gconst4;
7072 cj5=gconst5;
7073 j5=((3.14159265)+(((-1.0)*x560)));
7074 IkReal gconst3=((3.14159265358979)+(((-1.0)*x560)));
7075 IkReal gconst4=(x559*x561);
7076 IkReal gconst5=((1.0)*new_r01*x561);
7077 IkReal x563=new_r01*new_r01;
7078 IkReal x564=new_r11*new_r11;
7079 CheckValue<IkReal> x571=IKPowWithIntegerCheck((x564+x563),-1);
7080 if(!x571.valid){
7081 continue;
7082 }
7083 IkReal x565=x571.value;
7084 IkReal x566=(x564*x565);
7085 CheckValue<IkReal> x572=IKPowWithIntegerCheck(((((-1.0)*x563))+(((-1.0)*x564))),-1);
7086 if(!x572.valid){
7087 continue;
7088 }
7089 IkReal x567=x572.value;
7090 IkReal x568=((1.0)*x567);
7091 IkReal x569=(new_r11*x568);
7092 IkReal x570=(new_r01*x568);
7093 j3eval[0]=((IKabs((((x563*x566))+((x565*(x563*x563)))+(((-1.0)*x566)))))+(IKabs(((((-1.0)*new_r01*x569*(new_r11*new_r11)))+(((-1.0)*x569*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x569))))));
7094 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7095 {
7096 {
7097 IkReal evalcond[3];
7098 bool bgotonextstatement = true;
7099 do
7100 {
7101 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7102 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7103 {
7104 bgotonextstatement=false;
7105 {
7106 IkReal j3array[2], cj3array[2], sj3array[2];
7107 bool j3valid[2]={false};
7108 _nj3 = 2;
7109 CheckValue<IkReal> x573=IKPowWithIntegerCheck(gconst5,-1);
7110 if(!x573.valid){
7111 continue;
7112 }
7113 sj3array[0]=(new_r10*(x573.value));
7114 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7115 {
7116  j3valid[0] = j3valid[1] = true;
7117  j3array[0] = IKasin(sj3array[0]);
7118  cj3array[0] = IKcos(j3array[0]);
7119  sj3array[1] = sj3array[0];
7120  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7121  cj3array[1] = -cj3array[0];
7122 }
7123 else if( isnan(sj3array[0]) )
7124 {
7125  // probably any value will work
7126  j3valid[0] = true;
7127  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7128 }
7129 for(int ij3 = 0; ij3 < 2; ++ij3)
7130 {
7131 if( !j3valid[ij3] )
7132 {
7133  continue;
7134 }
7135 _ij3[0] = ij3; _ij3[1] = -1;
7136 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7137 {
7138 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7139 {
7140  j3valid[iij3]=false; _ij3[1] = iij3; break;
7141 }
7142 }
7143 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7144 {
7145 IkReal evalcond[6];
7146 IkReal x574=IKcos(j3);
7147 IkReal x575=IKsin(j3);
7148 IkReal x576=((-1.0)*x574);
7149 evalcond[0]=(new_r01*x574);
7150 evalcond[1]=(new_r10*x576);
7151 evalcond[2]=(gconst5*x576);
7152 evalcond[3]=(((new_r01*x575))+gconst5);
7153 evalcond[4]=(((gconst5*x575))+new_r01);
7154 evalcond[5]=(((new_r10*x575))+(((-1.0)*gconst5)));
7155 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 )
7156 {
7157 continue;
7158 }
7159 }
7160 
7161 {
7162 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7163 vinfos[0].jointtype = 1;
7164 vinfos[0].foffset = j0;
7165 vinfos[0].indices[0] = _ij0[0];
7166 vinfos[0].indices[1] = _ij0[1];
7167 vinfos[0].maxsolutions = _nj0;
7168 vinfos[1].jointtype = 1;
7169 vinfos[1].foffset = j1;
7170 vinfos[1].indices[0] = _ij1[0];
7171 vinfos[1].indices[1] = _ij1[1];
7172 vinfos[1].maxsolutions = _nj1;
7173 vinfos[2].jointtype = 1;
7174 vinfos[2].foffset = j2;
7175 vinfos[2].indices[0] = _ij2[0];
7176 vinfos[2].indices[1] = _ij2[1];
7177 vinfos[2].maxsolutions = _nj2;
7178 vinfos[3].jointtype = 1;
7179 vinfos[3].foffset = j3;
7180 vinfos[3].indices[0] = _ij3[0];
7181 vinfos[3].indices[1] = _ij3[1];
7182 vinfos[3].maxsolutions = _nj3;
7183 vinfos[4].jointtype = 1;
7184 vinfos[4].foffset = j4;
7185 vinfos[4].indices[0] = _ij4[0];
7186 vinfos[4].indices[1] = _ij4[1];
7187 vinfos[4].maxsolutions = _nj4;
7188 vinfos[5].jointtype = 1;
7189 vinfos[5].foffset = j5;
7190 vinfos[5].indices[0] = _ij5[0];
7191 vinfos[5].indices[1] = _ij5[1];
7192 vinfos[5].maxsolutions = _nj5;
7193 std::vector<int> vfree(0);
7194 solutions.AddSolution(vinfos,vfree);
7195 }
7196 }
7197 }
7198 
7199 }
7200 } while(0);
7201 if( bgotonextstatement )
7202 {
7203 bool bgotonextstatement = true;
7204 do
7205 {
7206 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7207 evalcond[1]=gconst4;
7208 evalcond[2]=gconst5;
7209 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
7210 {
7211 bgotonextstatement=false;
7212 {
7213 IkReal j3eval[3];
7214 IkReal x577=((-1.0)*new_r11);
7215 CheckValue<IkReal> x579 = IKatan2WithCheck(IkReal(x577),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7216 if(!x579.valid){
7217 continue;
7218 }
7219 IkReal x578=((1.0)*(x579.value));
7220 sj4=0;
7221 cj4=1.0;
7222 j4=0;
7223 sj5=gconst4;
7224 cj5=gconst5;
7225 j5=((3.14159265)+(((-1.0)*x578)));
7226 new_r00=0;
7227 new_r10=0;
7228 new_r21=0;
7229 new_r22=0;
7230 IkReal gconst3=((3.14159265358979)+(((-1.0)*x578)));
7231 IkReal gconst4=x577;
7232 IkReal gconst5=((1.0)*new_r01);
7233 j3eval[0]=1.0;
7234 j3eval[1]=1.0;
7235 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(IKabs(((1.0)*new_r01*new_r11))));
7236 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7237 {
7238 {
7239 IkReal j3eval[4];
7240 IkReal x580=((-1.0)*new_r11);
7241 CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal(x580),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7242 if(!x582.valid){
7243 continue;
7244 }
7245 IkReal x581=((1.0)*(x582.value));
7246 sj4=0;
7247 cj4=1.0;
7248 j4=0;
7249 sj5=gconst4;
7250 cj5=gconst5;
7251 j5=((3.14159265)+(((-1.0)*x581)));
7252 new_r00=0;
7253 new_r10=0;
7254 new_r21=0;
7255 new_r22=0;
7256 IkReal gconst3=((3.14159265358979)+(((-1.0)*x581)));
7257 IkReal gconst4=x580;
7258 IkReal gconst5=((1.0)*new_r01);
7259 j3eval[0]=-1.0;
7260 j3eval[1]=new_r01;
7261 j3eval[2]=1.0;
7262 j3eval[3]=-1.0;
7263 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
7264 {
7265 {
7266 IkReal j3eval[3];
7267 IkReal x583=((-1.0)*new_r11);
7268 CheckValue<IkReal> x585 = IKatan2WithCheck(IkReal(x583),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7269 if(!x585.valid){
7270 continue;
7271 }
7272 IkReal x584=((1.0)*(x585.value));
7273 sj4=0;
7274 cj4=1.0;
7275 j4=0;
7276 sj5=gconst4;
7277 cj5=gconst5;
7278 j5=((3.14159265)+(((-1.0)*x584)));
7279 new_r00=0;
7280 new_r10=0;
7281 new_r21=0;
7282 new_r22=0;
7283 IkReal gconst3=((3.14159265358979)+(((-1.0)*x584)));
7284 IkReal gconst4=x583;
7285 IkReal gconst5=((1.0)*new_r01);
7286 j3eval[0]=-1.0;
7287 j3eval[1]=-1.0;
7288 j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7289 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7290 {
7291 continue; // 3 cases reached
7292 
7293 } else
7294 {
7295 {
7296 IkReal j3array[1], cj3array[1], sj3array[1];
7297 bool j3valid[1]={false};
7298 _nj3 = 1;
7299 IkReal x586=((1.0)*new_r01);
7300 CheckValue<IkReal> x587 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x586))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
7301 if(!x587.valid){
7302 continue;
7303 }
7304 CheckValue<IkReal> x588=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x586)))),-1);
7305 if(!x588.valid){
7306 continue;
7307 }
7308 j3array[0]=((-1.5707963267949)+(x587.value)+(((1.5707963267949)*(x588.value))));
7309 sj3array[0]=IKsin(j3array[0]);
7310 cj3array[0]=IKcos(j3array[0]);
7311 if( j3array[0] > IKPI )
7312 {
7313  j3array[0]-=IK2PI;
7314 }
7315 else if( j3array[0] < -IKPI )
7316 { j3array[0]+=IK2PI;
7317 }
7318 j3valid[0] = true;
7319 for(int ij3 = 0; ij3 < 1; ++ij3)
7320 {
7321 if( !j3valid[ij3] )
7322 {
7323  continue;
7324 }
7325 _ij3[0] = ij3; _ij3[1] = -1;
7326 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7327 {
7328 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7329 {
7330  j3valid[iij3]=false; _ij3[1] = iij3; break;
7331 }
7332 }
7333 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7334 {
7335 IkReal evalcond[6];
7336 IkReal x589=IKsin(j3);
7337 IkReal x590=IKcos(j3);
7338 IkReal x591=(gconst4*x589);
7339 IkReal x592=(gconst4*x590);
7340 IkReal x593=((1.0)*x590);
7341 IkReal x594=(gconst5*x589);
7342 IkReal x595=(gconst5*x593);
7343 evalcond[0]=(gconst4+((new_r01*x590))+((new_r11*x589)));
7344 evalcond[1]=(x594+x592+new_r01);
7345 evalcond[2]=((((-1.0)*x595))+x591);
7346 evalcond[3]=((((-1.0)*new_r11*x593))+gconst5+((new_r01*x589)));
7347 evalcond[4]=((((-1.0)*x595))+x591+new_r11);
7348 evalcond[5]=((((-1.0)*x594))+(((-1.0)*x592)));
7349 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 )
7350 {
7351 continue;
7352 }
7353 }
7354 
7355 {
7356 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7357 vinfos[0].jointtype = 1;
7358 vinfos[0].foffset = j0;
7359 vinfos[0].indices[0] = _ij0[0];
7360 vinfos[0].indices[1] = _ij0[1];
7361 vinfos[0].maxsolutions = _nj0;
7362 vinfos[1].jointtype = 1;
7363 vinfos[1].foffset = j1;
7364 vinfos[1].indices[0] = _ij1[0];
7365 vinfos[1].indices[1] = _ij1[1];
7366 vinfos[1].maxsolutions = _nj1;
7367 vinfos[2].jointtype = 1;
7368 vinfos[2].foffset = j2;
7369 vinfos[2].indices[0] = _ij2[0];
7370 vinfos[2].indices[1] = _ij2[1];
7371 vinfos[2].maxsolutions = _nj2;
7372 vinfos[3].jointtype = 1;
7373 vinfos[3].foffset = j3;
7374 vinfos[3].indices[0] = _ij3[0];
7375 vinfos[3].indices[1] = _ij3[1];
7376 vinfos[3].maxsolutions = _nj3;
7377 vinfos[4].jointtype = 1;
7378 vinfos[4].foffset = j4;
7379 vinfos[4].indices[0] = _ij4[0];
7380 vinfos[4].indices[1] = _ij4[1];
7381 vinfos[4].maxsolutions = _nj4;
7382 vinfos[5].jointtype = 1;
7383 vinfos[5].foffset = j5;
7384 vinfos[5].indices[0] = _ij5[0];
7385 vinfos[5].indices[1] = _ij5[1];
7386 vinfos[5].maxsolutions = _nj5;
7387 std::vector<int> vfree(0);
7388 solutions.AddSolution(vinfos,vfree);
7389 }
7390 }
7391 }
7392 
7393 }
7394 
7395 }
7396 
7397 } else
7398 {
7399 {
7400 IkReal j3array[1], cj3array[1], sj3array[1];
7401 bool j3valid[1]={false};
7402 _nj3 = 1;
7403 CheckValue<IkReal> x596=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst4*gconst4)))+(((-1.0)*(gconst5*gconst5))))),-1);
7404 if(!x596.valid){
7405 continue;
7406 }
7407 CheckValue<IkReal> x597 = IKatan2WithCheck(IkReal((gconst5*new_r01)),IkReal((gconst4*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7408 if(!x597.valid){
7409 continue;
7410 }
7411 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x596.value)))+(x597.value));
7412 sj3array[0]=IKsin(j3array[0]);
7413 cj3array[0]=IKcos(j3array[0]);
7414 if( j3array[0] > IKPI )
7415 {
7416  j3array[0]-=IK2PI;
7417 }
7418 else if( j3array[0] < -IKPI )
7419 { j3array[0]+=IK2PI;
7420 }
7421 j3valid[0] = true;
7422 for(int ij3 = 0; ij3 < 1; ++ij3)
7423 {
7424 if( !j3valid[ij3] )
7425 {
7426  continue;
7427 }
7428 _ij3[0] = ij3; _ij3[1] = -1;
7429 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7430 {
7431 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7432 {
7433  j3valid[iij3]=false; _ij3[1] = iij3; break;
7434 }
7435 }
7436 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7437 {
7438 IkReal evalcond[6];
7439 IkReal x598=IKsin(j3);
7440 IkReal x599=IKcos(j3);
7441 IkReal x600=(gconst4*x598);
7442 IkReal x601=(gconst4*x599);
7443 IkReal x602=((1.0)*x599);
7444 IkReal x603=(gconst5*x598);
7445 IkReal x604=(gconst5*x602);
7446 evalcond[0]=(((new_r11*x598))+gconst4+((new_r01*x599)));
7447 evalcond[1]=(x603+x601+new_r01);
7448 evalcond[2]=(x600+(((-1.0)*x604)));
7449 evalcond[3]=((((-1.0)*new_r11*x602))+gconst5+((new_r01*x598)));
7450 evalcond[4]=(x600+(((-1.0)*x604))+new_r11);
7451 evalcond[5]=((((-1.0)*x601))+(((-1.0)*x603)));
7452 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 )
7453 {
7454 continue;
7455 }
7456 }
7457 
7458 {
7459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7460 vinfos[0].jointtype = 1;
7461 vinfos[0].foffset = j0;
7462 vinfos[0].indices[0] = _ij0[0];
7463 vinfos[0].indices[1] = _ij0[1];
7464 vinfos[0].maxsolutions = _nj0;
7465 vinfos[1].jointtype = 1;
7466 vinfos[1].foffset = j1;
7467 vinfos[1].indices[0] = _ij1[0];
7468 vinfos[1].indices[1] = _ij1[1];
7469 vinfos[1].maxsolutions = _nj1;
7470 vinfos[2].jointtype = 1;
7471 vinfos[2].foffset = j2;
7472 vinfos[2].indices[0] = _ij2[0];
7473 vinfos[2].indices[1] = _ij2[1];
7474 vinfos[2].maxsolutions = _nj2;
7475 vinfos[3].jointtype = 1;
7476 vinfos[3].foffset = j3;
7477 vinfos[3].indices[0] = _ij3[0];
7478 vinfos[3].indices[1] = _ij3[1];
7479 vinfos[3].maxsolutions = _nj3;
7480 vinfos[4].jointtype = 1;
7481 vinfos[4].foffset = j4;
7482 vinfos[4].indices[0] = _ij4[0];
7483 vinfos[4].indices[1] = _ij4[1];
7484 vinfos[4].maxsolutions = _nj4;
7485 vinfos[5].jointtype = 1;
7486 vinfos[5].foffset = j5;
7487 vinfos[5].indices[0] = _ij5[0];
7488 vinfos[5].indices[1] = _ij5[1];
7489 vinfos[5].maxsolutions = _nj5;
7490 std::vector<int> vfree(0);
7491 solutions.AddSolution(vinfos,vfree);
7492 }
7493 }
7494 }
7495 
7496 }
7497 
7498 }
7499 
7500 } else
7501 {
7502 {
7503 IkReal j3array[1], cj3array[1], sj3array[1];
7504 bool j3valid[1]={false};
7505 _nj3 = 1;
7506 CheckValue<IkReal> x605=IKPowWithIntegerCheck(IKsign((((gconst5*new_r01))+(((-1.0)*gconst4*new_r11)))),-1);
7507 if(!x605.valid){
7508 continue;
7509 }
7510 CheckValue<IkReal> x606 = IKatan2WithCheck(IkReal(gconst4*gconst4),IkReal(((-1.0)*gconst4*gconst5)),IKFAST_ATAN2_MAGTHRESH);
7511 if(!x606.valid){
7512 continue;
7513 }
7514 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x605.value)))+(x606.value));
7515 sj3array[0]=IKsin(j3array[0]);
7516 cj3array[0]=IKcos(j3array[0]);
7517 if( j3array[0] > IKPI )
7518 {
7519  j3array[0]-=IK2PI;
7520 }
7521 else if( j3array[0] < -IKPI )
7522 { j3array[0]+=IK2PI;
7523 }
7524 j3valid[0] = true;
7525 for(int ij3 = 0; ij3 < 1; ++ij3)
7526 {
7527 if( !j3valid[ij3] )
7528 {
7529  continue;
7530 }
7531 _ij3[0] = ij3; _ij3[1] = -1;
7532 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7533 {
7534 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7535 {
7536  j3valid[iij3]=false; _ij3[1] = iij3; break;
7537 }
7538 }
7539 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7540 {
7541 IkReal evalcond[6];
7542 IkReal x607=IKsin(j3);
7543 IkReal x608=IKcos(j3);
7544 IkReal x609=(gconst4*x607);
7545 IkReal x610=(gconst4*x608);
7546 IkReal x611=((1.0)*x608);
7547 IkReal x612=(gconst5*x607);
7548 IkReal x613=(gconst5*x611);
7549 evalcond[0]=(gconst4+((new_r11*x607))+((new_r01*x608)));
7550 evalcond[1]=(x610+x612+new_r01);
7551 evalcond[2]=((((-1.0)*x613))+x609);
7552 evalcond[3]=(gconst5+(((-1.0)*new_r11*x611))+((new_r01*x607)));
7553 evalcond[4]=((((-1.0)*x613))+x609+new_r11);
7554 evalcond[5]=((((-1.0)*x610))+(((-1.0)*x612)));
7555 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 )
7556 {
7557 continue;
7558 }
7559 }
7560 
7561 {
7562 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7563 vinfos[0].jointtype = 1;
7564 vinfos[0].foffset = j0;
7565 vinfos[0].indices[0] = _ij0[0];
7566 vinfos[0].indices[1] = _ij0[1];
7567 vinfos[0].maxsolutions = _nj0;
7568 vinfos[1].jointtype = 1;
7569 vinfos[1].foffset = j1;
7570 vinfos[1].indices[0] = _ij1[0];
7571 vinfos[1].indices[1] = _ij1[1];
7572 vinfos[1].maxsolutions = _nj1;
7573 vinfos[2].jointtype = 1;
7574 vinfos[2].foffset = j2;
7575 vinfos[2].indices[0] = _ij2[0];
7576 vinfos[2].indices[1] = _ij2[1];
7577 vinfos[2].maxsolutions = _nj2;
7578 vinfos[3].jointtype = 1;
7579 vinfos[3].foffset = j3;
7580 vinfos[3].indices[0] = _ij3[0];
7581 vinfos[3].indices[1] = _ij3[1];
7582 vinfos[3].maxsolutions = _nj3;
7583 vinfos[4].jointtype = 1;
7584 vinfos[4].foffset = j4;
7585 vinfos[4].indices[0] = _ij4[0];
7586 vinfos[4].indices[1] = _ij4[1];
7587 vinfos[4].maxsolutions = _nj4;
7588 vinfos[5].jointtype = 1;
7589 vinfos[5].foffset = j5;
7590 vinfos[5].indices[0] = _ij5[0];
7591 vinfos[5].indices[1] = _ij5[1];
7592 vinfos[5].maxsolutions = _nj5;
7593 std::vector<int> vfree(0);
7594 solutions.AddSolution(vinfos,vfree);
7595 }
7596 }
7597 }
7598 
7599 }
7600 
7601 }
7602 
7603 }
7604 } while(0);
7605 if( bgotonextstatement )
7606 {
7607 bool bgotonextstatement = true;
7608 do
7609 {
7610 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
7611 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7612 {
7613 bgotonextstatement=false;
7614 {
7615 IkReal j3eval[1];
7616 IkReal x614=((-1.0)*new_r11);
7617 CheckValue<IkReal> x616 = IKatan2WithCheck(IkReal(x614),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7618 if(!x616.valid){
7619 continue;
7620 }
7621 IkReal x615=((1.0)*(x616.value));
7622 sj4=0;
7623 cj4=1.0;
7624 j4=0;
7625 sj5=gconst4;
7626 cj5=gconst5;
7627 j5=((3.14159265)+(((-1.0)*x615)));
7628 new_r01=0;
7629 new_r10=0;
7630 IkReal gconst3=((3.14159265358979)+(((-1.0)*x615)));
7631 IkReal x617 = new_r11*new_r11;
7632 if(IKabs(x617)==0){
7633 continue;
7634 }
7635 IkReal gconst4=(x614*(pow(x617,-0.5)));
7636 IkReal gconst5=0;
7637 j3eval[0]=new_r00;
7638 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7639 {
7640 {
7641 IkReal j3eval[1];
7642 IkReal x618=((-1.0)*new_r11);
7643 CheckValue<IkReal> x620 = IKatan2WithCheck(IkReal(x618),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7644 if(!x620.valid){
7645 continue;
7646 }
7647 IkReal x619=((1.0)*(x620.value));
7648 sj4=0;
7649 cj4=1.0;
7650 j4=0;
7651 sj5=gconst4;
7652 cj5=gconst5;
7653 j5=((3.14159265)+(((-1.0)*x619)));
7654 new_r01=0;
7655 new_r10=0;
7656 IkReal gconst3=((3.14159265358979)+(((-1.0)*x619)));
7657 IkReal x621 = new_r11*new_r11;
7658 if(IKabs(x621)==0){
7659 continue;
7660 }
7661 IkReal gconst4=(x618*(pow(x621,-0.5)));
7662 IkReal gconst5=0;
7663 j3eval[0]=new_r11;
7664 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7665 {
7666 {
7667 IkReal j3array[2], cj3array[2], sj3array[2];
7668 bool j3valid[2]={false};
7669 _nj3 = 2;
7670 CheckValue<IkReal> x622=IKPowWithIntegerCheck(gconst4,-1);
7671 if(!x622.valid){
7672 continue;
7673 }
7674 sj3array[0]=((-1.0)*new_r00*(x622.value));
7675 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7676 {
7677  j3valid[0] = j3valid[1] = true;
7678  j3array[0] = IKasin(sj3array[0]);
7679  cj3array[0] = IKcos(j3array[0]);
7680  sj3array[1] = sj3array[0];
7681  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7682  cj3array[1] = -cj3array[0];
7683 }
7684 else if( isnan(sj3array[0]) )
7685 {
7686  // probably any value will work
7687  j3valid[0] = true;
7688  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7689 }
7690 for(int ij3 = 0; ij3 < 2; ++ij3)
7691 {
7692 if( !j3valid[ij3] )
7693 {
7694  continue;
7695 }
7696 _ij3[0] = ij3; _ij3[1] = -1;
7697 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7698 {
7699 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7700 {
7701  j3valid[iij3]=false; _ij3[1] = iij3; break;
7702 }
7703 }
7704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7705 {
7706 IkReal evalcond[6];
7707 IkReal x623=IKcos(j3);
7708 IkReal x624=IKsin(j3);
7709 evalcond[0]=(gconst4*x623);
7710 evalcond[1]=(new_r00*x623);
7711 evalcond[2]=((-1.0)*new_r11*x623);
7712 evalcond[3]=(gconst4+((new_r00*x624)));
7713 evalcond[4]=(gconst4+((new_r11*x624)));
7714 evalcond[5]=(((gconst4*x624))+new_r11);
7715 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 )
7716 {
7717 continue;
7718 }
7719 }
7720 
7721 {
7722 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7723 vinfos[0].jointtype = 1;
7724 vinfos[0].foffset = j0;
7725 vinfos[0].indices[0] = _ij0[0];
7726 vinfos[0].indices[1] = _ij0[1];
7727 vinfos[0].maxsolutions = _nj0;
7728 vinfos[1].jointtype = 1;
7729 vinfos[1].foffset = j1;
7730 vinfos[1].indices[0] = _ij1[0];
7731 vinfos[1].indices[1] = _ij1[1];
7732 vinfos[1].maxsolutions = _nj1;
7733 vinfos[2].jointtype = 1;
7734 vinfos[2].foffset = j2;
7735 vinfos[2].indices[0] = _ij2[0];
7736 vinfos[2].indices[1] = _ij2[1];
7737 vinfos[2].maxsolutions = _nj2;
7738 vinfos[3].jointtype = 1;
7739 vinfos[3].foffset = j3;
7740 vinfos[3].indices[0] = _ij3[0];
7741 vinfos[3].indices[1] = _ij3[1];
7742 vinfos[3].maxsolutions = _nj3;
7743 vinfos[4].jointtype = 1;
7744 vinfos[4].foffset = j4;
7745 vinfos[4].indices[0] = _ij4[0];
7746 vinfos[4].indices[1] = _ij4[1];
7747 vinfos[4].maxsolutions = _nj4;
7748 vinfos[5].jointtype = 1;
7749 vinfos[5].foffset = j5;
7750 vinfos[5].indices[0] = _ij5[0];
7751 vinfos[5].indices[1] = _ij5[1];
7752 vinfos[5].maxsolutions = _nj5;
7753 std::vector<int> vfree(0);
7754 solutions.AddSolution(vinfos,vfree);
7755 }
7756 }
7757 }
7758 
7759 } else
7760 {
7761 {
7762 IkReal j3array[2], cj3array[2], sj3array[2];
7763 bool j3valid[2]={false};
7764 _nj3 = 2;
7765 CheckValue<IkReal> x625=IKPowWithIntegerCheck(new_r11,-1);
7766 if(!x625.valid){
7767 continue;
7768 }
7769 sj3array[0]=((-1.0)*gconst4*(x625.value));
7770 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7771 {
7772  j3valid[0] = j3valid[1] = true;
7773  j3array[0] = IKasin(sj3array[0]);
7774  cj3array[0] = IKcos(j3array[0]);
7775  sj3array[1] = sj3array[0];
7776  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7777  cj3array[1] = -cj3array[0];
7778 }
7779 else if( isnan(sj3array[0]) )
7780 {
7781  // probably any value will work
7782  j3valid[0] = true;
7783  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7784 }
7785 for(int ij3 = 0; ij3 < 2; ++ij3)
7786 {
7787 if( !j3valid[ij3] )
7788 {
7789  continue;
7790 }
7791 _ij3[0] = ij3; _ij3[1] = -1;
7792 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7793 {
7794 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7795 {
7796  j3valid[iij3]=false; _ij3[1] = iij3; break;
7797 }
7798 }
7799 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7800 {
7801 IkReal evalcond[6];
7802 IkReal x626=IKcos(j3);
7803 IkReal x627=IKsin(j3);
7804 IkReal x628=(gconst4*x627);
7805 evalcond[0]=(gconst4*x626);
7806 evalcond[1]=(new_r00*x626);
7807 evalcond[2]=((-1.0)*new_r11*x626);
7808 evalcond[3]=(gconst4+((new_r00*x627)));
7809 evalcond[4]=(x628+new_r00);
7810 evalcond[5]=(x628+new_r11);
7811 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 )
7812 {
7813 continue;
7814 }
7815 }
7816 
7817 {
7818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7819 vinfos[0].jointtype = 1;
7820 vinfos[0].foffset = j0;
7821 vinfos[0].indices[0] = _ij0[0];
7822 vinfos[0].indices[1] = _ij0[1];
7823 vinfos[0].maxsolutions = _nj0;
7824 vinfos[1].jointtype = 1;
7825 vinfos[1].foffset = j1;
7826 vinfos[1].indices[0] = _ij1[0];
7827 vinfos[1].indices[1] = _ij1[1];
7828 vinfos[1].maxsolutions = _nj1;
7829 vinfos[2].jointtype = 1;
7830 vinfos[2].foffset = j2;
7831 vinfos[2].indices[0] = _ij2[0];
7832 vinfos[2].indices[1] = _ij2[1];
7833 vinfos[2].maxsolutions = _nj2;
7834 vinfos[3].jointtype = 1;
7835 vinfos[3].foffset = j3;
7836 vinfos[3].indices[0] = _ij3[0];
7837 vinfos[3].indices[1] = _ij3[1];
7838 vinfos[3].maxsolutions = _nj3;
7839 vinfos[4].jointtype = 1;
7840 vinfos[4].foffset = j4;
7841 vinfos[4].indices[0] = _ij4[0];
7842 vinfos[4].indices[1] = _ij4[1];
7843 vinfos[4].maxsolutions = _nj4;
7844 vinfos[5].jointtype = 1;
7845 vinfos[5].foffset = j5;
7846 vinfos[5].indices[0] = _ij5[0];
7847 vinfos[5].indices[1] = _ij5[1];
7848 vinfos[5].maxsolutions = _nj5;
7849 std::vector<int> vfree(0);
7850 solutions.AddSolution(vinfos,vfree);
7851 }
7852 }
7853 }
7854 
7855 }
7856 
7857 }
7858 
7859 } else
7860 {
7861 {
7862 IkReal j3array[2], cj3array[2], sj3array[2];
7863 bool j3valid[2]={false};
7864 _nj3 = 2;
7865 CheckValue<IkReal> x629=IKPowWithIntegerCheck(new_r00,-1);
7866 if(!x629.valid){
7867 continue;
7868 }
7869 sj3array[0]=((-1.0)*gconst4*(x629.value));
7870 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7871 {
7872  j3valid[0] = j3valid[1] = true;
7873  j3array[0] = IKasin(sj3array[0]);
7874  cj3array[0] = IKcos(j3array[0]);
7875  sj3array[1] = sj3array[0];
7876  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7877  cj3array[1] = -cj3array[0];
7878 }
7879 else if( isnan(sj3array[0]) )
7880 {
7881  // probably any value will work
7882  j3valid[0] = true;
7883  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7884 }
7885 for(int ij3 = 0; ij3 < 2; ++ij3)
7886 {
7887 if( !j3valid[ij3] )
7888 {
7889  continue;
7890 }
7891 _ij3[0] = ij3; _ij3[1] = -1;
7892 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7893 {
7894 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7895 {
7896  j3valid[iij3]=false; _ij3[1] = iij3; break;
7897 }
7898 }
7899 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7900 {
7901 IkReal evalcond[6];
7902 IkReal x630=IKcos(j3);
7903 IkReal x631=IKsin(j3);
7904 IkReal x632=(gconst4*x631);
7905 evalcond[0]=(gconst4*x630);
7906 evalcond[1]=(new_r00*x630);
7907 evalcond[2]=((-1.0)*new_r11*x630);
7908 evalcond[3]=(gconst4+((new_r11*x631)));
7909 evalcond[4]=(x632+new_r00);
7910 evalcond[5]=(x632+new_r11);
7911 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 )
7912 {
7913 continue;
7914 }
7915 }
7916 
7917 {
7918 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7919 vinfos[0].jointtype = 1;
7920 vinfos[0].foffset = j0;
7921 vinfos[0].indices[0] = _ij0[0];
7922 vinfos[0].indices[1] = _ij0[1];
7923 vinfos[0].maxsolutions = _nj0;
7924 vinfos[1].jointtype = 1;
7925 vinfos[1].foffset = j1;
7926 vinfos[1].indices[0] = _ij1[0];
7927 vinfos[1].indices[1] = _ij1[1];
7928 vinfos[1].maxsolutions = _nj1;
7929 vinfos[2].jointtype = 1;
7930 vinfos[2].foffset = j2;
7931 vinfos[2].indices[0] = _ij2[0];
7932 vinfos[2].indices[1] = _ij2[1];
7933 vinfos[2].maxsolutions = _nj2;
7934 vinfos[3].jointtype = 1;
7935 vinfos[3].foffset = j3;
7936 vinfos[3].indices[0] = _ij3[0];
7937 vinfos[3].indices[1] = _ij3[1];
7938 vinfos[3].maxsolutions = _nj3;
7939 vinfos[4].jointtype = 1;
7940 vinfos[4].foffset = j4;
7941 vinfos[4].indices[0] = _ij4[0];
7942 vinfos[4].indices[1] = _ij4[1];
7943 vinfos[4].maxsolutions = _nj4;
7944 vinfos[5].jointtype = 1;
7945 vinfos[5].foffset = j5;
7946 vinfos[5].indices[0] = _ij5[0];
7947 vinfos[5].indices[1] = _ij5[1];
7948 vinfos[5].maxsolutions = _nj5;
7949 std::vector<int> vfree(0);
7950 solutions.AddSolution(vinfos,vfree);
7951 }
7952 }
7953 }
7954 
7955 }
7956 
7957 }
7958 
7959 }
7960 } while(0);
7961 if( bgotonextstatement )
7962 {
7963 bool bgotonextstatement = true;
7964 do
7965 {
7966 evalcond[0]=IKabs(new_r11);
7967 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7968 {
7969 bgotonextstatement=false;
7970 {
7971 IkReal j3eval[1];
7972 CheckValue<IkReal> x634 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7973 if(!x634.valid){
7974 continue;
7975 }
7976 IkReal x633=((1.0)*(x634.value));
7977 sj4=0;
7978 cj4=1.0;
7979 j4=0;
7980 sj5=gconst4;
7981 cj5=gconst5;
7982 j5=((3.14159265)+(((-1.0)*x633)));
7983 new_r11=0;
7984 IkReal gconst3=((3.14159265358979)+(((-1.0)*x633)));
7985 IkReal gconst4=0;
7986 IkReal x635 = new_r01*new_r01;
7987 if(IKabs(x635)==0){
7988 continue;
7989 }
7990 IkReal gconst5=((1.0)*new_r01*(pow(x635,-0.5)));
7991 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7992 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7993 {
7994 {
7995 IkReal j3eval[1];
7996 CheckValue<IkReal> x637 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7997 if(!x637.valid){
7998 continue;
7999 }
8000 IkReal x636=((1.0)*(x637.value));
8001 sj4=0;
8002 cj4=1.0;
8003 j4=0;
8004 sj5=gconst4;
8005 cj5=gconst5;
8006 j5=((3.14159265)+(((-1.0)*x636)));
8007 new_r11=0;
8008 IkReal gconst3=((3.14159265358979)+(((-1.0)*x636)));
8009 IkReal gconst4=0;
8010 IkReal x638 = new_r01*new_r01;
8011 if(IKabs(x638)==0){
8012 continue;
8013 }
8014 IkReal gconst5=((1.0)*new_r01*(pow(x638,-0.5)));
8015 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
8016 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8017 {
8018 {
8019 IkReal j3eval[1];
8020 CheckValue<IkReal> x640 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
8021 if(!x640.valid){
8022 continue;
8023 }
8024 IkReal x639=((1.0)*(x640.value));
8025 sj4=0;
8026 cj4=1.0;
8027 j4=0;
8028 sj5=gconst4;
8029 cj5=gconst5;
8030 j5=((3.14159265)+(((-1.0)*x639)));
8031 new_r11=0;
8032 IkReal gconst3=((3.14159265358979)+(((-1.0)*x639)));
8033 IkReal gconst4=0;
8034 IkReal x641 = new_r01*new_r01;
8035 if(IKabs(x641)==0){
8036 continue;
8037 }
8038 IkReal gconst5=((1.0)*new_r01*(pow(x641,-0.5)));
8039 j3eval[0]=new_r01;
8040 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8041 {
8042 continue; // 3 cases reached
8043 
8044 } else
8045 {
8046 {
8047 IkReal j3array[1], cj3array[1], sj3array[1];
8048 bool j3valid[1]={false};
8049 _nj3 = 1;
8050 CheckValue<IkReal> x642=IKPowWithIntegerCheck(new_r01,-1);
8051 if(!x642.valid){
8052 continue;
8053 }
8054 CheckValue<IkReal> x643=IKPowWithIntegerCheck(gconst5,-1);
8055 if(!x643.valid){
8056 continue;
8057 }
8058 if( IKabs(((-1.0)*gconst5*(x642.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x643.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst5*(x642.value)))+IKsqr((new_r00*(x643.value)))-1) <= IKFAST_SINCOS_THRESH )
8059  continue;
8060 j3array[0]=IKatan2(((-1.0)*gconst5*(x642.value)), (new_r00*(x643.value)));
8061 sj3array[0]=IKsin(j3array[0]);
8062 cj3array[0]=IKcos(j3array[0]);
8063 if( j3array[0] > IKPI )
8064 {
8065  j3array[0]-=IK2PI;
8066 }
8067 else if( j3array[0] < -IKPI )
8068 { j3array[0]+=IK2PI;
8069 }
8070 j3valid[0] = true;
8071 for(int ij3 = 0; ij3 < 1; ++ij3)
8072 {
8073 if( !j3valid[ij3] )
8074 {
8075  continue;
8076 }
8077 _ij3[0] = ij3; _ij3[1] = -1;
8078 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8079 {
8080 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8081 {
8082  j3valid[iij3]=false; _ij3[1] = iij3; break;
8083 }
8084 }
8085 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8086 {
8087 IkReal evalcond[8];
8088 IkReal x644=IKcos(j3);
8089 IkReal x645=IKsin(j3);
8090 IkReal x646=((1.0)*gconst5);
8091 evalcond[0]=(new_r01*x644);
8092 evalcond[1]=((-1.0)*gconst5*x644);
8093 evalcond[2]=(gconst5+((new_r01*x645)));
8094 evalcond[3]=(new_r01+((gconst5*x645)));
8095 evalcond[4]=((((-1.0)*x644*x646))+new_r00);
8096 evalcond[5]=((((-1.0)*x645*x646))+new_r10);
8097 evalcond[6]=((((-1.0)*new_r10*x644))+((new_r00*x645)));
8098 evalcond[7]=((((-1.0)*x646))+((new_r10*x645))+((new_r00*x644)));
8099 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 )
8100 {
8101 continue;
8102 }
8103 }
8104 
8105 {
8106 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8107 vinfos[0].jointtype = 1;
8108 vinfos[0].foffset = j0;
8109 vinfos[0].indices[0] = _ij0[0];
8110 vinfos[0].indices[1] = _ij0[1];
8111 vinfos[0].maxsolutions = _nj0;
8112 vinfos[1].jointtype = 1;
8113 vinfos[1].foffset = j1;
8114 vinfos[1].indices[0] = _ij1[0];
8115 vinfos[1].indices[1] = _ij1[1];
8116 vinfos[1].maxsolutions = _nj1;
8117 vinfos[2].jointtype = 1;
8118 vinfos[2].foffset = j2;
8119 vinfos[2].indices[0] = _ij2[0];
8120 vinfos[2].indices[1] = _ij2[1];
8121 vinfos[2].maxsolutions = _nj2;
8122 vinfos[3].jointtype = 1;
8123 vinfos[3].foffset = j3;
8124 vinfos[3].indices[0] = _ij3[0];
8125 vinfos[3].indices[1] = _ij3[1];
8126 vinfos[3].maxsolutions = _nj3;
8127 vinfos[4].jointtype = 1;
8128 vinfos[4].foffset = j4;
8129 vinfos[4].indices[0] = _ij4[0];
8130 vinfos[4].indices[1] = _ij4[1];
8131 vinfos[4].maxsolutions = _nj4;
8132 vinfos[5].jointtype = 1;
8133 vinfos[5].foffset = j5;
8134 vinfos[5].indices[0] = _ij5[0];
8135 vinfos[5].indices[1] = _ij5[1];
8136 vinfos[5].maxsolutions = _nj5;
8137 std::vector<int> vfree(0);
8138 solutions.AddSolution(vinfos,vfree);
8139 }
8140 }
8141 }
8142 
8143 }
8144 
8145 }
8146 
8147 } else
8148 {
8149 {
8150 IkReal j3array[1], cj3array[1], sj3array[1];
8151 bool j3valid[1]={false};
8152 _nj3 = 1;
8153 CheckValue<IkReal> x647 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8154 if(!x647.valid){
8155 continue;
8156 }
8157 CheckValue<IkReal> x648=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8158 if(!x648.valid){
8159 continue;
8160 }
8161 j3array[0]=((-1.5707963267949)+(x647.value)+(((1.5707963267949)*(x648.value))));
8162 sj3array[0]=IKsin(j3array[0]);
8163 cj3array[0]=IKcos(j3array[0]);
8164 if( j3array[0] > IKPI )
8165 {
8166  j3array[0]-=IK2PI;
8167 }
8168 else if( j3array[0] < -IKPI )
8169 { j3array[0]+=IK2PI;
8170 }
8171 j3valid[0] = true;
8172 for(int ij3 = 0; ij3 < 1; ++ij3)
8173 {
8174 if( !j3valid[ij3] )
8175 {
8176  continue;
8177 }
8178 _ij3[0] = ij3; _ij3[1] = -1;
8179 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8180 {
8181 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8182 {
8183  j3valid[iij3]=false; _ij3[1] = iij3; break;
8184 }
8185 }
8186 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8187 {
8188 IkReal evalcond[8];
8189 IkReal x649=IKcos(j3);
8190 IkReal x650=IKsin(j3);
8191 IkReal x651=((1.0)*gconst5);
8192 evalcond[0]=(new_r01*x649);
8193 evalcond[1]=((-1.0)*gconst5*x649);
8194 evalcond[2]=(gconst5+((new_r01*x650)));
8195 evalcond[3]=(new_r01+((gconst5*x650)));
8196 evalcond[4]=((((-1.0)*x649*x651))+new_r00);
8197 evalcond[5]=((((-1.0)*x650*x651))+new_r10);
8198 evalcond[6]=((((-1.0)*new_r10*x649))+((new_r00*x650)));
8199 evalcond[7]=((((-1.0)*x651))+((new_r00*x649))+((new_r10*x650)));
8200 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 )
8201 {
8202 continue;
8203 }
8204 }
8205 
8206 {
8207 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8208 vinfos[0].jointtype = 1;
8209 vinfos[0].foffset = j0;
8210 vinfos[0].indices[0] = _ij0[0];
8211 vinfos[0].indices[1] = _ij0[1];
8212 vinfos[0].maxsolutions = _nj0;
8213 vinfos[1].jointtype = 1;
8214 vinfos[1].foffset = j1;
8215 vinfos[1].indices[0] = _ij1[0];
8216 vinfos[1].indices[1] = _ij1[1];
8217 vinfos[1].maxsolutions = _nj1;
8218 vinfos[2].jointtype = 1;
8219 vinfos[2].foffset = j2;
8220 vinfos[2].indices[0] = _ij2[0];
8221 vinfos[2].indices[1] = _ij2[1];
8222 vinfos[2].maxsolutions = _nj2;
8223 vinfos[3].jointtype = 1;
8224 vinfos[3].foffset = j3;
8225 vinfos[3].indices[0] = _ij3[0];
8226 vinfos[3].indices[1] = _ij3[1];
8227 vinfos[3].maxsolutions = _nj3;
8228 vinfos[4].jointtype = 1;
8229 vinfos[4].foffset = j4;
8230 vinfos[4].indices[0] = _ij4[0];
8231 vinfos[4].indices[1] = _ij4[1];
8232 vinfos[4].maxsolutions = _nj4;
8233 vinfos[5].jointtype = 1;
8234 vinfos[5].foffset = j5;
8235 vinfos[5].indices[0] = _ij5[0];
8236 vinfos[5].indices[1] = _ij5[1];
8237 vinfos[5].maxsolutions = _nj5;
8238 std::vector<int> vfree(0);
8239 solutions.AddSolution(vinfos,vfree);
8240 }
8241 }
8242 }
8243 
8244 }
8245 
8246 }
8247 
8248 } else
8249 {
8250 {
8251 IkReal j3array[1], cj3array[1], sj3array[1];
8252 bool j3valid[1]={false};
8253 _nj3 = 1;
8254 CheckValue<IkReal> x652 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8255 if(!x652.valid){
8256 continue;
8257 }
8258 CheckValue<IkReal> x653=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8259 if(!x653.valid){
8260 continue;
8261 }
8262 j3array[0]=((-1.5707963267949)+(x652.value)+(((1.5707963267949)*(x653.value))));
8263 sj3array[0]=IKsin(j3array[0]);
8264 cj3array[0]=IKcos(j3array[0]);
8265 if( j3array[0] > IKPI )
8266 {
8267  j3array[0]-=IK2PI;
8268 }
8269 else if( j3array[0] < -IKPI )
8270 { j3array[0]+=IK2PI;
8271 }
8272 j3valid[0] = true;
8273 for(int ij3 = 0; ij3 < 1; ++ij3)
8274 {
8275 if( !j3valid[ij3] )
8276 {
8277  continue;
8278 }
8279 _ij3[0] = ij3; _ij3[1] = -1;
8280 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8281 {
8282 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8283 {
8284  j3valid[iij3]=false; _ij3[1] = iij3; break;
8285 }
8286 }
8287 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8288 {
8289 IkReal evalcond[8];
8290 IkReal x654=IKcos(j3);
8291 IkReal x655=IKsin(j3);
8292 IkReal x656=((1.0)*gconst5);
8293 evalcond[0]=(new_r01*x654);
8294 evalcond[1]=((-1.0)*gconst5*x654);
8295 evalcond[2]=(gconst5+((new_r01*x655)));
8296 evalcond[3]=(new_r01+((gconst5*x655)));
8297 evalcond[4]=(new_r00+(((-1.0)*x654*x656)));
8298 evalcond[5]=((((-1.0)*x655*x656))+new_r10);
8299 evalcond[6]=((((-1.0)*new_r10*x654))+((new_r00*x655)));
8300 evalcond[7]=((((-1.0)*x656))+((new_r10*x655))+((new_r00*x654)));
8301 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 )
8302 {
8303 continue;
8304 }
8305 }
8306 
8307 {
8308 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8309 vinfos[0].jointtype = 1;
8310 vinfos[0].foffset = j0;
8311 vinfos[0].indices[0] = _ij0[0];
8312 vinfos[0].indices[1] = _ij0[1];
8313 vinfos[0].maxsolutions = _nj0;
8314 vinfos[1].jointtype = 1;
8315 vinfos[1].foffset = j1;
8316 vinfos[1].indices[0] = _ij1[0];
8317 vinfos[1].indices[1] = _ij1[1];
8318 vinfos[1].maxsolutions = _nj1;
8319 vinfos[2].jointtype = 1;
8320 vinfos[2].foffset = j2;
8321 vinfos[2].indices[0] = _ij2[0];
8322 vinfos[2].indices[1] = _ij2[1];
8323 vinfos[2].maxsolutions = _nj2;
8324 vinfos[3].jointtype = 1;
8325 vinfos[3].foffset = j3;
8326 vinfos[3].indices[0] = _ij3[0];
8327 vinfos[3].indices[1] = _ij3[1];
8328 vinfos[3].maxsolutions = _nj3;
8329 vinfos[4].jointtype = 1;
8330 vinfos[4].foffset = j4;
8331 vinfos[4].indices[0] = _ij4[0];
8332 vinfos[4].indices[1] = _ij4[1];
8333 vinfos[4].maxsolutions = _nj4;
8334 vinfos[5].jointtype = 1;
8335 vinfos[5].foffset = j5;
8336 vinfos[5].indices[0] = _ij5[0];
8337 vinfos[5].indices[1] = _ij5[1];
8338 vinfos[5].maxsolutions = _nj5;
8339 std::vector<int> vfree(0);
8340 solutions.AddSolution(vinfos,vfree);
8341 }
8342 }
8343 }
8344 
8345 }
8346 
8347 }
8348 
8349 }
8350 } while(0);
8351 if( bgotonextstatement )
8352 {
8353 bool bgotonextstatement = true;
8354 do
8355 {
8356 if( 1 )
8357 {
8358 bgotonextstatement=false;
8359 continue; // branch miss [j3]
8360 
8361 }
8362 } while(0);
8363 if( bgotonextstatement )
8364 {
8365 }
8366 }
8367 }
8368 }
8369 }
8370 }
8371 
8372 } else
8373 {
8374 {
8375 IkReal j3array[1], cj3array[1], sj3array[1];
8376 bool j3valid[1]={false};
8377 _nj3 = 1;
8378 IkReal x657=((1.0)*new_r01);
8379 CheckValue<IkReal> x658 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x657))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
8380 if(!x658.valid){
8381 continue;
8382 }
8383 CheckValue<IkReal> x659=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x657)))),-1);
8384 if(!x659.valid){
8385 continue;
8386 }
8387 j3array[0]=((-1.5707963267949)+(x658.value)+(((1.5707963267949)*(x659.value))));
8388 sj3array[0]=IKsin(j3array[0]);
8389 cj3array[0]=IKcos(j3array[0]);
8390 if( j3array[0] > IKPI )
8391 {
8392  j3array[0]-=IK2PI;
8393 }
8394 else if( j3array[0] < -IKPI )
8395 { j3array[0]+=IK2PI;
8396 }
8397 j3valid[0] = true;
8398 for(int ij3 = 0; ij3 < 1; ++ij3)
8399 {
8400 if( !j3valid[ij3] )
8401 {
8402  continue;
8403 }
8404 _ij3[0] = ij3; _ij3[1] = -1;
8405 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8406 {
8407 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8408 {
8409  j3valid[iij3]=false; _ij3[1] = iij3; break;
8410 }
8411 }
8412 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8413 {
8414 IkReal evalcond[8];
8415 IkReal x660=IKsin(j3);
8416 IkReal x661=IKcos(j3);
8417 IkReal x662=((1.0)*gconst5);
8418 IkReal x663=(gconst4*x660);
8419 IkReal x664=(gconst4*x661);
8420 IkReal x665=((1.0)*x661);
8421 IkReal x666=(x661*x662);
8422 evalcond[0]=(gconst4+((new_r11*x660))+((new_r01*x661)));
8423 evalcond[1]=(((gconst5*x660))+x664+new_r01);
8424 evalcond[2]=(gconst4+(((-1.0)*new_r10*x665))+((new_r00*x660)));
8425 evalcond[3]=(gconst5+(((-1.0)*new_r11*x665))+((new_r01*x660)));
8426 evalcond[4]=((((-1.0)*x666))+x663+new_r00);
8427 evalcond[5]=((((-1.0)*x666))+x663+new_r11);
8428 evalcond[6]=(((new_r10*x660))+(((-1.0)*x662))+((new_r00*x661)));
8429 evalcond[7]=((((-1.0)*x660*x662))+(((-1.0)*x664))+new_r10);
8430 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 )
8431 {
8432 continue;
8433 }
8434 }
8435 
8436 {
8437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8438 vinfos[0].jointtype = 1;
8439 vinfos[0].foffset = j0;
8440 vinfos[0].indices[0] = _ij0[0];
8441 vinfos[0].indices[1] = _ij0[1];
8442 vinfos[0].maxsolutions = _nj0;
8443 vinfos[1].jointtype = 1;
8444 vinfos[1].foffset = j1;
8445 vinfos[1].indices[0] = _ij1[0];
8446 vinfos[1].indices[1] = _ij1[1];
8447 vinfos[1].maxsolutions = _nj1;
8448 vinfos[2].jointtype = 1;
8449 vinfos[2].foffset = j2;
8450 vinfos[2].indices[0] = _ij2[0];
8451 vinfos[2].indices[1] = _ij2[1];
8452 vinfos[2].maxsolutions = _nj2;
8453 vinfos[3].jointtype = 1;
8454 vinfos[3].foffset = j3;
8455 vinfos[3].indices[0] = _ij3[0];
8456 vinfos[3].indices[1] = _ij3[1];
8457 vinfos[3].maxsolutions = _nj3;
8458 vinfos[4].jointtype = 1;
8459 vinfos[4].foffset = j4;
8460 vinfos[4].indices[0] = _ij4[0];
8461 vinfos[4].indices[1] = _ij4[1];
8462 vinfos[4].maxsolutions = _nj4;
8463 vinfos[5].jointtype = 1;
8464 vinfos[5].foffset = j5;
8465 vinfos[5].indices[0] = _ij5[0];
8466 vinfos[5].indices[1] = _ij5[1];
8467 vinfos[5].maxsolutions = _nj5;
8468 std::vector<int> vfree(0);
8469 solutions.AddSolution(vinfos,vfree);
8470 }
8471 }
8472 }
8473 
8474 }
8475 
8476 }
8477 
8478 } else
8479 {
8480 {
8481 IkReal j3array[1], cj3array[1], sj3array[1];
8482 bool j3valid[1]={false};
8483 _nj3 = 1;
8484 IkReal x667=((1.0)*gconst4);
8485 CheckValue<IkReal> x668 = IKatan2WithCheck(IkReal(((gconst4*gconst4)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst5*x667)))),IKFAST_ATAN2_MAGTHRESH);
8486 if(!x668.valid){
8487 continue;
8488 }
8489 CheckValue<IkReal> x669=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst5*new_r10))+(((-1.0)*new_r00*x667)))),-1);
8490 if(!x669.valid){
8491 continue;
8492 }
8493 j3array[0]=((-1.5707963267949)+(x668.value)+(((1.5707963267949)*(x669.value))));
8494 sj3array[0]=IKsin(j3array[0]);
8495 cj3array[0]=IKcos(j3array[0]);
8496 if( j3array[0] > IKPI )
8497 {
8498  j3array[0]-=IK2PI;
8499 }
8500 else if( j3array[0] < -IKPI )
8501 { j3array[0]+=IK2PI;
8502 }
8503 j3valid[0] = true;
8504 for(int ij3 = 0; ij3 < 1; ++ij3)
8505 {
8506 if( !j3valid[ij3] )
8507 {
8508  continue;
8509 }
8510 _ij3[0] = ij3; _ij3[1] = -1;
8511 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8512 {
8513 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8514 {
8515  j3valid[iij3]=false; _ij3[1] = iij3; break;
8516 }
8517 }
8518 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8519 {
8520 IkReal evalcond[8];
8521 IkReal x670=IKsin(j3);
8522 IkReal x671=IKcos(j3);
8523 IkReal x672=((1.0)*gconst5);
8524 IkReal x673=(gconst4*x670);
8525 IkReal x674=(gconst4*x671);
8526 IkReal x675=((1.0)*x671);
8527 IkReal x676=(x671*x672);
8528 evalcond[0]=(((new_r11*x670))+((new_r01*x671))+gconst4);
8529 evalcond[1]=(((gconst5*x670))+x674+new_r01);
8530 evalcond[2]=(((new_r00*x670))+gconst4+(((-1.0)*new_r10*x675)));
8531 evalcond[3]=(((new_r01*x670))+gconst5+(((-1.0)*new_r11*x675)));
8532 evalcond[4]=(x673+new_r00+(((-1.0)*x676)));
8533 evalcond[5]=(x673+new_r11+(((-1.0)*x676)));
8534 evalcond[6]=(((new_r00*x671))+((new_r10*x670))+(((-1.0)*x672)));
8535 evalcond[7]=((((-1.0)*x670*x672))+(((-1.0)*x674))+new_r10);
8536 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 )
8537 {
8538 continue;
8539 }
8540 }
8541 
8542 {
8543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8544 vinfos[0].jointtype = 1;
8545 vinfos[0].foffset = j0;
8546 vinfos[0].indices[0] = _ij0[0];
8547 vinfos[0].indices[1] = _ij0[1];
8548 vinfos[0].maxsolutions = _nj0;
8549 vinfos[1].jointtype = 1;
8550 vinfos[1].foffset = j1;
8551 vinfos[1].indices[0] = _ij1[0];
8552 vinfos[1].indices[1] = _ij1[1];
8553 vinfos[1].maxsolutions = _nj1;
8554 vinfos[2].jointtype = 1;
8555 vinfos[2].foffset = j2;
8556 vinfos[2].indices[0] = _ij2[0];
8557 vinfos[2].indices[1] = _ij2[1];
8558 vinfos[2].maxsolutions = _nj2;
8559 vinfos[3].jointtype = 1;
8560 vinfos[3].foffset = j3;
8561 vinfos[3].indices[0] = _ij3[0];
8562 vinfos[3].indices[1] = _ij3[1];
8563 vinfos[3].maxsolutions = _nj3;
8564 vinfos[4].jointtype = 1;
8565 vinfos[4].foffset = j4;
8566 vinfos[4].indices[0] = _ij4[0];
8567 vinfos[4].indices[1] = _ij4[1];
8568 vinfos[4].maxsolutions = _nj4;
8569 vinfos[5].jointtype = 1;
8570 vinfos[5].foffset = j5;
8571 vinfos[5].indices[0] = _ij5[0];
8572 vinfos[5].indices[1] = _ij5[1];
8573 vinfos[5].maxsolutions = _nj5;
8574 std::vector<int> vfree(0);
8575 solutions.AddSolution(vinfos,vfree);
8576 }
8577 }
8578 }
8579 
8580 }
8581 
8582 }
8583 
8584 } else
8585 {
8586 {
8587 IkReal j3array[1], cj3array[1], sj3array[1];
8588 bool j3valid[1]={false};
8589 _nj3 = 1;
8590 IkReal x677=((1.0)*new_r11);
8591 CheckValue<IkReal> x678=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r10*x677)))),-1);
8592 if(!x678.valid){
8593 continue;
8594 }
8595 CheckValue<IkReal> x679 = IKatan2WithCheck(IkReal((((gconst4*new_r10))+((gconst4*new_r01)))),IkReal((((gconst4*new_r00))+(((-1.0)*gconst4*x677)))),IKFAST_ATAN2_MAGTHRESH);
8596 if(!x679.valid){
8597 continue;
8598 }
8599 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x678.value)))+(x679.value));
8600 sj3array[0]=IKsin(j3array[0]);
8601 cj3array[0]=IKcos(j3array[0]);
8602 if( j3array[0] > IKPI )
8603 {
8604  j3array[0]-=IK2PI;
8605 }
8606 else if( j3array[0] < -IKPI )
8607 { j3array[0]+=IK2PI;
8608 }
8609 j3valid[0] = true;
8610 for(int ij3 = 0; ij3 < 1; ++ij3)
8611 {
8612 if( !j3valid[ij3] )
8613 {
8614  continue;
8615 }
8616 _ij3[0] = ij3; _ij3[1] = -1;
8617 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8618 {
8619 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8620 {
8621  j3valid[iij3]=false; _ij3[1] = iij3; break;
8622 }
8623 }
8624 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8625 {
8626 IkReal evalcond[8];
8627 IkReal x680=IKsin(j3);
8628 IkReal x681=IKcos(j3);
8629 IkReal x682=((1.0)*gconst5);
8630 IkReal x683=(gconst4*x680);
8631 IkReal x684=(gconst4*x681);
8632 IkReal x685=((1.0)*x681);
8633 IkReal x686=(x681*x682);
8634 evalcond[0]=(gconst4+((new_r01*x681))+((new_r11*x680)));
8635 evalcond[1]=(((gconst5*x680))+x684+new_r01);
8636 evalcond[2]=((((-1.0)*new_r10*x685))+gconst4+((new_r00*x680)));
8637 evalcond[3]=(gconst5+((new_r01*x680))+(((-1.0)*new_r11*x685)));
8638 evalcond[4]=((((-1.0)*x686))+x683+new_r00);
8639 evalcond[5]=((((-1.0)*x686))+x683+new_r11);
8640 evalcond[6]=((((-1.0)*x682))+((new_r00*x681))+((new_r10*x680)));
8641 evalcond[7]=((((-1.0)*x680*x682))+new_r10+(((-1.0)*x684)));
8642 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 )
8643 {
8644 continue;
8645 }
8646 }
8647 
8648 {
8649 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8650 vinfos[0].jointtype = 1;
8651 vinfos[0].foffset = j0;
8652 vinfos[0].indices[0] = _ij0[0];
8653 vinfos[0].indices[1] = _ij0[1];
8654 vinfos[0].maxsolutions = _nj0;
8655 vinfos[1].jointtype = 1;
8656 vinfos[1].foffset = j1;
8657 vinfos[1].indices[0] = _ij1[0];
8658 vinfos[1].indices[1] = _ij1[1];
8659 vinfos[1].maxsolutions = _nj1;
8660 vinfos[2].jointtype = 1;
8661 vinfos[2].foffset = j2;
8662 vinfos[2].indices[0] = _ij2[0];
8663 vinfos[2].indices[1] = _ij2[1];
8664 vinfos[2].maxsolutions = _nj2;
8665 vinfos[3].jointtype = 1;
8666 vinfos[3].foffset = j3;
8667 vinfos[3].indices[0] = _ij3[0];
8668 vinfos[3].indices[1] = _ij3[1];
8669 vinfos[3].maxsolutions = _nj3;
8670 vinfos[4].jointtype = 1;
8671 vinfos[4].foffset = j4;
8672 vinfos[4].indices[0] = _ij4[0];
8673 vinfos[4].indices[1] = _ij4[1];
8674 vinfos[4].maxsolutions = _nj4;
8675 vinfos[5].jointtype = 1;
8676 vinfos[5].foffset = j5;
8677 vinfos[5].indices[0] = _ij5[0];
8678 vinfos[5].indices[1] = _ij5[1];
8679 vinfos[5].maxsolutions = _nj5;
8680 std::vector<int> vfree(0);
8681 solutions.AddSolution(vinfos,vfree);
8682 }
8683 }
8684 }
8685 
8686 }
8687 
8688 }
8689 
8690 }
8691 } while(0);
8692 if( bgotonextstatement )
8693 {
8694 bool bgotonextstatement = true;
8695 do
8696 {
8697 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8698 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8699 {
8700 bgotonextstatement=false;
8701 {
8702 IkReal j3array[1], cj3array[1], sj3array[1];
8703 bool j3valid[1]={false};
8704 _nj3 = 1;
8705 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 )
8706  continue;
8707 j3array[0]=IKatan2(((-1.0)*new_r01), new_r00);
8708 sj3array[0]=IKsin(j3array[0]);
8709 cj3array[0]=IKcos(j3array[0]);
8710 if( j3array[0] > IKPI )
8711 {
8712  j3array[0]-=IK2PI;
8713 }
8714 else if( j3array[0] < -IKPI )
8715 { j3array[0]+=IK2PI;
8716 }
8717 j3valid[0] = true;
8718 for(int ij3 = 0; ij3 < 1; ++ij3)
8719 {
8720 if( !j3valid[ij3] )
8721 {
8722  continue;
8723 }
8724 _ij3[0] = ij3; _ij3[1] = -1;
8725 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8726 {
8727 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8728 {
8729  j3valid[iij3]=false; _ij3[1] = iij3; break;
8730 }
8731 }
8732 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8733 {
8734 IkReal evalcond[8];
8735 IkReal x687=IKsin(j3);
8736 IkReal x688=IKcos(j3);
8737 IkReal x689=((1.0)*x688);
8738 evalcond[0]=(x687+new_r01);
8739 evalcond[1]=((((-1.0)*x689))+new_r00);
8740 evalcond[2]=((((-1.0)*x689))+new_r11);
8741 evalcond[3]=(new_r10+(((-1.0)*x687)));
8742 evalcond[4]=(((new_r01*x688))+((new_r11*x687)));
8743 evalcond[5]=((((-1.0)*new_r10*x689))+((new_r00*x687)));
8744 evalcond[6]=((-1.0)+((new_r00*x688))+((new_r10*x687)));
8745 evalcond[7]=((1.0)+((new_r01*x687))+(((-1.0)*new_r11*x689)));
8746 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 )
8747 {
8748 continue;
8749 }
8750 }
8751 
8752 {
8753 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8754 vinfos[0].jointtype = 1;
8755 vinfos[0].foffset = j0;
8756 vinfos[0].indices[0] = _ij0[0];
8757 vinfos[0].indices[1] = _ij0[1];
8758 vinfos[0].maxsolutions = _nj0;
8759 vinfos[1].jointtype = 1;
8760 vinfos[1].foffset = j1;
8761 vinfos[1].indices[0] = _ij1[0];
8762 vinfos[1].indices[1] = _ij1[1];
8763 vinfos[1].maxsolutions = _nj1;
8764 vinfos[2].jointtype = 1;
8765 vinfos[2].foffset = j2;
8766 vinfos[2].indices[0] = _ij2[0];
8767 vinfos[2].indices[1] = _ij2[1];
8768 vinfos[2].maxsolutions = _nj2;
8769 vinfos[3].jointtype = 1;
8770 vinfos[3].foffset = j3;
8771 vinfos[3].indices[0] = _ij3[0];
8772 vinfos[3].indices[1] = _ij3[1];
8773 vinfos[3].maxsolutions = _nj3;
8774 vinfos[4].jointtype = 1;
8775 vinfos[4].foffset = j4;
8776 vinfos[4].indices[0] = _ij4[0];
8777 vinfos[4].indices[1] = _ij4[1];
8778 vinfos[4].maxsolutions = _nj4;
8779 vinfos[5].jointtype = 1;
8780 vinfos[5].foffset = j5;
8781 vinfos[5].indices[0] = _ij5[0];
8782 vinfos[5].indices[1] = _ij5[1];
8783 vinfos[5].maxsolutions = _nj5;
8784 std::vector<int> vfree(0);
8785 solutions.AddSolution(vinfos,vfree);
8786 }
8787 }
8788 }
8789 
8790 }
8791 } while(0);
8792 if( bgotonextstatement )
8793 {
8794 bool bgotonextstatement = true;
8795 do
8796 {
8797 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8798 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8799 {
8800 bgotonextstatement=false;
8801 {
8802 IkReal j3array[1], cj3array[1], sj3array[1];
8803 bool j3valid[1]={false};
8804 _nj3 = 1;
8805 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
8806  continue;
8807 j3array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8808 sj3array[0]=IKsin(j3array[0]);
8809 cj3array[0]=IKcos(j3array[0]);
8810 if( j3array[0] > IKPI )
8811 {
8812  j3array[0]-=IK2PI;
8813 }
8814 else if( j3array[0] < -IKPI )
8815 { j3array[0]+=IK2PI;
8816 }
8817 j3valid[0] = true;
8818 for(int ij3 = 0; ij3 < 1; ++ij3)
8819 {
8820 if( !j3valid[ij3] )
8821 {
8822  continue;
8823 }
8824 _ij3[0] = ij3; _ij3[1] = -1;
8825 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8826 {
8827 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8828 {
8829  j3valid[iij3]=false; _ij3[1] = iij3; break;
8830 }
8831 }
8832 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8833 {
8834 IkReal evalcond[8];
8835 IkReal x690=IKcos(j3);
8836 IkReal x691=IKsin(j3);
8837 IkReal x692=((1.0)*x690);
8838 evalcond[0]=(x690+new_r00);
8839 evalcond[1]=(x690+new_r11);
8840 evalcond[2]=(x691+new_r10);
8841 evalcond[3]=(new_r01+(((-1.0)*x691)));
8842 evalcond[4]=(((new_r11*x691))+((new_r01*x690)));
8843 evalcond[5]=((((-1.0)*new_r10*x692))+((new_r00*x691)));
8844 evalcond[6]=((1.0)+((new_r10*x691))+((new_r00*x690)));
8845 evalcond[7]=((-1.0)+(((-1.0)*new_r11*x692))+((new_r01*x691)));
8846 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 )
8847 {
8848 continue;
8849 }
8850 }
8851 
8852 {
8853 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8854 vinfos[0].jointtype = 1;
8855 vinfos[0].foffset = j0;
8856 vinfos[0].indices[0] = _ij0[0];
8857 vinfos[0].indices[1] = _ij0[1];
8858 vinfos[0].maxsolutions = _nj0;
8859 vinfos[1].jointtype = 1;
8860 vinfos[1].foffset = j1;
8861 vinfos[1].indices[0] = _ij1[0];
8862 vinfos[1].indices[1] = _ij1[1];
8863 vinfos[1].maxsolutions = _nj1;
8864 vinfos[2].jointtype = 1;
8865 vinfos[2].foffset = j2;
8866 vinfos[2].indices[0] = _ij2[0];
8867 vinfos[2].indices[1] = _ij2[1];
8868 vinfos[2].maxsolutions = _nj2;
8869 vinfos[3].jointtype = 1;
8870 vinfos[3].foffset = j3;
8871 vinfos[3].indices[0] = _ij3[0];
8872 vinfos[3].indices[1] = _ij3[1];
8873 vinfos[3].maxsolutions = _nj3;
8874 vinfos[4].jointtype = 1;
8875 vinfos[4].foffset = j4;
8876 vinfos[4].indices[0] = _ij4[0];
8877 vinfos[4].indices[1] = _ij4[1];
8878 vinfos[4].maxsolutions = _nj4;
8879 vinfos[5].jointtype = 1;
8880 vinfos[5].foffset = j5;
8881 vinfos[5].indices[0] = _ij5[0];
8882 vinfos[5].indices[1] = _ij5[1];
8883 vinfos[5].maxsolutions = _nj5;
8884 std::vector<int> vfree(0);
8885 solutions.AddSolution(vinfos,vfree);
8886 }
8887 }
8888 }
8889 
8890 }
8891 } while(0);
8892 if( bgotonextstatement )
8893 {
8894 bool bgotonextstatement = true;
8895 do
8896 {
8897 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
8898 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8899 {
8900 bgotonextstatement=false;
8901 {
8902 IkReal j3eval[3];
8903 sj4=0;
8904 cj4=1.0;
8905 j4=0;
8906 new_r11=0;
8907 new_r00=0;
8908 j3eval[0]=new_r01;
8909 j3eval[1]=IKsign(new_r01);
8910 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
8911 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
8912 {
8913 {
8914 IkReal j3eval[2];
8915 sj4=0;
8916 cj4=1.0;
8917 j4=0;
8918 new_r11=0;
8919 new_r00=0;
8920 j3eval[0]=new_r01;
8921 j3eval[1]=new_r10;
8922 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8923 {
8924 {
8925 IkReal j3eval[2];
8926 sj4=0;
8927 cj4=1.0;
8928 j4=0;
8929 new_r11=0;
8930 new_r00=0;
8931 j3eval[0]=new_r01;
8932 j3eval[1]=sj5;
8933 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8934 {
8935 {
8936 IkReal evalcond[1];
8937 bool bgotonextstatement = true;
8938 do
8939 {
8940 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8941 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8942 {
8943 bgotonextstatement=false;
8944 {
8945 IkReal j3array[2], cj3array[2], sj3array[2];
8946 bool j3valid[2]={false};
8947 _nj3 = 2;
8948 sj3array[0]=new_r10;
8949 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8950 {
8951  j3valid[0] = j3valid[1] = true;
8952  j3array[0] = IKasin(sj3array[0]);
8953  cj3array[0] = IKcos(j3array[0]);
8954  sj3array[1] = sj3array[0];
8955  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8956  cj3array[1] = -cj3array[0];
8957 }
8958 else if( isnan(sj3array[0]) )
8959 {
8960  // probably any value will work
8961  j3valid[0] = true;
8962  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8963 }
8964 for(int ij3 = 0; ij3 < 2; ++ij3)
8965 {
8966 if( !j3valid[ij3] )
8967 {
8968  continue;
8969 }
8970 _ij3[0] = ij3; _ij3[1] = -1;
8971 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8972 {
8973 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8974 {
8975  j3valid[iij3]=false; _ij3[1] = iij3; break;
8976 }
8977 }
8978 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8979 {
8980 IkReal evalcond[6];
8981 IkReal x693=IKcos(j3);
8982 IkReal x694=IKsin(j3);
8983 IkReal x695=((-1.0)*x693);
8984 evalcond[0]=(new_r01*x693);
8985 evalcond[1]=(x694+new_r01);
8986 evalcond[2]=x695;
8987 evalcond[3]=(new_r10*x695);
8988 evalcond[4]=((1.0)+((new_r01*x694)));
8989 evalcond[5]=((-1.0)+((new_r10*x694)));
8990 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 )
8991 {
8992 continue;
8993 }
8994 }
8995 
8996 {
8997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8998 vinfos[0].jointtype = 1;
8999 vinfos[0].foffset = j0;
9000 vinfos[0].indices[0] = _ij0[0];
9001 vinfos[0].indices[1] = _ij0[1];
9002 vinfos[0].maxsolutions = _nj0;
9003 vinfos[1].jointtype = 1;
9004 vinfos[1].foffset = j1;
9005 vinfos[1].indices[0] = _ij1[0];
9006 vinfos[1].indices[1] = _ij1[1];
9007 vinfos[1].maxsolutions = _nj1;
9008 vinfos[2].jointtype = 1;
9009 vinfos[2].foffset = j2;
9010 vinfos[2].indices[0] = _ij2[0];
9011 vinfos[2].indices[1] = _ij2[1];
9012 vinfos[2].maxsolutions = _nj2;
9013 vinfos[3].jointtype = 1;
9014 vinfos[3].foffset = j3;
9015 vinfos[3].indices[0] = _ij3[0];
9016 vinfos[3].indices[1] = _ij3[1];
9017 vinfos[3].maxsolutions = _nj3;
9018 vinfos[4].jointtype = 1;
9019 vinfos[4].foffset = j4;
9020 vinfos[4].indices[0] = _ij4[0];
9021 vinfos[4].indices[1] = _ij4[1];
9022 vinfos[4].maxsolutions = _nj4;
9023 vinfos[5].jointtype = 1;
9024 vinfos[5].foffset = j5;
9025 vinfos[5].indices[0] = _ij5[0];
9026 vinfos[5].indices[1] = _ij5[1];
9027 vinfos[5].maxsolutions = _nj5;
9028 std::vector<int> vfree(0);
9029 solutions.AddSolution(vinfos,vfree);
9030 }
9031 }
9032 }
9033 
9034 }
9035 } while(0);
9036 if( bgotonextstatement )
9037 {
9038 bool bgotonextstatement = true;
9039 do
9040 {
9041 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
9042 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9043 {
9044 bgotonextstatement=false;
9045 {
9046 IkReal j3array[2], cj3array[2], sj3array[2];
9047 bool j3valid[2]={false};
9048 _nj3 = 2;
9049 sj3array[0]=new_r01;
9050 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
9051 {
9052  j3valid[0] = j3valid[1] = true;
9053  j3array[0] = IKasin(sj3array[0]);
9054  cj3array[0] = IKcos(j3array[0]);
9055  sj3array[1] = sj3array[0];
9056  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
9057  cj3array[1] = -cj3array[0];
9058 }
9059 else if( isnan(sj3array[0]) )
9060 {
9061  // probably any value will work
9062  j3valid[0] = true;
9063  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
9064 }
9065 for(int ij3 = 0; ij3 < 2; ++ij3)
9066 {
9067 if( !j3valid[ij3] )
9068 {
9069  continue;
9070 }
9071 _ij3[0] = ij3; _ij3[1] = -1;
9072 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9073 {
9074 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9075 {
9076  j3valid[iij3]=false; _ij3[1] = iij3; break;
9077 }
9078 }
9079 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9080 {
9081 IkReal evalcond[6];
9082 IkReal x696=IKcos(j3);
9083 IkReal x697=IKsin(j3);
9084 evalcond[0]=x696;
9085 evalcond[1]=(new_r01*x696);
9086 evalcond[2]=(x697+new_r10);
9087 evalcond[3]=((-1.0)*new_r10*x696);
9088 evalcond[4]=((-1.0)+((new_r01*x697)));
9089 evalcond[5]=((1.0)+((new_r10*x697)));
9090 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 )
9091 {
9092 continue;
9093 }
9094 }
9095 
9096 {
9097 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9098 vinfos[0].jointtype = 1;
9099 vinfos[0].foffset = j0;
9100 vinfos[0].indices[0] = _ij0[0];
9101 vinfos[0].indices[1] = _ij0[1];
9102 vinfos[0].maxsolutions = _nj0;
9103 vinfos[1].jointtype = 1;
9104 vinfos[1].foffset = j1;
9105 vinfos[1].indices[0] = _ij1[0];
9106 vinfos[1].indices[1] = _ij1[1];
9107 vinfos[1].maxsolutions = _nj1;
9108 vinfos[2].jointtype = 1;
9109 vinfos[2].foffset = j2;
9110 vinfos[2].indices[0] = _ij2[0];
9111 vinfos[2].indices[1] = _ij2[1];
9112 vinfos[2].maxsolutions = _nj2;
9113 vinfos[3].jointtype = 1;
9114 vinfos[3].foffset = j3;
9115 vinfos[3].indices[0] = _ij3[0];
9116 vinfos[3].indices[1] = _ij3[1];
9117 vinfos[3].maxsolutions = _nj3;
9118 vinfos[4].jointtype = 1;
9119 vinfos[4].foffset = j4;
9120 vinfos[4].indices[0] = _ij4[0];
9121 vinfos[4].indices[1] = _ij4[1];
9122 vinfos[4].maxsolutions = _nj4;
9123 vinfos[5].jointtype = 1;
9124 vinfos[5].foffset = j5;
9125 vinfos[5].indices[0] = _ij5[0];
9126 vinfos[5].indices[1] = _ij5[1];
9127 vinfos[5].maxsolutions = _nj5;
9128 std::vector<int> vfree(0);
9129 solutions.AddSolution(vinfos,vfree);
9130 }
9131 }
9132 }
9133 
9134 }
9135 } while(0);
9136 if( bgotonextstatement )
9137 {
9138 bool bgotonextstatement = true;
9139 do
9140 {
9141 if( 1 )
9142 {
9143 bgotonextstatement=false;
9144 continue; // branch miss [j3]
9145 
9146 }
9147 } while(0);
9148 if( bgotonextstatement )
9149 {
9150 }
9151 }
9152 }
9153 }
9154 
9155 } else
9156 {
9157 {
9158 IkReal j3array[1], cj3array[1], sj3array[1];
9159 bool j3valid[1]={false};
9160 _nj3 = 1;
9161 CheckValue<IkReal> x699=IKPowWithIntegerCheck(new_r01,-1);
9162 if(!x699.valid){
9163 continue;
9164 }
9165 IkReal x698=x699.value;
9166 CheckValue<IkReal> x700=IKPowWithIntegerCheck(sj5,-1);
9167 if(!x700.valid){
9168 continue;
9169 }
9170 CheckValue<IkReal> x701=IKPowWithIntegerCheck(x698,-2);
9171 if(!x701.valid){
9172 continue;
9173 }
9174 if( IKabs(((-1.0)*cj5*x698)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*x698))+IKsqr((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))))-1) <= IKFAST_SINCOS_THRESH )
9175  continue;
9176 j3array[0]=IKatan2(((-1.0)*cj5*x698), (x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))));
9177 sj3array[0]=IKsin(j3array[0]);
9178 cj3array[0]=IKcos(j3array[0]);
9179 if( j3array[0] > IKPI )
9180 {
9181  j3array[0]-=IK2PI;
9182 }
9183 else if( j3array[0] < -IKPI )
9184 { j3array[0]+=IK2PI;
9185 }
9186 j3valid[0] = true;
9187 for(int ij3 = 0; ij3 < 1; ++ij3)
9188 {
9189 if( !j3valid[ij3] )
9190 {
9191  continue;
9192 }
9193 _ij3[0] = ij3; _ij3[1] = -1;
9194 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9195 {
9196 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9197 {
9198  j3valid[iij3]=false; _ij3[1] = iij3; break;
9199 }
9200 }
9201 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9202 {
9203 IkReal evalcond[7];
9204 IkReal x702=IKcos(j3);
9205 IkReal x703=IKsin(j3);
9206 IkReal x704=((1.0)*cj5);
9207 IkReal x705=(sj5*x702);
9208 evalcond[0]=(cj5+((new_r01*x703)));
9209 evalcond[1]=(sj5+((new_r01*x702)));
9210 evalcond[2]=(sj5+(((-1.0)*new_r10*x702)));
9211 evalcond[3]=((((-1.0)*x704))+((new_r10*x703)));
9212 evalcond[4]=(((cj5*x703))+x705+new_r01);
9213 evalcond[5]=((((-1.0)*x702*x704))+((sj5*x703)));
9214 evalcond[6]=((((-1.0)*x705))+(((-1.0)*x703*x704))+new_r10);
9215 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 )
9216 {
9217 continue;
9218 }
9219 }
9220 
9221 {
9222 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9223 vinfos[0].jointtype = 1;
9224 vinfos[0].foffset = j0;
9225 vinfos[0].indices[0] = _ij0[0];
9226 vinfos[0].indices[1] = _ij0[1];
9227 vinfos[0].maxsolutions = _nj0;
9228 vinfos[1].jointtype = 1;
9229 vinfos[1].foffset = j1;
9230 vinfos[1].indices[0] = _ij1[0];
9231 vinfos[1].indices[1] = _ij1[1];
9232 vinfos[1].maxsolutions = _nj1;
9233 vinfos[2].jointtype = 1;
9234 vinfos[2].foffset = j2;
9235 vinfos[2].indices[0] = _ij2[0];
9236 vinfos[2].indices[1] = _ij2[1];
9237 vinfos[2].maxsolutions = _nj2;
9238 vinfos[3].jointtype = 1;
9239 vinfos[3].foffset = j3;
9240 vinfos[3].indices[0] = _ij3[0];
9241 vinfos[3].indices[1] = _ij3[1];
9242 vinfos[3].maxsolutions = _nj3;
9243 vinfos[4].jointtype = 1;
9244 vinfos[4].foffset = j4;
9245 vinfos[4].indices[0] = _ij4[0];
9246 vinfos[4].indices[1] = _ij4[1];
9247 vinfos[4].maxsolutions = _nj4;
9248 vinfos[5].jointtype = 1;
9249 vinfos[5].foffset = j5;
9250 vinfos[5].indices[0] = _ij5[0];
9251 vinfos[5].indices[1] = _ij5[1];
9252 vinfos[5].maxsolutions = _nj5;
9253 std::vector<int> vfree(0);
9254 solutions.AddSolution(vinfos,vfree);
9255 }
9256 }
9257 }
9258 
9259 }
9260 
9261 }
9262 
9263 } else
9264 {
9265 {
9266 IkReal j3array[1], cj3array[1], sj3array[1];
9267 bool j3valid[1]={false};
9268 _nj3 = 1;
9269 CheckValue<IkReal> x706=IKPowWithIntegerCheck(new_r01,-1);
9270 if(!x706.valid){
9271 continue;
9272 }
9273 CheckValue<IkReal> x707=IKPowWithIntegerCheck(new_r10,-1);
9274 if(!x707.valid){
9275 continue;
9276 }
9277 if( IKabs(((-1.0)*cj5*(x706.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x707.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x706.value)))+IKsqr((sj5*(x707.value)))-1) <= IKFAST_SINCOS_THRESH )
9278  continue;
9279 j3array[0]=IKatan2(((-1.0)*cj5*(x706.value)), (sj5*(x707.value)));
9280 sj3array[0]=IKsin(j3array[0]);
9281 cj3array[0]=IKcos(j3array[0]);
9282 if( j3array[0] > IKPI )
9283 {
9284  j3array[0]-=IK2PI;
9285 }
9286 else if( j3array[0] < -IKPI )
9287 { j3array[0]+=IK2PI;
9288 }
9289 j3valid[0] = true;
9290 for(int ij3 = 0; ij3 < 1; ++ij3)
9291 {
9292 if( !j3valid[ij3] )
9293 {
9294  continue;
9295 }
9296 _ij3[0] = ij3; _ij3[1] = -1;
9297 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9298 {
9299 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9300 {
9301  j3valid[iij3]=false; _ij3[1] = iij3; break;
9302 }
9303 }
9304 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9305 {
9306 IkReal evalcond[7];
9307 IkReal x708=IKcos(j3);
9308 IkReal x709=IKsin(j3);
9309 IkReal x710=((1.0)*cj5);
9310 IkReal x711=(sj5*x708);
9311 evalcond[0]=(cj5+((new_r01*x709)));
9312 evalcond[1]=(sj5+((new_r01*x708)));
9313 evalcond[2]=(sj5+(((-1.0)*new_r10*x708)));
9314 evalcond[3]=((((-1.0)*x710))+((new_r10*x709)));
9315 evalcond[4]=(((cj5*x709))+x711+new_r01);
9316 evalcond[5]=(((sj5*x709))+(((-1.0)*x708*x710)));
9317 evalcond[6]=((((-1.0)*x709*x710))+(((-1.0)*x711))+new_r10);
9318 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 )
9319 {
9320 continue;
9321 }
9322 }
9323 
9324 {
9325 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9326 vinfos[0].jointtype = 1;
9327 vinfos[0].foffset = j0;
9328 vinfos[0].indices[0] = _ij0[0];
9329 vinfos[0].indices[1] = _ij0[1];
9330 vinfos[0].maxsolutions = _nj0;
9331 vinfos[1].jointtype = 1;
9332 vinfos[1].foffset = j1;
9333 vinfos[1].indices[0] = _ij1[0];
9334 vinfos[1].indices[1] = _ij1[1];
9335 vinfos[1].maxsolutions = _nj1;
9336 vinfos[2].jointtype = 1;
9337 vinfos[2].foffset = j2;
9338 vinfos[2].indices[0] = _ij2[0];
9339 vinfos[2].indices[1] = _ij2[1];
9340 vinfos[2].maxsolutions = _nj2;
9341 vinfos[3].jointtype = 1;
9342 vinfos[3].foffset = j3;
9343 vinfos[3].indices[0] = _ij3[0];
9344 vinfos[3].indices[1] = _ij3[1];
9345 vinfos[3].maxsolutions = _nj3;
9346 vinfos[4].jointtype = 1;
9347 vinfos[4].foffset = j4;
9348 vinfos[4].indices[0] = _ij4[0];
9349 vinfos[4].indices[1] = _ij4[1];
9350 vinfos[4].maxsolutions = _nj4;
9351 vinfos[5].jointtype = 1;
9352 vinfos[5].foffset = j5;
9353 vinfos[5].indices[0] = _ij5[0];
9354 vinfos[5].indices[1] = _ij5[1];
9355 vinfos[5].maxsolutions = _nj5;
9356 std::vector<int> vfree(0);
9357 solutions.AddSolution(vinfos,vfree);
9358 }
9359 }
9360 }
9361 
9362 }
9363 
9364 }
9365 
9366 } else
9367 {
9368 {
9369 IkReal j3array[1], cj3array[1], sj3array[1];
9370 bool j3valid[1]={false};
9371 _nj3 = 1;
9372 CheckValue<IkReal> x712=IKPowWithIntegerCheck(IKsign(new_r01),-1);
9373 if(!x712.valid){
9374 continue;
9375 }
9376 CheckValue<IkReal> x713 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(((-1.0)*sj5)),IKFAST_ATAN2_MAGTHRESH);
9377 if(!x713.valid){
9378 continue;
9379 }
9380 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x712.value)))+(x713.value));
9381 sj3array[0]=IKsin(j3array[0]);
9382 cj3array[0]=IKcos(j3array[0]);
9383 if( j3array[0] > IKPI )
9384 {
9385  j3array[0]-=IK2PI;
9386 }
9387 else if( j3array[0] < -IKPI )
9388 { j3array[0]+=IK2PI;
9389 }
9390 j3valid[0] = true;
9391 for(int ij3 = 0; ij3 < 1; ++ij3)
9392 {
9393 if( !j3valid[ij3] )
9394 {
9395  continue;
9396 }
9397 _ij3[0] = ij3; _ij3[1] = -1;
9398 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9399 {
9400 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9401 {
9402  j3valid[iij3]=false; _ij3[1] = iij3; break;
9403 }
9404 }
9405 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9406 {
9407 IkReal evalcond[7];
9408 IkReal x714=IKcos(j3);
9409 IkReal x715=IKsin(j3);
9410 IkReal x716=((1.0)*cj5);
9411 IkReal x717=(sj5*x714);
9412 evalcond[0]=(cj5+((new_r01*x715)));
9413 evalcond[1]=(sj5+((new_r01*x714)));
9414 evalcond[2]=(sj5+(((-1.0)*new_r10*x714)));
9415 evalcond[3]=((((-1.0)*x716))+((new_r10*x715)));
9416 evalcond[4]=(((cj5*x715))+x717+new_r01);
9417 evalcond[5]=((((-1.0)*x714*x716))+((sj5*x715)));
9418 evalcond[6]=((((-1.0)*x717))+new_r10+(((-1.0)*x715*x716)));
9419 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 )
9420 {
9421 continue;
9422 }
9423 }
9424 
9425 {
9426 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9427 vinfos[0].jointtype = 1;
9428 vinfos[0].foffset = j0;
9429 vinfos[0].indices[0] = _ij0[0];
9430 vinfos[0].indices[1] = _ij0[1];
9431 vinfos[0].maxsolutions = _nj0;
9432 vinfos[1].jointtype = 1;
9433 vinfos[1].foffset = j1;
9434 vinfos[1].indices[0] = _ij1[0];
9435 vinfos[1].indices[1] = _ij1[1];
9436 vinfos[1].maxsolutions = _nj1;
9437 vinfos[2].jointtype = 1;
9438 vinfos[2].foffset = j2;
9439 vinfos[2].indices[0] = _ij2[0];
9440 vinfos[2].indices[1] = _ij2[1];
9441 vinfos[2].maxsolutions = _nj2;
9442 vinfos[3].jointtype = 1;
9443 vinfos[3].foffset = j3;
9444 vinfos[3].indices[0] = _ij3[0];
9445 vinfos[3].indices[1] = _ij3[1];
9446 vinfos[3].maxsolutions = _nj3;
9447 vinfos[4].jointtype = 1;
9448 vinfos[4].foffset = j4;
9449 vinfos[4].indices[0] = _ij4[0];
9450 vinfos[4].indices[1] = _ij4[1];
9451 vinfos[4].maxsolutions = _nj4;
9452 vinfos[5].jointtype = 1;
9453 vinfos[5].foffset = j5;
9454 vinfos[5].indices[0] = _ij5[0];
9455 vinfos[5].indices[1] = _ij5[1];
9456 vinfos[5].maxsolutions = _nj5;
9457 std::vector<int> vfree(0);
9458 solutions.AddSolution(vinfos,vfree);
9459 }
9460 }
9461 }
9462 
9463 }
9464 
9465 }
9466 
9467 }
9468 } while(0);
9469 if( bgotonextstatement )
9470 {
9471 bool bgotonextstatement = true;
9472 do
9473 {
9474 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9475 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9476 {
9477 bgotonextstatement=false;
9478 {
9479 IkReal j3eval[1];
9480 sj4=0;
9481 cj4=1.0;
9482 j4=0;
9483 new_r11=0;
9484 new_r01=0;
9485 new_r22=0;
9486 new_r20=0;
9487 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9488 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9489 {
9490 continue; // no branches [j3]
9491 
9492 } else
9493 {
9494 {
9495 IkReal j3array[2], cj3array[2], sj3array[2];
9496 bool j3valid[2]={false};
9497 _nj3 = 2;
9498 CheckValue<IkReal> x719 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9499 if(!x719.valid){
9500 continue;
9501 }
9502 IkReal x718=x719.value;
9503 j3array[0]=((-1.0)*x718);
9504 sj3array[0]=IKsin(j3array[0]);
9505 cj3array[0]=IKcos(j3array[0]);
9506 j3array[1]=((3.14159265358979)+(((-1.0)*x718)));
9507 sj3array[1]=IKsin(j3array[1]);
9508 cj3array[1]=IKcos(j3array[1]);
9509 if( j3array[0] > IKPI )
9510 {
9511  j3array[0]-=IK2PI;
9512 }
9513 else if( j3array[0] < -IKPI )
9514 { j3array[0]+=IK2PI;
9515 }
9516 j3valid[0] = true;
9517 if( j3array[1] > IKPI )
9518 {
9519  j3array[1]-=IK2PI;
9520 }
9521 else if( j3array[1] < -IKPI )
9522 { j3array[1]+=IK2PI;
9523 }
9524 j3valid[1] = true;
9525 for(int ij3 = 0; ij3 < 2; ++ij3)
9526 {
9527 if( !j3valid[ij3] )
9528 {
9529  continue;
9530 }
9531 _ij3[0] = ij3; _ij3[1] = -1;
9532 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9533 {
9534 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9535 {
9536  j3valid[iij3]=false; _ij3[1] = iij3; break;
9537 }
9538 }
9539 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9540 {
9541 IkReal evalcond[1];
9542 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
9543 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9544 {
9545 continue;
9546 }
9547 }
9548 
9549 {
9550 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9551 vinfos[0].jointtype = 1;
9552 vinfos[0].foffset = j0;
9553 vinfos[0].indices[0] = _ij0[0];
9554 vinfos[0].indices[1] = _ij0[1];
9555 vinfos[0].maxsolutions = _nj0;
9556 vinfos[1].jointtype = 1;
9557 vinfos[1].foffset = j1;
9558 vinfos[1].indices[0] = _ij1[0];
9559 vinfos[1].indices[1] = _ij1[1];
9560 vinfos[1].maxsolutions = _nj1;
9561 vinfos[2].jointtype = 1;
9562 vinfos[2].foffset = j2;
9563 vinfos[2].indices[0] = _ij2[0];
9564 vinfos[2].indices[1] = _ij2[1];
9565 vinfos[2].maxsolutions = _nj2;
9566 vinfos[3].jointtype = 1;
9567 vinfos[3].foffset = j3;
9568 vinfos[3].indices[0] = _ij3[0];
9569 vinfos[3].indices[1] = _ij3[1];
9570 vinfos[3].maxsolutions = _nj3;
9571 vinfos[4].jointtype = 1;
9572 vinfos[4].foffset = j4;
9573 vinfos[4].indices[0] = _ij4[0];
9574 vinfos[4].indices[1] = _ij4[1];
9575 vinfos[4].maxsolutions = _nj4;
9576 vinfos[5].jointtype = 1;
9577 vinfos[5].foffset = j5;
9578 vinfos[5].indices[0] = _ij5[0];
9579 vinfos[5].indices[1] = _ij5[1];
9580 vinfos[5].maxsolutions = _nj5;
9581 std::vector<int> vfree(0);
9582 solutions.AddSolution(vinfos,vfree);
9583 }
9584 }
9585 }
9586 
9587 }
9588 
9589 }
9590 
9591 }
9592 } while(0);
9593 if( bgotonextstatement )
9594 {
9595 bool bgotonextstatement = true;
9596 do
9597 {
9598 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9599 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9600 {
9601 bgotonextstatement=false;
9602 {
9603 IkReal j3eval[1];
9604 sj4=0;
9605 cj4=1.0;
9606 j4=0;
9607 new_r00=0;
9608 new_r10=0;
9609 new_r21=0;
9610 new_r22=0;
9611 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9612 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9613 {
9614 continue; // no branches [j3]
9615 
9616 } else
9617 {
9618 {
9619 IkReal j3array[2], cj3array[2], sj3array[2];
9620 bool j3valid[2]={false};
9621 _nj3 = 2;
9622 CheckValue<IkReal> x721 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9623 if(!x721.valid){
9624 continue;
9625 }
9626 IkReal x720=x721.value;
9627 j3array[0]=((-1.0)*x720);
9628 sj3array[0]=IKsin(j3array[0]);
9629 cj3array[0]=IKcos(j3array[0]);
9630 j3array[1]=((3.14159265358979)+(((-1.0)*x720)));
9631 sj3array[1]=IKsin(j3array[1]);
9632 cj3array[1]=IKcos(j3array[1]);
9633 if( j3array[0] > IKPI )
9634 {
9635  j3array[0]-=IK2PI;
9636 }
9637 else if( j3array[0] < -IKPI )
9638 { j3array[0]+=IK2PI;
9639 }
9640 j3valid[0] = true;
9641 if( j3array[1] > IKPI )
9642 {
9643  j3array[1]-=IK2PI;
9644 }
9645 else if( j3array[1] < -IKPI )
9646 { j3array[1]+=IK2PI;
9647 }
9648 j3valid[1] = true;
9649 for(int ij3 = 0; ij3 < 2; ++ij3)
9650 {
9651 if( !j3valid[ij3] )
9652 {
9653  continue;
9654 }
9655 _ij3[0] = ij3; _ij3[1] = -1;
9656 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9657 {
9658 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9659 {
9660  j3valid[iij3]=false; _ij3[1] = iij3; break;
9661 }
9662 }
9663 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9664 {
9665 IkReal evalcond[1];
9666 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
9667 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9668 {
9669 continue;
9670 }
9671 }
9672 
9673 {
9674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9675 vinfos[0].jointtype = 1;
9676 vinfos[0].foffset = j0;
9677 vinfos[0].indices[0] = _ij0[0];
9678 vinfos[0].indices[1] = _ij0[1];
9679 vinfos[0].maxsolutions = _nj0;
9680 vinfos[1].jointtype = 1;
9681 vinfos[1].foffset = j1;
9682 vinfos[1].indices[0] = _ij1[0];
9683 vinfos[1].indices[1] = _ij1[1];
9684 vinfos[1].maxsolutions = _nj1;
9685 vinfos[2].jointtype = 1;
9686 vinfos[2].foffset = j2;
9687 vinfos[2].indices[0] = _ij2[0];
9688 vinfos[2].indices[1] = _ij2[1];
9689 vinfos[2].maxsolutions = _nj2;
9690 vinfos[3].jointtype = 1;
9691 vinfos[3].foffset = j3;
9692 vinfos[3].indices[0] = _ij3[0];
9693 vinfos[3].indices[1] = _ij3[1];
9694 vinfos[3].maxsolutions = _nj3;
9695 vinfos[4].jointtype = 1;
9696 vinfos[4].foffset = j4;
9697 vinfos[4].indices[0] = _ij4[0];
9698 vinfos[4].indices[1] = _ij4[1];
9699 vinfos[4].maxsolutions = _nj4;
9700 vinfos[5].jointtype = 1;
9701 vinfos[5].foffset = j5;
9702 vinfos[5].indices[0] = _ij5[0];
9703 vinfos[5].indices[1] = _ij5[1];
9704 vinfos[5].maxsolutions = _nj5;
9705 std::vector<int> vfree(0);
9706 solutions.AddSolution(vinfos,vfree);
9707 }
9708 }
9709 }
9710 
9711 }
9712 
9713 }
9714 
9715 }
9716 } while(0);
9717 if( bgotonextstatement )
9718 {
9719 bool bgotonextstatement = true;
9720 do
9721 {
9722 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
9723 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9724 {
9725 bgotonextstatement=false;
9726 {
9727 IkReal j3eval[3];
9728 sj4=0;
9729 cj4=1.0;
9730 j4=0;
9731 new_r01=0;
9732 new_r10=0;
9733 j3eval[0]=new_r11;
9734 j3eval[1]=IKsign(new_r11);
9735 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
9736 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9737 {
9738 {
9739 IkReal j3eval[3];
9740 sj4=0;
9741 cj4=1.0;
9742 j4=0;
9743 new_r01=0;
9744 new_r10=0;
9745 j3eval[0]=new_r00;
9746 j3eval[1]=((IKabs(cj5))+(IKabs(sj5)));
9747 j3eval[2]=IKsign(new_r00);
9748 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9749 {
9750 {
9751 IkReal j3eval[2];
9752 sj4=0;
9753 cj4=1.0;
9754 j4=0;
9755 new_r01=0;
9756 new_r10=0;
9757 j3eval[0]=new_r00;
9758 j3eval[1]=new_r11;
9759 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9760 {
9761 continue; // no branches [j3]
9762 
9763 } else
9764 {
9765 {
9766 IkReal j3array[1], cj3array[1], sj3array[1];
9767 bool j3valid[1]={false};
9768 _nj3 = 1;
9769 CheckValue<IkReal> x722=IKPowWithIntegerCheck(new_r00,-1);
9770 if(!x722.valid){
9771 continue;
9772 }
9773 CheckValue<IkReal> x723=IKPowWithIntegerCheck(new_r11,-1);
9774 if(!x723.valid){
9775 continue;
9776 }
9777 if( IKabs(((-1.0)*sj5*(x722.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x723.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x722.value)))+IKsqr((cj5*(x723.value)))-1) <= IKFAST_SINCOS_THRESH )
9778  continue;
9779 j3array[0]=IKatan2(((-1.0)*sj5*(x722.value)), (cj5*(x723.value)));
9780 sj3array[0]=IKsin(j3array[0]);
9781 cj3array[0]=IKcos(j3array[0]);
9782 if( j3array[0] > IKPI )
9783 {
9784  j3array[0]-=IK2PI;
9785 }
9786 else if( j3array[0] < -IKPI )
9787 { j3array[0]+=IK2PI;
9788 }
9789 j3valid[0] = true;
9790 for(int ij3 = 0; ij3 < 1; ++ij3)
9791 {
9792 if( !j3valid[ij3] )
9793 {
9794  continue;
9795 }
9796 _ij3[0] = ij3; _ij3[1] = -1;
9797 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9798 {
9799 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9800 {
9801  j3valid[iij3]=false; _ij3[1] = iij3; break;
9802 }
9803 }
9804 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9805 {
9806 IkReal evalcond[7];
9807 IkReal x724=IKsin(j3);
9808 IkReal x725=IKcos(j3);
9809 IkReal x726=(sj5*x724);
9810 IkReal x727=((1.0)*x725);
9811 IkReal x728=(cj5*x727);
9812 evalcond[0]=(sj5+((new_r00*x724)));
9813 evalcond[1]=(sj5+((new_r11*x724)));
9814 evalcond[2]=(cj5+(((-1.0)*new_r11*x727)));
9815 evalcond[3]=(((new_r00*x725))+(((-1.0)*cj5)));
9816 evalcond[4]=(((cj5*x724))+((sj5*x725)));
9817 evalcond[5]=((((-1.0)*x728))+x726+new_r00);
9818 evalcond[6]=((((-1.0)*x728))+x726+new_r11);
9819 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 )
9820 {
9821 continue;
9822 }
9823 }
9824 
9825 {
9826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9827 vinfos[0].jointtype = 1;
9828 vinfos[0].foffset = j0;
9829 vinfos[0].indices[0] = _ij0[0];
9830 vinfos[0].indices[1] = _ij0[1];
9831 vinfos[0].maxsolutions = _nj0;
9832 vinfos[1].jointtype = 1;
9833 vinfos[1].foffset = j1;
9834 vinfos[1].indices[0] = _ij1[0];
9835 vinfos[1].indices[1] = _ij1[1];
9836 vinfos[1].maxsolutions = _nj1;
9837 vinfos[2].jointtype = 1;
9838 vinfos[2].foffset = j2;
9839 vinfos[2].indices[0] = _ij2[0];
9840 vinfos[2].indices[1] = _ij2[1];
9841 vinfos[2].maxsolutions = _nj2;
9842 vinfos[3].jointtype = 1;
9843 vinfos[3].foffset = j3;
9844 vinfos[3].indices[0] = _ij3[0];
9845 vinfos[3].indices[1] = _ij3[1];
9846 vinfos[3].maxsolutions = _nj3;
9847 vinfos[4].jointtype = 1;
9848 vinfos[4].foffset = j4;
9849 vinfos[4].indices[0] = _ij4[0];
9850 vinfos[4].indices[1] = _ij4[1];
9851 vinfos[4].maxsolutions = _nj4;
9852 vinfos[5].jointtype = 1;
9853 vinfos[5].foffset = j5;
9854 vinfos[5].indices[0] = _ij5[0];
9855 vinfos[5].indices[1] = _ij5[1];
9856 vinfos[5].maxsolutions = _nj5;
9857 std::vector<int> vfree(0);
9858 solutions.AddSolution(vinfos,vfree);
9859 }
9860 }
9861 }
9862 
9863 }
9864 
9865 }
9866 
9867 } else
9868 {
9869 {
9870 IkReal j3array[1], cj3array[1], sj3array[1];
9871 bool j3valid[1]={false};
9872 _nj3 = 1;
9873 CheckValue<IkReal> x729=IKPowWithIntegerCheck(IKsign(new_r00),-1);
9874 if(!x729.valid){
9875 continue;
9876 }
9877 CheckValue<IkReal> x730 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9878 if(!x730.valid){
9879 continue;
9880 }
9881 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x729.value)))+(x730.value));
9882 sj3array[0]=IKsin(j3array[0]);
9883 cj3array[0]=IKcos(j3array[0]);
9884 if( j3array[0] > IKPI )
9885 {
9886  j3array[0]-=IK2PI;
9887 }
9888 else if( j3array[0] < -IKPI )
9889 { j3array[0]+=IK2PI;
9890 }
9891 j3valid[0] = true;
9892 for(int ij3 = 0; ij3 < 1; ++ij3)
9893 {
9894 if( !j3valid[ij3] )
9895 {
9896  continue;
9897 }
9898 _ij3[0] = ij3; _ij3[1] = -1;
9899 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9900 {
9901 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9902 {
9903  j3valid[iij3]=false; _ij3[1] = iij3; break;
9904 }
9905 }
9906 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9907 {
9908 IkReal evalcond[7];
9909 IkReal x731=IKsin(j3);
9910 IkReal x732=IKcos(j3);
9911 IkReal x733=(sj5*x731);
9912 IkReal x734=((1.0)*x732);
9913 IkReal x735=(cj5*x734);
9914 evalcond[0]=(sj5+((new_r00*x731)));
9915 evalcond[1]=(sj5+((new_r11*x731)));
9916 evalcond[2]=(cj5+(((-1.0)*new_r11*x734)));
9917 evalcond[3]=(((new_r00*x732))+(((-1.0)*cj5)));
9918 evalcond[4]=(((cj5*x731))+((sj5*x732)));
9919 evalcond[5]=((((-1.0)*x735))+x733+new_r00);
9920 evalcond[6]=((((-1.0)*x735))+x733+new_r11);
9921 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 )
9922 {
9923 continue;
9924 }
9925 }
9926 
9927 {
9928 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9929 vinfos[0].jointtype = 1;
9930 vinfos[0].foffset = j0;
9931 vinfos[0].indices[0] = _ij0[0];
9932 vinfos[0].indices[1] = _ij0[1];
9933 vinfos[0].maxsolutions = _nj0;
9934 vinfos[1].jointtype = 1;
9935 vinfos[1].foffset = j1;
9936 vinfos[1].indices[0] = _ij1[0];
9937 vinfos[1].indices[1] = _ij1[1];
9938 vinfos[1].maxsolutions = _nj1;
9939 vinfos[2].jointtype = 1;
9940 vinfos[2].foffset = j2;
9941 vinfos[2].indices[0] = _ij2[0];
9942 vinfos[2].indices[1] = _ij2[1];
9943 vinfos[2].maxsolutions = _nj2;
9944 vinfos[3].jointtype = 1;
9945 vinfos[3].foffset = j3;
9946 vinfos[3].indices[0] = _ij3[0];
9947 vinfos[3].indices[1] = _ij3[1];
9948 vinfos[3].maxsolutions = _nj3;
9949 vinfos[4].jointtype = 1;
9950 vinfos[4].foffset = j4;
9951 vinfos[4].indices[0] = _ij4[0];
9952 vinfos[4].indices[1] = _ij4[1];
9953 vinfos[4].maxsolutions = _nj4;
9954 vinfos[5].jointtype = 1;
9955 vinfos[5].foffset = j5;
9956 vinfos[5].indices[0] = _ij5[0];
9957 vinfos[5].indices[1] = _ij5[1];
9958 vinfos[5].maxsolutions = _nj5;
9959 std::vector<int> vfree(0);
9960 solutions.AddSolution(vinfos,vfree);
9961 }
9962 }
9963 }
9964 
9965 }
9966 
9967 }
9968 
9969 } else
9970 {
9971 {
9972 IkReal j3array[1], cj3array[1], sj3array[1];
9973 bool j3valid[1]={false};
9974 _nj3 = 1;
9975 CheckValue<IkReal> x736=IKPowWithIntegerCheck(IKsign(new_r11),-1);
9976 if(!x736.valid){
9977 continue;
9978 }
9979 CheckValue<IkReal> x737 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9980 if(!x737.valid){
9981 continue;
9982 }
9983 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x736.value)))+(x737.value));
9984 sj3array[0]=IKsin(j3array[0]);
9985 cj3array[0]=IKcos(j3array[0]);
9986 if( j3array[0] > IKPI )
9987 {
9988  j3array[0]-=IK2PI;
9989 }
9990 else if( j3array[0] < -IKPI )
9991 { j3array[0]+=IK2PI;
9992 }
9993 j3valid[0] = true;
9994 for(int ij3 = 0; ij3 < 1; ++ij3)
9995 {
9996 if( !j3valid[ij3] )
9997 {
9998  continue;
9999 }
10000 _ij3[0] = ij3; _ij3[1] = -1;
10001 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10002 {
10003 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10004 {
10005  j3valid[iij3]=false; _ij3[1] = iij3; break;
10006 }
10007 }
10008 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10009 {
10010 IkReal evalcond[7];
10011 IkReal x738=IKsin(j3);
10012 IkReal x739=IKcos(j3);
10013 IkReal x740=(sj5*x738);
10014 IkReal x741=((1.0)*x739);
10015 IkReal x742=(cj5*x741);
10016 evalcond[0]=(sj5+((new_r00*x738)));
10017 evalcond[1]=(sj5+((new_r11*x738)));
10018 evalcond[2]=(cj5+(((-1.0)*new_r11*x741)));
10019 evalcond[3]=(((new_r00*x739))+(((-1.0)*cj5)));
10020 evalcond[4]=(((cj5*x738))+((sj5*x739)));
10021 evalcond[5]=(x740+new_r00+(((-1.0)*x742)));
10022 evalcond[6]=(x740+new_r11+(((-1.0)*x742)));
10023 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 )
10024 {
10025 continue;
10026 }
10027 }
10028 
10029 {
10030 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10031 vinfos[0].jointtype = 1;
10032 vinfos[0].foffset = j0;
10033 vinfos[0].indices[0] = _ij0[0];
10034 vinfos[0].indices[1] = _ij0[1];
10035 vinfos[0].maxsolutions = _nj0;
10036 vinfos[1].jointtype = 1;
10037 vinfos[1].foffset = j1;
10038 vinfos[1].indices[0] = _ij1[0];
10039 vinfos[1].indices[1] = _ij1[1];
10040 vinfos[1].maxsolutions = _nj1;
10041 vinfos[2].jointtype = 1;
10042 vinfos[2].foffset = j2;
10043 vinfos[2].indices[0] = _ij2[0];
10044 vinfos[2].indices[1] = _ij2[1];
10045 vinfos[2].maxsolutions = _nj2;
10046 vinfos[3].jointtype = 1;
10047 vinfos[3].foffset = j3;
10048 vinfos[3].indices[0] = _ij3[0];
10049 vinfos[3].indices[1] = _ij3[1];
10050 vinfos[3].maxsolutions = _nj3;
10051 vinfos[4].jointtype = 1;
10052 vinfos[4].foffset = j4;
10053 vinfos[4].indices[0] = _ij4[0];
10054 vinfos[4].indices[1] = _ij4[1];
10055 vinfos[4].maxsolutions = _nj4;
10056 vinfos[5].jointtype = 1;
10057 vinfos[5].foffset = j5;
10058 vinfos[5].indices[0] = _ij5[0];
10059 vinfos[5].indices[1] = _ij5[1];
10060 vinfos[5].maxsolutions = _nj5;
10061 std::vector<int> vfree(0);
10062 solutions.AddSolution(vinfos,vfree);
10063 }
10064 }
10065 }
10066 
10067 }
10068 
10069 }
10070 
10071 }
10072 } while(0);
10073 if( bgotonextstatement )
10074 {
10075 bool bgotonextstatement = true;
10076 do
10077 {
10078 if( 1 )
10079 {
10080 bgotonextstatement=false;
10081 continue; // branch miss [j3]
10082 
10083 }
10084 } while(0);
10085 if( bgotonextstatement )
10086 {
10087 }
10088 }
10089 }
10090 }
10091 }
10092 }
10093 }
10094 }
10095 }
10096 }
10097 
10098 } else
10099 {
10100 {
10101 IkReal j3array[1], cj3array[1], sj3array[1];
10102 bool j3valid[1]={false};
10103 _nj3 = 1;
10104 IkReal x743=((1.0)*new_r11);
10105 CheckValue<IkReal> x744 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((1.0)+(((-1.0)*new_r00*x743))+(((-1.0)*(cj5*cj5))))),IKFAST_ATAN2_MAGTHRESH);
10106 if(!x744.valid){
10107 continue;
10108 }
10109 CheckValue<IkReal> x745=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*x743)))),-1);
10110 if(!x745.valid){
10111 continue;
10112 }
10113 j3array[0]=((-1.5707963267949)+(x744.value)+(((1.5707963267949)*(x745.value))));
10114 sj3array[0]=IKsin(j3array[0]);
10115 cj3array[0]=IKcos(j3array[0]);
10116 if( j3array[0] > IKPI )
10117 {
10118  j3array[0]-=IK2PI;
10119 }
10120 else if( j3array[0] < -IKPI )
10121 { j3array[0]+=IK2PI;
10122 }
10123 j3valid[0] = true;
10124 for(int ij3 = 0; ij3 < 1; ++ij3)
10125 {
10126 if( !j3valid[ij3] )
10127 {
10128  continue;
10129 }
10130 _ij3[0] = ij3; _ij3[1] = -1;
10131 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10132 {
10133 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10134 {
10135  j3valid[iij3]=false; _ij3[1] = iij3; break;
10136 }
10137 }
10138 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10139 {
10140 IkReal evalcond[8];
10141 IkReal x746=IKcos(j3);
10142 IkReal x747=IKsin(j3);
10143 IkReal x748=(sj5*x747);
10144 IkReal x749=(cj5*x747);
10145 IkReal x750=(sj5*x746);
10146 IkReal x751=((1.0)*x746);
10147 IkReal x752=(cj5*x751);
10148 evalcond[0]=(sj5+((new_r01*x746))+((new_r11*x747)));
10149 evalcond[1]=(x750+x749+new_r01);
10150 evalcond[2]=(sj5+(((-1.0)*new_r10*x751))+((new_r00*x747)));
10151 evalcond[3]=(cj5+(((-1.0)*new_r11*x751))+((new_r01*x747)));
10152 evalcond[4]=(x748+new_r00+(((-1.0)*x752)));
10153 evalcond[5]=(x748+new_r11+(((-1.0)*x752)));
10154 evalcond[6]=(((new_r10*x747))+((new_r00*x746))+(((-1.0)*cj5)));
10155 evalcond[7]=((((-1.0)*x750))+(((-1.0)*x749))+new_r10);
10156 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 )
10157 {
10158 continue;
10159 }
10160 }
10161 
10162 {
10163 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10164 vinfos[0].jointtype = 1;
10165 vinfos[0].foffset = j0;
10166 vinfos[0].indices[0] = _ij0[0];
10167 vinfos[0].indices[1] = _ij0[1];
10168 vinfos[0].maxsolutions = _nj0;
10169 vinfos[1].jointtype = 1;
10170 vinfos[1].foffset = j1;
10171 vinfos[1].indices[0] = _ij1[0];
10172 vinfos[1].indices[1] = _ij1[1];
10173 vinfos[1].maxsolutions = _nj1;
10174 vinfos[2].jointtype = 1;
10175 vinfos[2].foffset = j2;
10176 vinfos[2].indices[0] = _ij2[0];
10177 vinfos[2].indices[1] = _ij2[1];
10178 vinfos[2].maxsolutions = _nj2;
10179 vinfos[3].jointtype = 1;
10180 vinfos[3].foffset = j3;
10181 vinfos[3].indices[0] = _ij3[0];
10182 vinfos[3].indices[1] = _ij3[1];
10183 vinfos[3].maxsolutions = _nj3;
10184 vinfos[4].jointtype = 1;
10185 vinfos[4].foffset = j4;
10186 vinfos[4].indices[0] = _ij4[0];
10187 vinfos[4].indices[1] = _ij4[1];
10188 vinfos[4].maxsolutions = _nj4;
10189 vinfos[5].jointtype = 1;
10190 vinfos[5].foffset = j5;
10191 vinfos[5].indices[0] = _ij5[0];
10192 vinfos[5].indices[1] = _ij5[1];
10193 vinfos[5].maxsolutions = _nj5;
10194 std::vector<int> vfree(0);
10195 solutions.AddSolution(vinfos,vfree);
10196 }
10197 }
10198 }
10199 
10200 }
10201 
10202 }
10203 
10204 } else
10205 {
10206 {
10207 IkReal j3array[1], cj3array[1], sj3array[1];
10208 bool j3valid[1]={false};
10209 _nj3 = 1;
10210 CheckValue<IkReal> x753 = IKatan2WithCheck(IkReal((((new_r11*sj5))+((cj5*new_r01)))),IkReal((((new_r01*sj5))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10211 if(!x753.valid){
10212 continue;
10213 }
10214 CheckValue<IkReal> x754=IKPowWithIntegerCheck(IKsign(((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))))),-1);
10215 if(!x754.valid){
10216 continue;
10217 }
10218 j3array[0]=((-1.5707963267949)+(x753.value)+(((1.5707963267949)*(x754.value))));
10219 sj3array[0]=IKsin(j3array[0]);
10220 cj3array[0]=IKcos(j3array[0]);
10221 if( j3array[0] > IKPI )
10222 {
10223  j3array[0]-=IK2PI;
10224 }
10225 else if( j3array[0] < -IKPI )
10226 { j3array[0]+=IK2PI;
10227 }
10228 j3valid[0] = true;
10229 for(int ij3 = 0; ij3 < 1; ++ij3)
10230 {
10231 if( !j3valid[ij3] )
10232 {
10233  continue;
10234 }
10235 _ij3[0] = ij3; _ij3[1] = -1;
10236 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10237 {
10238 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10239 {
10240  j3valid[iij3]=false; _ij3[1] = iij3; break;
10241 }
10242 }
10243 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10244 {
10245 IkReal evalcond[8];
10246 IkReal x755=IKcos(j3);
10247 IkReal x756=IKsin(j3);
10248 IkReal x757=(sj5*x756);
10249 IkReal x758=(cj5*x756);
10250 IkReal x759=(sj5*x755);
10251 IkReal x760=((1.0)*x755);
10252 IkReal x761=(cj5*x760);
10253 evalcond[0]=(sj5+((new_r11*x756))+((new_r01*x755)));
10254 evalcond[1]=(x759+x758+new_r01);
10255 evalcond[2]=(sj5+(((-1.0)*new_r10*x760))+((new_r00*x756)));
10256 evalcond[3]=(cj5+(((-1.0)*new_r11*x760))+((new_r01*x756)));
10257 evalcond[4]=((((-1.0)*x761))+x757+new_r00);
10258 evalcond[5]=((((-1.0)*x761))+x757+new_r11);
10259 evalcond[6]=(((new_r00*x755))+((new_r10*x756))+(((-1.0)*cj5)));
10260 evalcond[7]=((((-1.0)*x759))+(((-1.0)*x758))+new_r10);
10261 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 )
10262 {
10263 continue;
10264 }
10265 }
10266 
10267 {
10268 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10269 vinfos[0].jointtype = 1;
10270 vinfos[0].foffset = j0;
10271 vinfos[0].indices[0] = _ij0[0];
10272 vinfos[0].indices[1] = _ij0[1];
10273 vinfos[0].maxsolutions = _nj0;
10274 vinfos[1].jointtype = 1;
10275 vinfos[1].foffset = j1;
10276 vinfos[1].indices[0] = _ij1[0];
10277 vinfos[1].indices[1] = _ij1[1];
10278 vinfos[1].maxsolutions = _nj1;
10279 vinfos[2].jointtype = 1;
10280 vinfos[2].foffset = j2;
10281 vinfos[2].indices[0] = _ij2[0];
10282 vinfos[2].indices[1] = _ij2[1];
10283 vinfos[2].maxsolutions = _nj2;
10284 vinfos[3].jointtype = 1;
10285 vinfos[3].foffset = j3;
10286 vinfos[3].indices[0] = _ij3[0];
10287 vinfos[3].indices[1] = _ij3[1];
10288 vinfos[3].maxsolutions = _nj3;
10289 vinfos[4].jointtype = 1;
10290 vinfos[4].foffset = j4;
10291 vinfos[4].indices[0] = _ij4[0];
10292 vinfos[4].indices[1] = _ij4[1];
10293 vinfos[4].maxsolutions = _nj4;
10294 vinfos[5].jointtype = 1;
10295 vinfos[5].foffset = j5;
10296 vinfos[5].indices[0] = _ij5[0];
10297 vinfos[5].indices[1] = _ij5[1];
10298 vinfos[5].maxsolutions = _nj5;
10299 std::vector<int> vfree(0);
10300 solutions.AddSolution(vinfos,vfree);
10301 }
10302 }
10303 }
10304 
10305 }
10306 
10307 }
10308 
10309 } else
10310 {
10311 {
10312 IkReal j3array[1], cj3array[1], sj3array[1];
10313 bool j3valid[1]={false};
10314 _nj3 = 1;
10315 IkReal x762=((1.0)*new_r11);
10316 CheckValue<IkReal> x763 = IKatan2WithCheck(IkReal((((new_r10*sj5))+((new_r01*sj5)))),IkReal((((new_r00*sj5))+(((-1.0)*sj5*x762)))),IKFAST_ATAN2_MAGTHRESH);
10317 if(!x763.valid){
10318 continue;
10319 }
10320 CheckValue<IkReal> x764=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x762))+(((-1.0)*new_r00*new_r01)))),-1);
10321 if(!x764.valid){
10322 continue;
10323 }
10324 j3array[0]=((-1.5707963267949)+(x763.value)+(((1.5707963267949)*(x764.value))));
10325 sj3array[0]=IKsin(j3array[0]);
10326 cj3array[0]=IKcos(j3array[0]);
10327 if( j3array[0] > IKPI )
10328 {
10329  j3array[0]-=IK2PI;
10330 }
10331 else if( j3array[0] < -IKPI )
10332 { j3array[0]+=IK2PI;
10333 }
10334 j3valid[0] = true;
10335 for(int ij3 = 0; ij3 < 1; ++ij3)
10336 {
10337 if( !j3valid[ij3] )
10338 {
10339  continue;
10340 }
10341 _ij3[0] = ij3; _ij3[1] = -1;
10342 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10343 {
10344 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10345 {
10346  j3valid[iij3]=false; _ij3[1] = iij3; break;
10347 }
10348 }
10349 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10350 {
10351 IkReal evalcond[8];
10352 IkReal x765=IKcos(j3);
10353 IkReal x766=IKsin(j3);
10354 IkReal x767=(sj5*x766);
10355 IkReal x768=(cj5*x766);
10356 IkReal x769=(sj5*x765);
10357 IkReal x770=((1.0)*x765);
10358 IkReal x771=(cj5*x770);
10359 evalcond[0]=(sj5+((new_r11*x766))+((new_r01*x765)));
10360 evalcond[1]=(x768+x769+new_r01);
10361 evalcond[2]=(sj5+((new_r00*x766))+(((-1.0)*new_r10*x770)));
10362 evalcond[3]=(cj5+((new_r01*x766))+(((-1.0)*new_r11*x770)));
10363 evalcond[4]=(x767+(((-1.0)*x771))+new_r00);
10364 evalcond[5]=(x767+(((-1.0)*x771))+new_r11);
10365 evalcond[6]=(((new_r10*x766))+((new_r00*x765))+(((-1.0)*cj5)));
10366 evalcond[7]=((((-1.0)*x769))+(((-1.0)*x768))+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 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10368 {
10369 continue;
10370 }
10371 }
10372 
10373 {
10374 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10375 vinfos[0].jointtype = 1;
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 std::vector<int> vfree(0);
10406 solutions.AddSolution(vinfos,vfree);
10407 }
10408 }
10409 }
10410 
10411 }
10412 
10413 }
10414 
10415 }
10416 } while(0);
10417 if( bgotonextstatement )
10418 {
10419 bool bgotonextstatement = true;
10420 do
10421 {
10422 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10423 evalcond[1]=new_r02;
10424 evalcond[2]=new_r12;
10425 evalcond[3]=new_r21;
10426 evalcond[4]=new_r20;
10427 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 )
10428 {
10429 bgotonextstatement=false;
10430 {
10431 IkReal j3eval[3];
10432 sj4=0;
10433 cj4=-1.0;
10434 j4=3.14159265358979;
10435 IkReal x772=((1.0)*new_r10);
10436 IkReal x773=((((-1.0)*new_r11*x772))+(((-1.0)*new_r00*new_r01)));
10437 j3eval[0]=x773;
10438 j3eval[1]=((IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(IKabs((((cj5*new_r01))+(((-1.0)*cj5*x772))))));
10439 j3eval[2]=IKsign(x773);
10440 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10441 {
10442 {
10443 IkReal j3eval[3];
10444 sj4=0;
10445 cj4=-1.0;
10446 j4=3.14159265358979;
10447 IkReal x774=((1.0)*new_r10);
10448 IkReal x775=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x774)));
10449 j3eval[0]=x775;
10450 j3eval[1]=IKsign(x775);
10451 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((((-1.0)*new_r01*x774))+(cj5*cj5)))));
10452 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10453 {
10454 {
10455 IkReal j3eval[3];
10456 sj4=0;
10457 cj4=-1.0;
10458 j4=3.14159265358979;
10459 IkReal x776=((1.0)*new_r00);
10460 IkReal x777=((((-1.0)*sj5*x776))+((cj5*new_r10)));
10461 j3eval[0]=x777;
10462 j3eval[1]=((IKabs((((cj5*sj5))+(((-1.0)*new_r10*x776)))))+(IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00)))));
10463 j3eval[2]=IKsign(x777);
10464 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10465 {
10466 {
10467 IkReal evalcond[1];
10468 bool bgotonextstatement = true;
10469 do
10470 {
10471 IkReal x778=((-1.0)*new_r00);
10472 IkReal x780 = ((new_r10*new_r10)+(new_r00*new_r00));
10473 if(IKabs(x780)==0){
10474 continue;
10475 }
10476 IkReal x779=pow(x780,-0.5);
10477 CheckValue<IkReal> x781 = IKatan2WithCheck(IkReal(new_r10),IkReal(x778),IKFAST_ATAN2_MAGTHRESH);
10478 if(!x781.valid){
10479 continue;
10480 }
10481 IkReal gconst6=((-1.0)*(x781.value));
10482 IkReal gconst7=((-1.0)*new_r10*x779);
10483 IkReal gconst8=(x778*x779);
10484 CheckValue<IkReal> x782 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10485 if(!x782.valid){
10486 continue;
10487 }
10488 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j5+(x782.value))))), 6.28318530717959)));
10489 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10490 {
10491 bgotonextstatement=false;
10492 {
10493 IkReal j3eval[3];
10494 IkReal x783=((-1.0)*new_r00);
10495 CheckValue<IkReal> x786 = IKatan2WithCheck(IkReal(new_r10),IkReal(x783),IKFAST_ATAN2_MAGTHRESH);
10496 if(!x786.valid){
10497 continue;
10498 }
10499 IkReal x784=((-1.0)*(x786.value));
10500 IkReal x785=x779;
10501 sj4=0;
10502 cj4=-1.0;
10503 j4=3.14159265358979;
10504 sj5=gconst7;
10505 cj5=gconst8;
10506 j5=x784;
10507 IkReal gconst6=x784;
10508 IkReal gconst7=((-1.0)*new_r10*x785);
10509 IkReal gconst8=(x783*x785);
10510 IkReal x787=new_r00*new_r00;
10511 IkReal x788=((1.0)*new_r11);
10512 IkReal x789=((1.0)*new_r00*new_r01);
10513 IkReal x790=((((-1.0)*x789))+(((-1.0)*new_r10*x788)));
10514 IkReal x791=x779;
10515 IkReal x792=(new_r00*x791);
10516 j3eval[0]=x790;
10517 j3eval[1]=((IKabs(((((-1.0)*x788*x792))+(((-1.0)*x787*x791)))))+(IKabs((((new_r10*x792))+(((-1.0)*x789*x791))))));
10518 j3eval[2]=IKsign(x790);
10519 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10520 {
10521 {
10522 IkReal j3eval[1];
10523 IkReal x793=((-1.0)*new_r00);
10524 CheckValue<IkReal> x796 = IKatan2WithCheck(IkReal(new_r10),IkReal(x793),IKFAST_ATAN2_MAGTHRESH);
10525 if(!x796.valid){
10526 continue;
10527 }
10528 IkReal x794=((-1.0)*(x796.value));
10529 IkReal x795=x779;
10530 sj4=0;
10531 cj4=-1.0;
10532 j4=3.14159265358979;
10533 sj5=gconst7;
10534 cj5=gconst8;
10535 j5=x794;
10536 IkReal gconst6=x794;
10537 IkReal gconst7=((-1.0)*new_r10*x795);
10538 IkReal gconst8=(x793*x795);
10539 IkReal x797=new_r10*new_r10;
10540 IkReal x798=new_r00*new_r00;
10541 CheckValue<IkReal> x801=IKPowWithIntegerCheck((x797+x798),-1);
10542 if(!x801.valid){
10543 continue;
10544 }
10545 IkReal x799=x801.value;
10546 IkReal x800=(new_r00*x799);
10547 j3eval[0]=((IKabs((((new_r01*x797*x800))+((new_r10*x800))+((new_r01*x800*(new_r00*new_r00))))))+(IKabs((((x798*x799))+(((-1.0)*new_r01*new_r10))))));
10548 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10549 {
10550 {
10551 IkReal j3eval[1];
10552 IkReal x802=((-1.0)*new_r00);
10553 CheckValue<IkReal> x805 = IKatan2WithCheck(IkReal(new_r10),IkReal(x802),IKFAST_ATAN2_MAGTHRESH);
10554 if(!x805.valid){
10555 continue;
10556 }
10557 IkReal x803=((-1.0)*(x805.value));
10558 IkReal x804=x779;
10559 sj4=0;
10560 cj4=-1.0;
10561 j4=3.14159265358979;
10562 sj5=gconst7;
10563 cj5=gconst8;
10564 j5=x803;
10565 IkReal gconst6=x803;
10566 IkReal gconst7=((-1.0)*new_r10*x804);
10567 IkReal gconst8=(x802*x804);
10568 IkReal x806=new_r00*new_r00;
10569 IkReal x807=new_r10*new_r10;
10570 CheckValue<IkReal> x811=IKPowWithIntegerCheck((x807+x806),-1);
10571 if(!x811.valid){
10572 continue;
10573 }
10574 IkReal x808=x811.value;
10575 IkReal x809=(new_r10*x808);
10576 IkReal x810=((1.0)*x808);
10577 j3eval[0]=((IKabs((((new_r00*x809*(new_r10*new_r10)))+((new_r00*x809))+((x809*(new_r00*new_r00*new_r00))))))+(IKabs((((x806*x808))+(((-1.0)*x810*(x807*x807)))+(((-1.0)*x806*x807*x810))))));
10578 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10579 {
10580 {
10581 IkReal evalcond[2];
10582 bool bgotonextstatement = true;
10583 do
10584 {
10585 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10586 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10587 {
10588 bgotonextstatement=false;
10589 {
10590 IkReal j3eval[1];
10591 CheckValue<IkReal> x813 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10592 if(!x813.valid){
10593 continue;
10594 }
10595 IkReal x812=((-1.0)*(x813.value));
10596 sj4=0;
10597 cj4=-1.0;
10598 j4=3.14159265358979;
10599 sj5=gconst7;
10600 cj5=gconst8;
10601 j5=x812;
10602 new_r11=0;
10603 new_r00=0;
10604 IkReal gconst6=x812;
10605 IkReal x814 = new_r10*new_r10;
10606 if(IKabs(x814)==0){
10607 continue;
10608 }
10609 IkReal gconst7=((-1.0)*new_r10*(pow(x814,-0.5)));
10610 IkReal gconst8=0;
10611 j3eval[0]=new_r10;
10612 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10613 {
10614 {
10615 IkReal j3array[2], cj3array[2], sj3array[2];
10616 bool j3valid[2]={false};
10617 _nj3 = 2;
10618 CheckValue<IkReal> x815=IKPowWithIntegerCheck(gconst7,-1);
10619 if(!x815.valid){
10620 continue;
10621 }
10622 cj3array[0]=(new_r01*(x815.value));
10623 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10624 {
10625  j3valid[0] = j3valid[1] = true;
10626  j3array[0] = IKacos(cj3array[0]);
10627  sj3array[0] = IKsin(j3array[0]);
10628  cj3array[1] = cj3array[0];
10629  j3array[1] = -j3array[0];
10630  sj3array[1] = -sj3array[0];
10631 }
10632 else if( isnan(cj3array[0]) )
10633 {
10634  // probably any value will work
10635  j3valid[0] = true;
10636  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10637 }
10638 for(int ij3 = 0; ij3 < 2; ++ij3)
10639 {
10640 if( !j3valid[ij3] )
10641 {
10642  continue;
10643 }
10644 _ij3[0] = ij3; _ij3[1] = -1;
10645 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10646 {
10647 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10648 {
10649  j3valid[iij3]=false; _ij3[1] = iij3; break;
10650 }
10651 }
10652 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10653 {
10654 IkReal evalcond[6];
10655 IkReal x816=IKsin(j3);
10656 IkReal x817=IKcos(j3);
10657 IkReal x818=((1.0)*gconst7);
10658 evalcond[0]=(new_r01*x816);
10659 evalcond[1]=(new_r10*x816);
10660 evalcond[2]=(gconst7*x816);
10661 evalcond[3]=((((-1.0)*new_r10*x817))+gconst7);
10662 evalcond[4]=((((-1.0)*x817*x818))+new_r10);
10663 evalcond[5]=(((new_r01*x817))+(((-1.0)*x818)));
10664 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 )
10665 {
10666 continue;
10667 }
10668 }
10669 
10670 {
10671 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10672 vinfos[0].jointtype = 1;
10673 vinfos[0].foffset = j0;
10674 vinfos[0].indices[0] = _ij0[0];
10675 vinfos[0].indices[1] = _ij0[1];
10676 vinfos[0].maxsolutions = _nj0;
10677 vinfos[1].jointtype = 1;
10678 vinfos[1].foffset = j1;
10679 vinfos[1].indices[0] = _ij1[0];
10680 vinfos[1].indices[1] = _ij1[1];
10681 vinfos[1].maxsolutions = _nj1;
10682 vinfos[2].jointtype = 1;
10683 vinfos[2].foffset = j2;
10684 vinfos[2].indices[0] = _ij2[0];
10685 vinfos[2].indices[1] = _ij2[1];
10686 vinfos[2].maxsolutions = _nj2;
10687 vinfos[3].jointtype = 1;
10688 vinfos[3].foffset = j3;
10689 vinfos[3].indices[0] = _ij3[0];
10690 vinfos[3].indices[1] = _ij3[1];
10691 vinfos[3].maxsolutions = _nj3;
10692 vinfos[4].jointtype = 1;
10693 vinfos[4].foffset = j4;
10694 vinfos[4].indices[0] = _ij4[0];
10695 vinfos[4].indices[1] = _ij4[1];
10696 vinfos[4].maxsolutions = _nj4;
10697 vinfos[5].jointtype = 1;
10698 vinfos[5].foffset = j5;
10699 vinfos[5].indices[0] = _ij5[0];
10700 vinfos[5].indices[1] = _ij5[1];
10701 vinfos[5].maxsolutions = _nj5;
10702 std::vector<int> vfree(0);
10703 solutions.AddSolution(vinfos,vfree);
10704 }
10705 }
10706 }
10707 
10708 } else
10709 {
10710 {
10711 IkReal j3array[2], cj3array[2], sj3array[2];
10712 bool j3valid[2]={false};
10713 _nj3 = 2;
10714 CheckValue<IkReal> x819=IKPowWithIntegerCheck(new_r10,-1);
10715 if(!x819.valid){
10716 continue;
10717 }
10718 cj3array[0]=(gconst7*(x819.value));
10719 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10720 {
10721  j3valid[0] = j3valid[1] = true;
10722  j3array[0] = IKacos(cj3array[0]);
10723  sj3array[0] = IKsin(j3array[0]);
10724  cj3array[1] = cj3array[0];
10725  j3array[1] = -j3array[0];
10726  sj3array[1] = -sj3array[0];
10727 }
10728 else if( isnan(cj3array[0]) )
10729 {
10730  // probably any value will work
10731  j3valid[0] = true;
10732  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10733 }
10734 for(int ij3 = 0; ij3 < 2; ++ij3)
10735 {
10736 if( !j3valid[ij3] )
10737 {
10738  continue;
10739 }
10740 _ij3[0] = ij3; _ij3[1] = -1;
10741 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10742 {
10743 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10744 {
10745  j3valid[iij3]=false; _ij3[1] = iij3; break;
10746 }
10747 }
10748 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10749 {
10750 IkReal evalcond[6];
10751 IkReal x820=IKsin(j3);
10752 IkReal x821=IKcos(j3);
10753 IkReal x822=((1.0)*gconst7);
10754 IkReal x823=(x821*x822);
10755 evalcond[0]=(new_r01*x820);
10756 evalcond[1]=(new_r10*x820);
10757 evalcond[2]=(gconst7*x820);
10758 evalcond[3]=((((-1.0)*x823))+new_r01);
10759 evalcond[4]=((((-1.0)*x823))+new_r10);
10760 evalcond[5]=(((new_r01*x821))+(((-1.0)*x822)));
10761 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 )
10762 {
10763 continue;
10764 }
10765 }
10766 
10767 {
10768 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10769 vinfos[0].jointtype = 1;
10770 vinfos[0].foffset = j0;
10771 vinfos[0].indices[0] = _ij0[0];
10772 vinfos[0].indices[1] = _ij0[1];
10773 vinfos[0].maxsolutions = _nj0;
10774 vinfos[1].jointtype = 1;
10775 vinfos[1].foffset = j1;
10776 vinfos[1].indices[0] = _ij1[0];
10777 vinfos[1].indices[1] = _ij1[1];
10778 vinfos[1].maxsolutions = _nj1;
10779 vinfos[2].jointtype = 1;
10780 vinfos[2].foffset = j2;
10781 vinfos[2].indices[0] = _ij2[0];
10782 vinfos[2].indices[1] = _ij2[1];
10783 vinfos[2].maxsolutions = _nj2;
10784 vinfos[3].jointtype = 1;
10785 vinfos[3].foffset = j3;
10786 vinfos[3].indices[0] = _ij3[0];
10787 vinfos[3].indices[1] = _ij3[1];
10788 vinfos[3].maxsolutions = _nj3;
10789 vinfos[4].jointtype = 1;
10790 vinfos[4].foffset = j4;
10791 vinfos[4].indices[0] = _ij4[0];
10792 vinfos[4].indices[1] = _ij4[1];
10793 vinfos[4].maxsolutions = _nj4;
10794 vinfos[5].jointtype = 1;
10795 vinfos[5].foffset = j5;
10796 vinfos[5].indices[0] = _ij5[0];
10797 vinfos[5].indices[1] = _ij5[1];
10798 vinfos[5].maxsolutions = _nj5;
10799 std::vector<int> vfree(0);
10800 solutions.AddSolution(vinfos,vfree);
10801 }
10802 }
10803 }
10804 
10805 }
10806 
10807 }
10808 
10809 }
10810 } while(0);
10811 if( bgotonextstatement )
10812 {
10813 bool bgotonextstatement = true;
10814 do
10815 {
10816 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
10817 evalcond[1]=gconst8;
10818 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
10819 {
10820 bgotonextstatement=false;
10821 {
10822 IkReal j3eval[3];
10823 IkReal x824=((-1.0)*new_r00);
10824 CheckValue<IkReal> x826 = IKatan2WithCheck(IkReal(new_r10),IkReal(x824),IKFAST_ATAN2_MAGTHRESH);
10825 if(!x826.valid){
10826 continue;
10827 }
10828 IkReal x825=((-1.0)*(x826.value));
10829 sj4=0;
10830 cj4=-1.0;
10831 j4=3.14159265358979;
10832 sj5=gconst7;
10833 cj5=gconst8;
10834 j5=x825;
10835 new_r11=0;
10836 new_r01=0;
10837 new_r22=0;
10838 new_r20=0;
10839 IkReal gconst6=x825;
10840 IkReal gconst7=((-1.0)*new_r10);
10841 IkReal gconst8=x824;
10842 j3eval[0]=1.0;
10843 j3eval[1]=1.0;
10844 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
10845 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10846 {
10847 {
10848 IkReal j3eval[3];
10849 IkReal x827=((-1.0)*new_r00);
10850 CheckValue<IkReal> x829 = IKatan2WithCheck(IkReal(new_r10),IkReal(x827),IKFAST_ATAN2_MAGTHRESH);
10851 if(!x829.valid){
10852 continue;
10853 }
10854 IkReal x828=((-1.0)*(x829.value));
10855 sj4=0;
10856 cj4=-1.0;
10857 j4=3.14159265358979;
10858 sj5=gconst7;
10859 cj5=gconst8;
10860 j5=x828;
10861 new_r11=0;
10862 new_r01=0;
10863 new_r22=0;
10864 new_r20=0;
10865 IkReal gconst6=x828;
10866 IkReal gconst7=((-1.0)*new_r10);
10867 IkReal gconst8=x827;
10868 j3eval[0]=-1.0;
10869 j3eval[1]=((IKabs(((-1.0)+(new_r10*new_r10))))+(IKabs((new_r00*new_r10))));
10870 j3eval[2]=-1.0;
10871 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10872 {
10873 {
10874 IkReal j3eval[3];
10875 IkReal x830=((-1.0)*new_r00);
10876 CheckValue<IkReal> x832 = IKatan2WithCheck(IkReal(new_r10),IkReal(x830),IKFAST_ATAN2_MAGTHRESH);
10877 if(!x832.valid){
10878 continue;
10879 }
10880 IkReal x831=((-1.0)*(x832.value));
10881 sj4=0;
10882 cj4=-1.0;
10883 j4=3.14159265358979;
10884 sj5=gconst7;
10885 cj5=gconst8;
10886 j5=x831;
10887 new_r11=0;
10888 new_r01=0;
10889 new_r22=0;
10890 new_r20=0;
10891 IkReal gconst6=x831;
10892 IkReal gconst7=((-1.0)*new_r10);
10893 IkReal gconst8=x830;
10894 j3eval[0]=1.0;
10895 j3eval[1]=((((0.5)*(IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
10896 j3eval[2]=1.0;
10897 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10898 {
10899 continue; // 3 cases reached
10900 
10901 } else
10902 {
10903 {
10904 IkReal j3array[1], cj3array[1], sj3array[1];
10905 bool j3valid[1]={false};
10906 _nj3 = 1;
10907 CheckValue<IkReal> x833=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
10908 if(!x833.valid){
10909 continue;
10910 }
10911 CheckValue<IkReal> x834 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
10912 if(!x834.valid){
10913 continue;
10914 }
10915 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x833.value)))+(x834.value));
10916 sj3array[0]=IKsin(j3array[0]);
10917 cj3array[0]=IKcos(j3array[0]);
10918 if( j3array[0] > IKPI )
10919 {
10920  j3array[0]-=IK2PI;
10921 }
10922 else if( j3array[0] < -IKPI )
10923 { j3array[0]+=IK2PI;
10924 }
10925 j3valid[0] = true;
10926 for(int ij3 = 0; ij3 < 1; ++ij3)
10927 {
10928 if( !j3valid[ij3] )
10929 {
10930  continue;
10931 }
10932 _ij3[0] = ij3; _ij3[1] = -1;
10933 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10934 {
10935 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10936 {
10937  j3valid[iij3]=false; _ij3[1] = iij3; break;
10938 }
10939 }
10940 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10941 {
10942 IkReal evalcond[6];
10943 IkReal x835=IKsin(j3);
10944 IkReal x836=IKcos(j3);
10945 IkReal x837=(gconst8*x835);
10946 IkReal x838=(gconst7*x835);
10947 IkReal x839=(gconst8*x836);
10948 IkReal x840=((1.0)*x836);
10949 IkReal x841=(gconst7*x840);
10950 evalcond[0]=((((-1.0)*x841))+x837);
10951 evalcond[1]=(gconst8+((new_r00*x836))+((new_r10*x835)));
10952 evalcond[2]=(new_r00+x838+x839);
10953 evalcond[3]=((((-1.0)*new_r10*x840))+gconst7+((new_r00*x835)));
10954 evalcond[4]=((((-1.0)*x838))+(((-1.0)*x839)));
10955 evalcond[5]=((((-1.0)*x841))+new_r10+x837);
10956 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 )
10957 {
10958 continue;
10959 }
10960 }
10961 
10962 {
10963 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10964 vinfos[0].jointtype = 1;
10965 vinfos[0].foffset = j0;
10966 vinfos[0].indices[0] = _ij0[0];
10967 vinfos[0].indices[1] = _ij0[1];
10968 vinfos[0].maxsolutions = _nj0;
10969 vinfos[1].jointtype = 1;
10970 vinfos[1].foffset = j1;
10971 vinfos[1].indices[0] = _ij1[0];
10972 vinfos[1].indices[1] = _ij1[1];
10973 vinfos[1].maxsolutions = _nj1;
10974 vinfos[2].jointtype = 1;
10975 vinfos[2].foffset = j2;
10976 vinfos[2].indices[0] = _ij2[0];
10977 vinfos[2].indices[1] = _ij2[1];
10978 vinfos[2].maxsolutions = _nj2;
10979 vinfos[3].jointtype = 1;
10980 vinfos[3].foffset = j3;
10981 vinfos[3].indices[0] = _ij3[0];
10982 vinfos[3].indices[1] = _ij3[1];
10983 vinfos[3].maxsolutions = _nj3;
10984 vinfos[4].jointtype = 1;
10985 vinfos[4].foffset = j4;
10986 vinfos[4].indices[0] = _ij4[0];
10987 vinfos[4].indices[1] = _ij4[1];
10988 vinfos[4].maxsolutions = _nj4;
10989 vinfos[5].jointtype = 1;
10990 vinfos[5].foffset = j5;
10991 vinfos[5].indices[0] = _ij5[0];
10992 vinfos[5].indices[1] = _ij5[1];
10993 vinfos[5].maxsolutions = _nj5;
10994 std::vector<int> vfree(0);
10995 solutions.AddSolution(vinfos,vfree);
10996 }
10997 }
10998 }
10999 
11000 }
11001 
11002 }
11003 
11004 } else
11005 {
11006 {
11007 IkReal j3array[1], cj3array[1], sj3array[1];
11008 bool j3valid[1]={false};
11009 _nj3 = 1;
11010 CheckValue<IkReal> x842 = IKatan2WithCheck(IkReal((gconst7*new_r00)),IkReal((gconst8*new_r00)),IKFAST_ATAN2_MAGTHRESH);
11011 if(!x842.valid){
11012 continue;
11013 }
11014 CheckValue<IkReal> x843=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
11015 if(!x843.valid){
11016 continue;
11017 }
11018 j3array[0]=((-1.5707963267949)+(x842.value)+(((1.5707963267949)*(x843.value))));
11019 sj3array[0]=IKsin(j3array[0]);
11020 cj3array[0]=IKcos(j3array[0]);
11021 if( j3array[0] > IKPI )
11022 {
11023  j3array[0]-=IK2PI;
11024 }
11025 else if( j3array[0] < -IKPI )
11026 { j3array[0]+=IK2PI;
11027 }
11028 j3valid[0] = true;
11029 for(int ij3 = 0; ij3 < 1; ++ij3)
11030 {
11031 if( !j3valid[ij3] )
11032 {
11033  continue;
11034 }
11035 _ij3[0] = ij3; _ij3[1] = -1;
11036 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11037 {
11038 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11039 {
11040  j3valid[iij3]=false; _ij3[1] = iij3; break;
11041 }
11042 }
11043 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11044 {
11045 IkReal evalcond[6];
11046 IkReal x844=IKsin(j3);
11047 IkReal x845=IKcos(j3);
11048 IkReal x846=(gconst8*x844);
11049 IkReal x847=(gconst7*x844);
11050 IkReal x848=(gconst8*x845);
11051 IkReal x849=((1.0)*x845);
11052 IkReal x850=(gconst7*x849);
11053 evalcond[0]=((((-1.0)*x850))+x846);
11054 evalcond[1]=(((new_r10*x844))+gconst8+((new_r00*x845)));
11055 evalcond[2]=(new_r00+x847+x848);
11056 evalcond[3]=((((-1.0)*new_r10*x849))+gconst7+((new_r00*x844)));
11057 evalcond[4]=((((-1.0)*x848))+(((-1.0)*x847)));
11058 evalcond[5]=((((-1.0)*x850))+new_r10+x846);
11059 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 )
11060 {
11061 continue;
11062 }
11063 }
11064 
11065 {
11066 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11067 vinfos[0].jointtype = 1;
11068 vinfos[0].foffset = j0;
11069 vinfos[0].indices[0] = _ij0[0];
11070 vinfos[0].indices[1] = _ij0[1];
11071 vinfos[0].maxsolutions = _nj0;
11072 vinfos[1].jointtype = 1;
11073 vinfos[1].foffset = j1;
11074 vinfos[1].indices[0] = _ij1[0];
11075 vinfos[1].indices[1] = _ij1[1];
11076 vinfos[1].maxsolutions = _nj1;
11077 vinfos[2].jointtype = 1;
11078 vinfos[2].foffset = j2;
11079 vinfos[2].indices[0] = _ij2[0];
11080 vinfos[2].indices[1] = _ij2[1];
11081 vinfos[2].maxsolutions = _nj2;
11082 vinfos[3].jointtype = 1;
11083 vinfos[3].foffset = j3;
11084 vinfos[3].indices[0] = _ij3[0];
11085 vinfos[3].indices[1] = _ij3[1];
11086 vinfos[3].maxsolutions = _nj3;
11087 vinfos[4].jointtype = 1;
11088 vinfos[4].foffset = j4;
11089 vinfos[4].indices[0] = _ij4[0];
11090 vinfos[4].indices[1] = _ij4[1];
11091 vinfos[4].maxsolutions = _nj4;
11092 vinfos[5].jointtype = 1;
11093 vinfos[5].foffset = j5;
11094 vinfos[5].indices[0] = _ij5[0];
11095 vinfos[5].indices[1] = _ij5[1];
11096 vinfos[5].maxsolutions = _nj5;
11097 std::vector<int> vfree(0);
11098 solutions.AddSolution(vinfos,vfree);
11099 }
11100 }
11101 }
11102 
11103 }
11104 
11105 }
11106 
11107 } else
11108 {
11109 {
11110 IkReal j3array[1], cj3array[1], sj3array[1];
11111 bool j3valid[1]={false};
11112 _nj3 = 1;
11113 CheckValue<IkReal> x851 = IKatan2WithCheck(IkReal((gconst7*gconst8)),IkReal(gconst8*gconst8),IKFAST_ATAN2_MAGTHRESH);
11114 if(!x851.valid){
11115 continue;
11116 }
11117 CheckValue<IkReal> x852=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11118 if(!x852.valid){
11119 continue;
11120 }
11121 j3array[0]=((-1.5707963267949)+(x851.value)+(((1.5707963267949)*(x852.value))));
11122 sj3array[0]=IKsin(j3array[0]);
11123 cj3array[0]=IKcos(j3array[0]);
11124 if( j3array[0] > IKPI )
11125 {
11126  j3array[0]-=IK2PI;
11127 }
11128 else if( j3array[0] < -IKPI )
11129 { j3array[0]+=IK2PI;
11130 }
11131 j3valid[0] = true;
11132 for(int ij3 = 0; ij3 < 1; ++ij3)
11133 {
11134 if( !j3valid[ij3] )
11135 {
11136  continue;
11137 }
11138 _ij3[0] = ij3; _ij3[1] = -1;
11139 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11140 {
11141 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11142 {
11143  j3valid[iij3]=false; _ij3[1] = iij3; break;
11144 }
11145 }
11146 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11147 {
11148 IkReal evalcond[6];
11149 IkReal x853=IKsin(j3);
11150 IkReal x854=IKcos(j3);
11151 IkReal x855=(gconst8*x853);
11152 IkReal x856=(gconst7*x853);
11153 IkReal x857=(gconst8*x854);
11154 IkReal x858=((1.0)*x854);
11155 IkReal x859=(gconst7*x858);
11156 evalcond[0]=((((-1.0)*x859))+x855);
11157 evalcond[1]=(gconst8+((new_r10*x853))+((new_r00*x854)));
11158 evalcond[2]=(new_r00+x856+x857);
11159 evalcond[3]=(gconst7+(((-1.0)*new_r10*x858))+((new_r00*x853)));
11160 evalcond[4]=((((-1.0)*x856))+(((-1.0)*x857)));
11161 evalcond[5]=((((-1.0)*x859))+new_r10+x855);
11162 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 )
11163 {
11164 continue;
11165 }
11166 }
11167 
11168 {
11169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11170 vinfos[0].jointtype = 1;
11171 vinfos[0].foffset = j0;
11172 vinfos[0].indices[0] = _ij0[0];
11173 vinfos[0].indices[1] = _ij0[1];
11174 vinfos[0].maxsolutions = _nj0;
11175 vinfos[1].jointtype = 1;
11176 vinfos[1].foffset = j1;
11177 vinfos[1].indices[0] = _ij1[0];
11178 vinfos[1].indices[1] = _ij1[1];
11179 vinfos[1].maxsolutions = _nj1;
11180 vinfos[2].jointtype = 1;
11181 vinfos[2].foffset = j2;
11182 vinfos[2].indices[0] = _ij2[0];
11183 vinfos[2].indices[1] = _ij2[1];
11184 vinfos[2].maxsolutions = _nj2;
11185 vinfos[3].jointtype = 1;
11186 vinfos[3].foffset = j3;
11187 vinfos[3].indices[0] = _ij3[0];
11188 vinfos[3].indices[1] = _ij3[1];
11189 vinfos[3].maxsolutions = _nj3;
11190 vinfos[4].jointtype = 1;
11191 vinfos[4].foffset = j4;
11192 vinfos[4].indices[0] = _ij4[0];
11193 vinfos[4].indices[1] = _ij4[1];
11194 vinfos[4].maxsolutions = _nj4;
11195 vinfos[5].jointtype = 1;
11196 vinfos[5].foffset = j5;
11197 vinfos[5].indices[0] = _ij5[0];
11198 vinfos[5].indices[1] = _ij5[1];
11199 vinfos[5].maxsolutions = _nj5;
11200 std::vector<int> vfree(0);
11201 solutions.AddSolution(vinfos,vfree);
11202 }
11203 }
11204 }
11205 
11206 }
11207 
11208 }
11209 
11210 }
11211 } while(0);
11212 if( bgotonextstatement )
11213 {
11214 bool bgotonextstatement = true;
11215 do
11216 {
11217 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
11218 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11219 {
11220 bgotonextstatement=false;
11221 {
11222 IkReal j3eval[1];
11223 IkReal x860=((-1.0)*new_r00);
11224 CheckValue<IkReal> x862 = IKatan2WithCheck(IkReal(0),IkReal(x860),IKFAST_ATAN2_MAGTHRESH);
11225 if(!x862.valid){
11226 continue;
11227 }
11228 IkReal x861=((-1.0)*(x862.value));
11229 sj4=0;
11230 cj4=-1.0;
11231 j4=3.14159265358979;
11232 sj5=gconst7;
11233 cj5=gconst8;
11234 j5=x861;
11235 new_r01=0;
11236 new_r10=0;
11237 IkReal gconst6=x861;
11238 IkReal gconst7=0;
11239 IkReal x863 = new_r00*new_r00;
11240 if(IKabs(x863)==0){
11241 continue;
11242 }
11243 IkReal gconst8=(x860*(pow(x863,-0.5)));
11244 j3eval[0]=new_r11;
11245 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11246 {
11247 {
11248 IkReal j3array[2], cj3array[2], sj3array[2];
11249 bool j3valid[2]={false};
11250 _nj3 = 2;
11251 CheckValue<IkReal> x864=IKPowWithIntegerCheck(gconst8,-1);
11252 if(!x864.valid){
11253 continue;
11254 }
11255 cj3array[0]=(new_r11*(x864.value));
11256 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11257 {
11258  j3valid[0] = j3valid[1] = true;
11259  j3array[0] = IKacos(cj3array[0]);
11260  sj3array[0] = IKsin(j3array[0]);
11261  cj3array[1] = cj3array[0];
11262  j3array[1] = -j3array[0];
11263  sj3array[1] = -sj3array[0];
11264 }
11265 else if( isnan(cj3array[0]) )
11266 {
11267  // probably any value will work
11268  j3valid[0] = true;
11269  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11270 }
11271 for(int ij3 = 0; ij3 < 2; ++ij3)
11272 {
11273 if( !j3valid[ij3] )
11274 {
11275  continue;
11276 }
11277 _ij3[0] = ij3; _ij3[1] = -1;
11278 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11279 {
11280 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11281 {
11282  j3valid[iij3]=false; _ij3[1] = iij3; break;
11283 }
11284 }
11285 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11286 {
11287 IkReal evalcond[6];
11288 IkReal x865=IKsin(j3);
11289 IkReal x866=IKcos(j3);
11290 evalcond[0]=(new_r00*x865);
11291 evalcond[1]=(new_r11*x865);
11292 evalcond[2]=(gconst8*x865);
11293 evalcond[3]=(((new_r00*x866))+gconst8);
11294 evalcond[4]=(((gconst8*x866))+new_r00);
11295 evalcond[5]=(gconst8+(((-1.0)*new_r11*x866)));
11296 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 )
11297 {
11298 continue;
11299 }
11300 }
11301 
11302 {
11303 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11304 vinfos[0].jointtype = 1;
11305 vinfos[0].foffset = j0;
11306 vinfos[0].indices[0] = _ij0[0];
11307 vinfos[0].indices[1] = _ij0[1];
11308 vinfos[0].maxsolutions = _nj0;
11309 vinfos[1].jointtype = 1;
11310 vinfos[1].foffset = j1;
11311 vinfos[1].indices[0] = _ij1[0];
11312 vinfos[1].indices[1] = _ij1[1];
11313 vinfos[1].maxsolutions = _nj1;
11314 vinfos[2].jointtype = 1;
11315 vinfos[2].foffset = j2;
11316 vinfos[2].indices[0] = _ij2[0];
11317 vinfos[2].indices[1] = _ij2[1];
11318 vinfos[2].maxsolutions = _nj2;
11319 vinfos[3].jointtype = 1;
11320 vinfos[3].foffset = j3;
11321 vinfos[3].indices[0] = _ij3[0];
11322 vinfos[3].indices[1] = _ij3[1];
11323 vinfos[3].maxsolutions = _nj3;
11324 vinfos[4].jointtype = 1;
11325 vinfos[4].foffset = j4;
11326 vinfos[4].indices[0] = _ij4[0];
11327 vinfos[4].indices[1] = _ij4[1];
11328 vinfos[4].maxsolutions = _nj4;
11329 vinfos[5].jointtype = 1;
11330 vinfos[5].foffset = j5;
11331 vinfos[5].indices[0] = _ij5[0];
11332 vinfos[5].indices[1] = _ij5[1];
11333 vinfos[5].maxsolutions = _nj5;
11334 std::vector<int> vfree(0);
11335 solutions.AddSolution(vinfos,vfree);
11336 }
11337 }
11338 }
11339 
11340 } else
11341 {
11342 {
11343 IkReal j3array[2], cj3array[2], sj3array[2];
11344 bool j3valid[2]={false};
11345 _nj3 = 2;
11346 CheckValue<IkReal> x867=IKPowWithIntegerCheck(new_r11,-1);
11347 if(!x867.valid){
11348 continue;
11349 }
11350 cj3array[0]=(gconst8*(x867.value));
11351 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11352 {
11353  j3valid[0] = j3valid[1] = true;
11354  j3array[0] = IKacos(cj3array[0]);
11355  sj3array[0] = IKsin(j3array[0]);
11356  cj3array[1] = cj3array[0];
11357  j3array[1] = -j3array[0];
11358  sj3array[1] = -sj3array[0];
11359 }
11360 else if( isnan(cj3array[0]) )
11361 {
11362  // probably any value will work
11363  j3valid[0] = true;
11364  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11365 }
11366 for(int ij3 = 0; ij3 < 2; ++ij3)
11367 {
11368 if( !j3valid[ij3] )
11369 {
11370  continue;
11371 }
11372 _ij3[0] = ij3; _ij3[1] = -1;
11373 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11374 {
11375 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11376 {
11377  j3valid[iij3]=false; _ij3[1] = iij3; break;
11378 }
11379 }
11380 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11381 {
11382 IkReal evalcond[6];
11383 IkReal x868=IKsin(j3);
11384 IkReal x869=IKcos(j3);
11385 IkReal x870=(gconst8*x869);
11386 evalcond[0]=(new_r00*x868);
11387 evalcond[1]=(new_r11*x868);
11388 evalcond[2]=(gconst8*x868);
11389 evalcond[3]=(((new_r00*x869))+gconst8);
11390 evalcond[4]=(new_r00+x870);
11391 evalcond[5]=((((-1.0)*x870))+new_r11);
11392 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 )
11393 {
11394 continue;
11395 }
11396 }
11397 
11398 {
11399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11400 vinfos[0].jointtype = 1;
11401 vinfos[0].foffset = j0;
11402 vinfos[0].indices[0] = _ij0[0];
11403 vinfos[0].indices[1] = _ij0[1];
11404 vinfos[0].maxsolutions = _nj0;
11405 vinfos[1].jointtype = 1;
11406 vinfos[1].foffset = j1;
11407 vinfos[1].indices[0] = _ij1[0];
11408 vinfos[1].indices[1] = _ij1[1];
11409 vinfos[1].maxsolutions = _nj1;
11410 vinfos[2].jointtype = 1;
11411 vinfos[2].foffset = j2;
11412 vinfos[2].indices[0] = _ij2[0];
11413 vinfos[2].indices[1] = _ij2[1];
11414 vinfos[2].maxsolutions = _nj2;
11415 vinfos[3].jointtype = 1;
11416 vinfos[3].foffset = j3;
11417 vinfos[3].indices[0] = _ij3[0];
11418 vinfos[3].indices[1] = _ij3[1];
11419 vinfos[3].maxsolutions = _nj3;
11420 vinfos[4].jointtype = 1;
11421 vinfos[4].foffset = j4;
11422 vinfos[4].indices[0] = _ij4[0];
11423 vinfos[4].indices[1] = _ij4[1];
11424 vinfos[4].maxsolutions = _nj4;
11425 vinfos[5].jointtype = 1;
11426 vinfos[5].foffset = j5;
11427 vinfos[5].indices[0] = _ij5[0];
11428 vinfos[5].indices[1] = _ij5[1];
11429 vinfos[5].maxsolutions = _nj5;
11430 std::vector<int> vfree(0);
11431 solutions.AddSolution(vinfos,vfree);
11432 }
11433 }
11434 }
11435 
11436 }
11437 
11438 }
11439 
11440 }
11441 } while(0);
11442 if( bgotonextstatement )
11443 {
11444 bool bgotonextstatement = true;
11445 do
11446 {
11447 evalcond[0]=IKabs(new_r00);
11448 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11449 {
11450 bgotonextstatement=false;
11451 {
11452 IkReal j3eval[1];
11453 CheckValue<IkReal> x872 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11454 if(!x872.valid){
11455 continue;
11456 }
11457 IkReal x871=((-1.0)*(x872.value));
11458 sj4=0;
11459 cj4=-1.0;
11460 j4=3.14159265358979;
11461 sj5=gconst7;
11462 cj5=gconst8;
11463 j5=x871;
11464 new_r00=0;
11465 IkReal gconst6=x871;
11466 IkReal x873 = new_r10*new_r10;
11467 if(IKabs(x873)==0){
11468 continue;
11469 }
11470 IkReal gconst7=((-1.0)*new_r10*(pow(x873,-0.5)));
11471 IkReal gconst8=0;
11472 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11473 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11474 {
11475 {
11476 IkReal j3eval[1];
11477 CheckValue<IkReal> x875 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11478 if(!x875.valid){
11479 continue;
11480 }
11481 IkReal x874=((-1.0)*(x875.value));
11482 sj4=0;
11483 cj4=-1.0;
11484 j4=3.14159265358979;
11485 sj5=gconst7;
11486 cj5=gconst8;
11487 j5=x874;
11488 new_r00=0;
11489 IkReal gconst6=x874;
11490 IkReal x876 = new_r10*new_r10;
11491 if(IKabs(x876)==0){
11492 continue;
11493 }
11494 IkReal gconst7=((-1.0)*new_r10*(pow(x876,-0.5)));
11495 IkReal gconst8=0;
11496 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11497 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11498 {
11499 {
11500 IkReal j3eval[1];
11501 CheckValue<IkReal> x878 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11502 if(!x878.valid){
11503 continue;
11504 }
11505 IkReal x877=((-1.0)*(x878.value));
11506 sj4=0;
11507 cj4=-1.0;
11508 j4=3.14159265358979;
11509 sj5=gconst7;
11510 cj5=gconst8;
11511 j5=x877;
11512 new_r00=0;
11513 IkReal gconst6=x877;
11514 IkReal x879 = new_r10*new_r10;
11515 if(IKabs(x879)==0){
11516 continue;
11517 }
11518 IkReal gconst7=((-1.0)*new_r10*(pow(x879,-0.5)));
11519 IkReal gconst8=0;
11520 j3eval[0]=new_r10;
11521 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11522 {
11523 continue; // 3 cases reached
11524 
11525 } else
11526 {
11527 {
11528 IkReal j3array[1], cj3array[1], sj3array[1];
11529 bool j3valid[1]={false};
11530 _nj3 = 1;
11531 CheckValue<IkReal> x880=IKPowWithIntegerCheck(gconst7,-1);
11532 if(!x880.valid){
11533 continue;
11534 }
11535 CheckValue<IkReal> x881=IKPowWithIntegerCheck(new_r10,-1);
11536 if(!x881.valid){
11537 continue;
11538 }
11539 if( IKabs((new_r11*(x880.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst7*(x881.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x880.value)))+IKsqr((gconst7*(x881.value)))-1) <= IKFAST_SINCOS_THRESH )
11540  continue;
11541 j3array[0]=IKatan2((new_r11*(x880.value)), (gconst7*(x881.value)));
11542 sj3array[0]=IKsin(j3array[0]);
11543 cj3array[0]=IKcos(j3array[0]);
11544 if( j3array[0] > IKPI )
11545 {
11546  j3array[0]-=IK2PI;
11547 }
11548 else if( j3array[0] < -IKPI )
11549 { j3array[0]+=IK2PI;
11550 }
11551 j3valid[0] = true;
11552 for(int ij3 = 0; ij3 < 1; ++ij3)
11553 {
11554 if( !j3valid[ij3] )
11555 {
11556  continue;
11557 }
11558 _ij3[0] = ij3; _ij3[1] = -1;
11559 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11560 {
11561 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11562 {
11563  j3valid[iij3]=false; _ij3[1] = iij3; break;
11564 }
11565 }
11566 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11567 {
11568 IkReal evalcond[8];
11569 IkReal x882=IKsin(j3);
11570 IkReal x883=IKcos(j3);
11571 IkReal x884=((1.0)*gconst7);
11572 IkReal x885=((1.0)*x883);
11573 IkReal x886=(x883*x884);
11574 evalcond[0]=(new_r10*x882);
11575 evalcond[1]=(gconst7*x882);
11576 evalcond[2]=((((-1.0)*new_r10*x885))+gconst7);
11577 evalcond[3]=((((-1.0)*x886))+new_r01);
11578 evalcond[4]=((((-1.0)*x882*x884))+new_r11);
11579 evalcond[5]=((((-1.0)*x886))+new_r10);
11580 evalcond[6]=(((new_r01*x882))+(((-1.0)*new_r11*x885)));
11581 evalcond[7]=(((new_r01*x883))+((new_r11*x882))+(((-1.0)*x884)));
11582 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 )
11583 {
11584 continue;
11585 }
11586 }
11587 
11588 {
11589 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11590 vinfos[0].jointtype = 1;
11591 vinfos[0].foffset = j0;
11592 vinfos[0].indices[0] = _ij0[0];
11593 vinfos[0].indices[1] = _ij0[1];
11594 vinfos[0].maxsolutions = _nj0;
11595 vinfos[1].jointtype = 1;
11596 vinfos[1].foffset = j1;
11597 vinfos[1].indices[0] = _ij1[0];
11598 vinfos[1].indices[1] = _ij1[1];
11599 vinfos[1].maxsolutions = _nj1;
11600 vinfos[2].jointtype = 1;
11601 vinfos[2].foffset = j2;
11602 vinfos[2].indices[0] = _ij2[0];
11603 vinfos[2].indices[1] = _ij2[1];
11604 vinfos[2].maxsolutions = _nj2;
11605 vinfos[3].jointtype = 1;
11606 vinfos[3].foffset = j3;
11607 vinfos[3].indices[0] = _ij3[0];
11608 vinfos[3].indices[1] = _ij3[1];
11609 vinfos[3].maxsolutions = _nj3;
11610 vinfos[4].jointtype = 1;
11611 vinfos[4].foffset = j4;
11612 vinfos[4].indices[0] = _ij4[0];
11613 vinfos[4].indices[1] = _ij4[1];
11614 vinfos[4].maxsolutions = _nj4;
11615 vinfos[5].jointtype = 1;
11616 vinfos[5].foffset = j5;
11617 vinfos[5].indices[0] = _ij5[0];
11618 vinfos[5].indices[1] = _ij5[1];
11619 vinfos[5].maxsolutions = _nj5;
11620 std::vector<int> vfree(0);
11621 solutions.AddSolution(vinfos,vfree);
11622 }
11623 }
11624 }
11625 
11626 }
11627 
11628 }
11629 
11630 } else
11631 {
11632 {
11633 IkReal j3array[1], cj3array[1], sj3array[1];
11634 bool j3valid[1]={false};
11635 _nj3 = 1;
11636 CheckValue<IkReal> x887=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11637 if(!x887.valid){
11638 continue;
11639 }
11640 CheckValue<IkReal> x888 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11641 if(!x888.valid){
11642 continue;
11643 }
11644 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x887.value)))+(x888.value));
11645 sj3array[0]=IKsin(j3array[0]);
11646 cj3array[0]=IKcos(j3array[0]);
11647 if( j3array[0] > IKPI )
11648 {
11649  j3array[0]-=IK2PI;
11650 }
11651 else if( j3array[0] < -IKPI )
11652 { j3array[0]+=IK2PI;
11653 }
11654 j3valid[0] = true;
11655 for(int ij3 = 0; ij3 < 1; ++ij3)
11656 {
11657 if( !j3valid[ij3] )
11658 {
11659  continue;
11660 }
11661 _ij3[0] = ij3; _ij3[1] = -1;
11662 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11663 {
11664 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11665 {
11666  j3valid[iij3]=false; _ij3[1] = iij3; break;
11667 }
11668 }
11669 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11670 {
11671 IkReal evalcond[8];
11672 IkReal x889=IKsin(j3);
11673 IkReal x890=IKcos(j3);
11674 IkReal x891=((1.0)*gconst7);
11675 IkReal x892=((1.0)*x890);
11676 IkReal x893=(x890*x891);
11677 evalcond[0]=(new_r10*x889);
11678 evalcond[1]=(gconst7*x889);
11679 evalcond[2]=(gconst7+(((-1.0)*new_r10*x892)));
11680 evalcond[3]=((((-1.0)*x893))+new_r01);
11681 evalcond[4]=((((-1.0)*x889*x891))+new_r11);
11682 evalcond[5]=((((-1.0)*x893))+new_r10);
11683 evalcond[6]=(((new_r01*x889))+(((-1.0)*new_r11*x892)));
11684 evalcond[7]=(((new_r11*x889))+((new_r01*x890))+(((-1.0)*x891)));
11685 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 )
11686 {
11687 continue;
11688 }
11689 }
11690 
11691 {
11692 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11693 vinfos[0].jointtype = 1;
11694 vinfos[0].foffset = j0;
11695 vinfos[0].indices[0] = _ij0[0];
11696 vinfos[0].indices[1] = _ij0[1];
11697 vinfos[0].maxsolutions = _nj0;
11698 vinfos[1].jointtype = 1;
11699 vinfos[1].foffset = j1;
11700 vinfos[1].indices[0] = _ij1[0];
11701 vinfos[1].indices[1] = _ij1[1];
11702 vinfos[1].maxsolutions = _nj1;
11703 vinfos[2].jointtype = 1;
11704 vinfos[2].foffset = j2;
11705 vinfos[2].indices[0] = _ij2[0];
11706 vinfos[2].indices[1] = _ij2[1];
11707 vinfos[2].maxsolutions = _nj2;
11708 vinfos[3].jointtype = 1;
11709 vinfos[3].foffset = j3;
11710 vinfos[3].indices[0] = _ij3[0];
11711 vinfos[3].indices[1] = _ij3[1];
11712 vinfos[3].maxsolutions = _nj3;
11713 vinfos[4].jointtype = 1;
11714 vinfos[4].foffset = j4;
11715 vinfos[4].indices[0] = _ij4[0];
11716 vinfos[4].indices[1] = _ij4[1];
11717 vinfos[4].maxsolutions = _nj4;
11718 vinfos[5].jointtype = 1;
11719 vinfos[5].foffset = j5;
11720 vinfos[5].indices[0] = _ij5[0];
11721 vinfos[5].indices[1] = _ij5[1];
11722 vinfos[5].maxsolutions = _nj5;
11723 std::vector<int> vfree(0);
11724 solutions.AddSolution(vinfos,vfree);
11725 }
11726 }
11727 }
11728 
11729 }
11730 
11731 }
11732 
11733 } else
11734 {
11735 {
11736 IkReal j3array[1], cj3array[1], sj3array[1];
11737 bool j3valid[1]={false};
11738 _nj3 = 1;
11739 CheckValue<IkReal> x894=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11740 if(!x894.valid){
11741 continue;
11742 }
11743 CheckValue<IkReal> x895 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
11744 if(!x895.valid){
11745 continue;
11746 }
11747 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x894.value)))+(x895.value));
11748 sj3array[0]=IKsin(j3array[0]);
11749 cj3array[0]=IKcos(j3array[0]);
11750 if( j3array[0] > IKPI )
11751 {
11752  j3array[0]-=IK2PI;
11753 }
11754 else if( j3array[0] < -IKPI )
11755 { j3array[0]+=IK2PI;
11756 }
11757 j3valid[0] = true;
11758 for(int ij3 = 0; ij3 < 1; ++ij3)
11759 {
11760 if( !j3valid[ij3] )
11761 {
11762  continue;
11763 }
11764 _ij3[0] = ij3; _ij3[1] = -1;
11765 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11766 {
11767 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11768 {
11769  j3valid[iij3]=false; _ij3[1] = iij3; break;
11770 }
11771 }
11772 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11773 {
11774 IkReal evalcond[8];
11775 IkReal x896=IKsin(j3);
11776 IkReal x897=IKcos(j3);
11777 IkReal x898=((1.0)*gconst7);
11778 IkReal x899=((1.0)*x897);
11779 IkReal x900=(x897*x898);
11780 evalcond[0]=(new_r10*x896);
11781 evalcond[1]=(gconst7*x896);
11782 evalcond[2]=(gconst7+(((-1.0)*new_r10*x899)));
11783 evalcond[3]=((((-1.0)*x900))+new_r01);
11784 evalcond[4]=((((-1.0)*x896*x898))+new_r11);
11785 evalcond[5]=((((-1.0)*x900))+new_r10);
11786 evalcond[6]=(((new_r01*x896))+(((-1.0)*new_r11*x899)));
11787 evalcond[7]=(((new_r11*x896))+((new_r01*x897))+(((-1.0)*x898)));
11788 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 )
11789 {
11790 continue;
11791 }
11792 }
11793 
11794 {
11795 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11796 vinfos[0].jointtype = 1;
11797 vinfos[0].foffset = j0;
11798 vinfos[0].indices[0] = _ij0[0];
11799 vinfos[0].indices[1] = _ij0[1];
11800 vinfos[0].maxsolutions = _nj0;
11801 vinfos[1].jointtype = 1;
11802 vinfos[1].foffset = j1;
11803 vinfos[1].indices[0] = _ij1[0];
11804 vinfos[1].indices[1] = _ij1[1];
11805 vinfos[1].maxsolutions = _nj1;
11806 vinfos[2].jointtype = 1;
11807 vinfos[2].foffset = j2;
11808 vinfos[2].indices[0] = _ij2[0];
11809 vinfos[2].indices[1] = _ij2[1];
11810 vinfos[2].maxsolutions = _nj2;
11811 vinfos[3].jointtype = 1;
11812 vinfos[3].foffset = j3;
11813 vinfos[3].indices[0] = _ij3[0];
11814 vinfos[3].indices[1] = _ij3[1];
11815 vinfos[3].maxsolutions = _nj3;
11816 vinfos[4].jointtype = 1;
11817 vinfos[4].foffset = j4;
11818 vinfos[4].indices[0] = _ij4[0];
11819 vinfos[4].indices[1] = _ij4[1];
11820 vinfos[4].maxsolutions = _nj4;
11821 vinfos[5].jointtype = 1;
11822 vinfos[5].foffset = j5;
11823 vinfos[5].indices[0] = _ij5[0];
11824 vinfos[5].indices[1] = _ij5[1];
11825 vinfos[5].maxsolutions = _nj5;
11826 std::vector<int> vfree(0);
11827 solutions.AddSolution(vinfos,vfree);
11828 }
11829 }
11830 }
11831 
11832 }
11833 
11834 }
11835 
11836 }
11837 } while(0);
11838 if( bgotonextstatement )
11839 {
11840 bool bgotonextstatement = true;
11841 do
11842 {
11843 if( 1 )
11844 {
11845 bgotonextstatement=false;
11846 continue; // branch miss [j3]
11847 
11848 }
11849 } while(0);
11850 if( bgotonextstatement )
11851 {
11852 }
11853 }
11854 }
11855 }
11856 }
11857 }
11858 
11859 } else
11860 {
11861 {
11862 IkReal j3array[1], cj3array[1], sj3array[1];
11863 bool j3valid[1]={false};
11864 _nj3 = 1;
11865 CheckValue<IkReal> x901=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11866 if(!x901.valid){
11867 continue;
11868 }
11869 CheckValue<IkReal> x902 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
11870 if(!x902.valid){
11871 continue;
11872 }
11873 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x901.value)))+(x902.value));
11874 sj3array[0]=IKsin(j3array[0]);
11875 cj3array[0]=IKcos(j3array[0]);
11876 if( j3array[0] > IKPI )
11877 {
11878  j3array[0]-=IK2PI;
11879 }
11880 else if( j3array[0] < -IKPI )
11881 { j3array[0]+=IK2PI;
11882 }
11883 j3valid[0] = true;
11884 for(int ij3 = 0; ij3 < 1; ++ij3)
11885 {
11886 if( !j3valid[ij3] )
11887 {
11888  continue;
11889 }
11890 _ij3[0] = ij3; _ij3[1] = -1;
11891 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11892 {
11893 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11894 {
11895  j3valid[iij3]=false; _ij3[1] = iij3; break;
11896 }
11897 }
11898 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11899 {
11900 IkReal evalcond[8];
11901 IkReal x903=IKcos(j3);
11902 IkReal x904=IKsin(j3);
11903 IkReal x905=(gconst8*x904);
11904 IkReal x906=(gconst7*x904);
11905 IkReal x907=((1.0)*x903);
11906 IkReal x908=(gconst7*x907);
11907 evalcond[0]=(gconst8+((new_r10*x904))+((new_r00*x903)));
11908 evalcond[1]=(new_r00+((gconst8*x903))+x906);
11909 evalcond[2]=(gconst7+((new_r00*x904))+(((-1.0)*new_r10*x907)));
11910 evalcond[3]=((((-1.0)*new_r11*x907))+gconst8+((new_r01*x904)));
11911 evalcond[4]=((((-1.0)*x908))+new_r01+x905);
11912 evalcond[5]=((((-1.0)*x908))+new_r10+x905);
11913 evalcond[6]=((((-1.0)*gconst7))+((new_r11*x904))+((new_r01*x903)));
11914 evalcond[7]=((((-1.0)*gconst8*x907))+(((-1.0)*x906))+new_r11);
11915 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 )
11916 {
11917 continue;
11918 }
11919 }
11920 
11921 {
11922 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11923 vinfos[0].jointtype = 1;
11924 vinfos[0].foffset = j0;
11925 vinfos[0].indices[0] = _ij0[0];
11926 vinfos[0].indices[1] = _ij0[1];
11927 vinfos[0].maxsolutions = _nj0;
11928 vinfos[1].jointtype = 1;
11929 vinfos[1].foffset = j1;
11930 vinfos[1].indices[0] = _ij1[0];
11931 vinfos[1].indices[1] = _ij1[1];
11932 vinfos[1].maxsolutions = _nj1;
11933 vinfos[2].jointtype = 1;
11934 vinfos[2].foffset = j2;
11935 vinfos[2].indices[0] = _ij2[0];
11936 vinfos[2].indices[1] = _ij2[1];
11937 vinfos[2].maxsolutions = _nj2;
11938 vinfos[3].jointtype = 1;
11939 vinfos[3].foffset = j3;
11940 vinfos[3].indices[0] = _ij3[0];
11941 vinfos[3].indices[1] = _ij3[1];
11942 vinfos[3].maxsolutions = _nj3;
11943 vinfos[4].jointtype = 1;
11944 vinfos[4].foffset = j4;
11945 vinfos[4].indices[0] = _ij4[0];
11946 vinfos[4].indices[1] = _ij4[1];
11947 vinfos[4].maxsolutions = _nj4;
11948 vinfos[5].jointtype = 1;
11949 vinfos[5].foffset = j5;
11950 vinfos[5].indices[0] = _ij5[0];
11951 vinfos[5].indices[1] = _ij5[1];
11952 vinfos[5].maxsolutions = _nj5;
11953 std::vector<int> vfree(0);
11954 solutions.AddSolution(vinfos,vfree);
11955 }
11956 }
11957 }
11958 
11959 }
11960 
11961 }
11962 
11963 } else
11964 {
11965 {
11966 IkReal j3array[1], cj3array[1], sj3array[1];
11967 bool j3valid[1]={false};
11968 _nj3 = 1;
11969 IkReal x909=((1.0)*new_r10);
11970 CheckValue<IkReal> x910=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*x909))+(((-1.0)*gconst8*new_r00)))),-1);
11971 if(!x910.valid){
11972 continue;
11973 }
11974 CheckValue<IkReal> x911 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*new_r01*x909)))),IKFAST_ATAN2_MAGTHRESH);
11975 if(!x911.valid){
11976 continue;
11977 }
11978 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x910.value)))+(x911.value));
11979 sj3array[0]=IKsin(j3array[0]);
11980 cj3array[0]=IKcos(j3array[0]);
11981 if( j3array[0] > IKPI )
11982 {
11983  j3array[0]-=IK2PI;
11984 }
11985 else if( j3array[0] < -IKPI )
11986 { j3array[0]+=IK2PI;
11987 }
11988 j3valid[0] = true;
11989 for(int ij3 = 0; ij3 < 1; ++ij3)
11990 {
11991 if( !j3valid[ij3] )
11992 {
11993  continue;
11994 }
11995 _ij3[0] = ij3; _ij3[1] = -1;
11996 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11997 {
11998 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11999 {
12000  j3valid[iij3]=false; _ij3[1] = iij3; break;
12001 }
12002 }
12003 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12004 {
12005 IkReal evalcond[8];
12006 IkReal x912=IKcos(j3);
12007 IkReal x913=IKsin(j3);
12008 IkReal x914=(gconst8*x913);
12009 IkReal x915=(gconst7*x913);
12010 IkReal x916=((1.0)*x912);
12011 IkReal x917=(gconst7*x916);
12012 evalcond[0]=(gconst8+((new_r10*x913))+((new_r00*x912)));
12013 evalcond[1]=(((gconst8*x912))+new_r00+x915);
12014 evalcond[2]=(gconst7+((new_r00*x913))+(((-1.0)*new_r10*x916)));
12015 evalcond[3]=(gconst8+((new_r01*x913))+(((-1.0)*new_r11*x916)));
12016 evalcond[4]=((((-1.0)*x917))+new_r01+x914);
12017 evalcond[5]=((((-1.0)*x917))+new_r10+x914);
12018 evalcond[6]=(((new_r11*x913))+(((-1.0)*gconst7))+((new_r01*x912)));
12019 evalcond[7]=((((-1.0)*gconst8*x916))+(((-1.0)*x915))+new_r11);
12020 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 )
12021 {
12022 continue;
12023 }
12024 }
12025 
12026 {
12027 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12028 vinfos[0].jointtype = 1;
12029 vinfos[0].foffset = j0;
12030 vinfos[0].indices[0] = _ij0[0];
12031 vinfos[0].indices[1] = _ij0[1];
12032 vinfos[0].maxsolutions = _nj0;
12033 vinfos[1].jointtype = 1;
12034 vinfos[1].foffset = j1;
12035 vinfos[1].indices[0] = _ij1[0];
12036 vinfos[1].indices[1] = _ij1[1];
12037 vinfos[1].maxsolutions = _nj1;
12038 vinfos[2].jointtype = 1;
12039 vinfos[2].foffset = j2;
12040 vinfos[2].indices[0] = _ij2[0];
12041 vinfos[2].indices[1] = _ij2[1];
12042 vinfos[2].maxsolutions = _nj2;
12043 vinfos[3].jointtype = 1;
12044 vinfos[3].foffset = j3;
12045 vinfos[3].indices[0] = _ij3[0];
12046 vinfos[3].indices[1] = _ij3[1];
12047 vinfos[3].maxsolutions = _nj3;
12048 vinfos[4].jointtype = 1;
12049 vinfos[4].foffset = j4;
12050 vinfos[4].indices[0] = _ij4[0];
12051 vinfos[4].indices[1] = _ij4[1];
12052 vinfos[4].maxsolutions = _nj4;
12053 vinfos[5].jointtype = 1;
12054 vinfos[5].foffset = j5;
12055 vinfos[5].indices[0] = _ij5[0];
12056 vinfos[5].indices[1] = _ij5[1];
12057 vinfos[5].maxsolutions = _nj5;
12058 std::vector<int> vfree(0);
12059 solutions.AddSolution(vinfos,vfree);
12060 }
12061 }
12062 }
12063 
12064 }
12065 
12066 }
12067 
12068 } else
12069 {
12070 {
12071 IkReal j3array[1], cj3array[1], sj3array[1];
12072 bool j3valid[1]={false};
12073 _nj3 = 1;
12074 IkReal x918=((1.0)*new_r10);
12075 CheckValue<IkReal> x919 = IKatan2WithCheck(IkReal((((gconst8*new_r11))+((gconst8*new_r00)))),IkReal((((gconst8*new_r01))+(((-1.0)*gconst8*x918)))),IKFAST_ATAN2_MAGTHRESH);
12076 if(!x919.valid){
12077 continue;
12078 }
12079 CheckValue<IkReal> x920=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x918)))),-1);
12080 if(!x920.valid){
12081 continue;
12082 }
12083 j3array[0]=((-1.5707963267949)+(x919.value)+(((1.5707963267949)*(x920.value))));
12084 sj3array[0]=IKsin(j3array[0]);
12085 cj3array[0]=IKcos(j3array[0]);
12086 if( j3array[0] > IKPI )
12087 {
12088  j3array[0]-=IK2PI;
12089 }
12090 else if( j3array[0] < -IKPI )
12091 { j3array[0]+=IK2PI;
12092 }
12093 j3valid[0] = true;
12094 for(int ij3 = 0; ij3 < 1; ++ij3)
12095 {
12096 if( !j3valid[ij3] )
12097 {
12098  continue;
12099 }
12100 _ij3[0] = ij3; _ij3[1] = -1;
12101 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12102 {
12103 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12104 {
12105  j3valid[iij3]=false; _ij3[1] = iij3; break;
12106 }
12107 }
12108 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12109 {
12110 IkReal evalcond[8];
12111 IkReal x921=IKcos(j3);
12112 IkReal x922=IKsin(j3);
12113 IkReal x923=(gconst8*x922);
12114 IkReal x924=(gconst7*x922);
12115 IkReal x925=((1.0)*x921);
12116 IkReal x926=(gconst7*x925);
12117 evalcond[0]=(gconst8+((new_r00*x921))+((new_r10*x922)));
12118 evalcond[1]=(((gconst8*x921))+new_r00+x924);
12119 evalcond[2]=(gconst7+(((-1.0)*new_r10*x925))+((new_r00*x922)));
12120 evalcond[3]=((((-1.0)*new_r11*x925))+gconst8+((new_r01*x922)));
12121 evalcond[4]=((((-1.0)*x926))+new_r01+x923);
12122 evalcond[5]=((((-1.0)*x926))+new_r10+x923);
12123 evalcond[6]=((((-1.0)*gconst7))+((new_r01*x921))+((new_r11*x922)));
12124 evalcond[7]=((((-1.0)*gconst8*x925))+(((-1.0)*x924))+new_r11);
12125 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 )
12126 {
12127 continue;
12128 }
12129 }
12130 
12131 {
12132 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12133 vinfos[0].jointtype = 1;
12134 vinfos[0].foffset = j0;
12135 vinfos[0].indices[0] = _ij0[0];
12136 vinfos[0].indices[1] = _ij0[1];
12137 vinfos[0].maxsolutions = _nj0;
12138 vinfos[1].jointtype = 1;
12139 vinfos[1].foffset = j1;
12140 vinfos[1].indices[0] = _ij1[0];
12141 vinfos[1].indices[1] = _ij1[1];
12142 vinfos[1].maxsolutions = _nj1;
12143 vinfos[2].jointtype = 1;
12144 vinfos[2].foffset = j2;
12145 vinfos[2].indices[0] = _ij2[0];
12146 vinfos[2].indices[1] = _ij2[1];
12147 vinfos[2].maxsolutions = _nj2;
12148 vinfos[3].jointtype = 1;
12149 vinfos[3].foffset = j3;
12150 vinfos[3].indices[0] = _ij3[0];
12151 vinfos[3].indices[1] = _ij3[1];
12152 vinfos[3].maxsolutions = _nj3;
12153 vinfos[4].jointtype = 1;
12154 vinfos[4].foffset = j4;
12155 vinfos[4].indices[0] = _ij4[0];
12156 vinfos[4].indices[1] = _ij4[1];
12157 vinfos[4].maxsolutions = _nj4;
12158 vinfos[5].jointtype = 1;
12159 vinfos[5].foffset = j5;
12160 vinfos[5].indices[0] = _ij5[0];
12161 vinfos[5].indices[1] = _ij5[1];
12162 vinfos[5].maxsolutions = _nj5;
12163 std::vector<int> vfree(0);
12164 solutions.AddSolution(vinfos,vfree);
12165 }
12166 }
12167 }
12168 
12169 }
12170 
12171 }
12172 
12173 }
12174 } while(0);
12175 if( bgotonextstatement )
12176 {
12177 bool bgotonextstatement = true;
12178 do
12179 {
12180 IkReal x929 = ((new_r10*new_r10)+(new_r00*new_r00));
12181 if(IKabs(x929)==0){
12182 continue;
12183 }
12184 IkReal x927=pow(x929,-0.5);
12185 IkReal x928=((1.0)*x927);
12186 CheckValue<IkReal> x930 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12187 if(!x930.valid){
12188 continue;
12189 }
12190 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x930.value))));
12191 IkReal gconst10=(new_r10*x928);
12192 IkReal gconst11=(new_r00*x928);
12193 CheckValue<IkReal> x931 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12194 if(!x931.valid){
12195 continue;
12196 }
12197 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5+(x931.value))))), 6.28318530717959)));
12198 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12199 {
12200 bgotonextstatement=false;
12201 {
12202 IkReal j3eval[3];
12203 CheckValue<IkReal> x935 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12204 if(!x935.valid){
12205 continue;
12206 }
12207 IkReal x932=((1.0)*(x935.value));
12208 IkReal x933=x927;
12209 IkReal x934=((1.0)*x933);
12210 sj4=0;
12211 cj4=-1.0;
12212 j4=3.14159265358979;
12213 sj5=gconst10;
12214 cj5=gconst11;
12215 j5=((3.14159265)+(((-1.0)*x932)));
12216 IkReal gconst9=((3.14159265358979)+(((-1.0)*x932)));
12217 IkReal gconst10=(new_r10*x934);
12218 IkReal gconst11=(new_r00*x934);
12219 IkReal x936=new_r00*new_r00;
12220 IkReal x937=((1.0)*new_r00);
12221 IkReal x938=((((-1.0)*new_r01*x937))+(((-1.0)*new_r10*new_r11)));
12222 IkReal x939=x927;
12223 IkReal x940=(new_r00*x939);
12224 j3eval[0]=x938;
12225 j3eval[1]=((IKabs((((new_r01*x940))+(((-1.0)*new_r10*x937*x939)))))+(IKabs((((x936*x939))+((new_r11*x940))))));
12226 j3eval[2]=IKsign(x938);
12227 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12228 {
12229 {
12230 IkReal j3eval[1];
12231 CheckValue<IkReal> x944 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12232 if(!x944.valid){
12233 continue;
12234 }
12235 IkReal x941=((1.0)*(x944.value));
12236 IkReal x942=x927;
12237 IkReal x943=((1.0)*x942);
12238 sj4=0;
12239 cj4=-1.0;
12240 j4=3.14159265358979;
12241 sj5=gconst10;
12242 cj5=gconst11;
12243 j5=((3.14159265)+(((-1.0)*x941)));
12244 IkReal gconst9=((3.14159265358979)+(((-1.0)*x941)));
12245 IkReal gconst10=(new_r10*x943);
12246 IkReal gconst11=(new_r00*x943);
12247 IkReal x945=new_r10*new_r10;
12248 IkReal x946=new_r00*new_r00;
12249 IkReal x947=((1.0)*new_r01);
12250 CheckValue<IkReal> x951=IKPowWithIntegerCheck((x945+x946),-1);
12251 if(!x951.valid){
12252 continue;
12253 }
12254 IkReal x948=x951.value;
12255 IkReal x949=(new_r10*x948);
12256 IkReal x950=(new_r01*x948);
12257 j3eval[0]=((IKabs((((new_r00*x949))+((new_r00*x945*x950))+((x950*(new_r00*new_r00*new_r00))))))+(IKabs((((x946*x948))+(((-1.0)*x947*x949*(new_r10*new_r10)))+(((-1.0)*x946*x947*x949))))));
12258 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12259 {
12260 {
12261 IkReal j3eval[1];
12262 CheckValue<IkReal> x955 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12263 if(!x955.valid){
12264 continue;
12265 }
12266 IkReal x952=((1.0)*(x955.value));
12267 IkReal x953=x927;
12268 IkReal x954=((1.0)*x953);
12269 sj4=0;
12270 cj4=-1.0;
12271 j4=3.14159265358979;
12272 sj5=gconst10;
12273 cj5=gconst11;
12274 j5=((3.14159265)+(((-1.0)*x952)));
12275 IkReal gconst9=((3.14159265358979)+(((-1.0)*x952)));
12276 IkReal gconst10=(new_r10*x954);
12277 IkReal gconst11=(new_r00*x954);
12278 IkReal x956=new_r00*new_r00;
12279 IkReal x957=new_r10*new_r10;
12280 CheckValue<IkReal> x961=IKPowWithIntegerCheck((x957+x956),-1);
12281 if(!x961.valid){
12282 continue;
12283 }
12284 IkReal x958=x961.value;
12285 IkReal x959=(new_r10*x958);
12286 IkReal x960=((1.0)*x958);
12287 j3eval[0]=((IKabs(((((-1.0)*x956*x957*x960))+((x956*x958))+(((-1.0)*x960*(x957*x957))))))+(IKabs((((new_r00*x959))+((x959*(new_r00*new_r00*new_r00)))+((new_r00*x959*(new_r10*new_r10)))))));
12288 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12289 {
12290 {
12291 IkReal evalcond[2];
12292 bool bgotonextstatement = true;
12293 do
12294 {
12295 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12296 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12297 {
12298 bgotonextstatement=false;
12299 {
12300 IkReal j3eval[1];
12301 CheckValue<IkReal> x963 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12302 if(!x963.valid){
12303 continue;
12304 }
12305 IkReal x962=((1.0)*(x963.value));
12306 sj4=0;
12307 cj4=-1.0;
12308 j4=3.14159265358979;
12309 sj5=gconst10;
12310 cj5=gconst11;
12311 j5=((3.14159265)+(((-1.0)*x962)));
12312 new_r11=0;
12313 new_r00=0;
12314 IkReal gconst9=((3.14159265358979)+(((-1.0)*x962)));
12315 IkReal x964 = new_r10*new_r10;
12316 if(IKabs(x964)==0){
12317 continue;
12318 }
12319 IkReal gconst10=((1.0)*new_r10*(pow(x964,-0.5)));
12320 IkReal gconst11=0;
12321 j3eval[0]=new_r10;
12322 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12323 {
12324 {
12325 IkReal j3array[2], cj3array[2], sj3array[2];
12326 bool j3valid[2]={false};
12327 _nj3 = 2;
12328 CheckValue<IkReal> x965=IKPowWithIntegerCheck(gconst10,-1);
12329 if(!x965.valid){
12330 continue;
12331 }
12332 cj3array[0]=(new_r01*(x965.value));
12333 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12334 {
12335  j3valid[0] = j3valid[1] = true;
12336  j3array[0] = IKacos(cj3array[0]);
12337  sj3array[0] = IKsin(j3array[0]);
12338  cj3array[1] = cj3array[0];
12339  j3array[1] = -j3array[0];
12340  sj3array[1] = -sj3array[0];
12341 }
12342 else if( isnan(cj3array[0]) )
12343 {
12344  // probably any value will work
12345  j3valid[0] = true;
12346  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12347 }
12348 for(int ij3 = 0; ij3 < 2; ++ij3)
12349 {
12350 if( !j3valid[ij3] )
12351 {
12352  continue;
12353 }
12354 _ij3[0] = ij3; _ij3[1] = -1;
12355 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12356 {
12357 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12358 {
12359  j3valid[iij3]=false; _ij3[1] = iij3; break;
12360 }
12361 }
12362 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12363 {
12364 IkReal evalcond[6];
12365 IkReal x966=IKsin(j3);
12366 IkReal x967=IKcos(j3);
12367 IkReal x968=((1.0)*gconst10);
12368 evalcond[0]=(new_r01*x966);
12369 evalcond[1]=(new_r10*x966);
12370 evalcond[2]=(gconst10*x966);
12371 evalcond[3]=(gconst10+(((-1.0)*new_r10*x967)));
12372 evalcond[4]=((((-1.0)*x967*x968))+new_r10);
12373 evalcond[5]=(((new_r01*x967))+(((-1.0)*x968)));
12374 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 )
12375 {
12376 continue;
12377 }
12378 }
12379 
12380 {
12381 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12382 vinfos[0].jointtype = 1;
12383 vinfos[0].foffset = j0;
12384 vinfos[0].indices[0] = _ij0[0];
12385 vinfos[0].indices[1] = _ij0[1];
12386 vinfos[0].maxsolutions = _nj0;
12387 vinfos[1].jointtype = 1;
12388 vinfos[1].foffset = j1;
12389 vinfos[1].indices[0] = _ij1[0];
12390 vinfos[1].indices[1] = _ij1[1];
12391 vinfos[1].maxsolutions = _nj1;
12392 vinfos[2].jointtype = 1;
12393 vinfos[2].foffset = j2;
12394 vinfos[2].indices[0] = _ij2[0];
12395 vinfos[2].indices[1] = _ij2[1];
12396 vinfos[2].maxsolutions = _nj2;
12397 vinfos[3].jointtype = 1;
12398 vinfos[3].foffset = j3;
12399 vinfos[3].indices[0] = _ij3[0];
12400 vinfos[3].indices[1] = _ij3[1];
12401 vinfos[3].maxsolutions = _nj3;
12402 vinfos[4].jointtype = 1;
12403 vinfos[4].foffset = j4;
12404 vinfos[4].indices[0] = _ij4[0];
12405 vinfos[4].indices[1] = _ij4[1];
12406 vinfos[4].maxsolutions = _nj4;
12407 vinfos[5].jointtype = 1;
12408 vinfos[5].foffset = j5;
12409 vinfos[5].indices[0] = _ij5[0];
12410 vinfos[5].indices[1] = _ij5[1];
12411 vinfos[5].maxsolutions = _nj5;
12412 std::vector<int> vfree(0);
12413 solutions.AddSolution(vinfos,vfree);
12414 }
12415 }
12416 }
12417 
12418 } else
12419 {
12420 {
12421 IkReal j3array[2], cj3array[2], sj3array[2];
12422 bool j3valid[2]={false};
12423 _nj3 = 2;
12424 CheckValue<IkReal> x969=IKPowWithIntegerCheck(new_r10,-1);
12425 if(!x969.valid){
12426 continue;
12427 }
12428 cj3array[0]=(gconst10*(x969.value));
12429 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12430 {
12431  j3valid[0] = j3valid[1] = true;
12432  j3array[0] = IKacos(cj3array[0]);
12433  sj3array[0] = IKsin(j3array[0]);
12434  cj3array[1] = cj3array[0];
12435  j3array[1] = -j3array[0];
12436  sj3array[1] = -sj3array[0];
12437 }
12438 else if( isnan(cj3array[0]) )
12439 {
12440  // probably any value will work
12441  j3valid[0] = true;
12442  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12443 }
12444 for(int ij3 = 0; ij3 < 2; ++ij3)
12445 {
12446 if( !j3valid[ij3] )
12447 {
12448  continue;
12449 }
12450 _ij3[0] = ij3; _ij3[1] = -1;
12451 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12452 {
12453 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12454 {
12455  j3valid[iij3]=false; _ij3[1] = iij3; break;
12456 }
12457 }
12458 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12459 {
12460 IkReal evalcond[6];
12461 IkReal x970=IKsin(j3);
12462 IkReal x971=IKcos(j3);
12463 IkReal x972=((1.0)*gconst10);
12464 IkReal x973=(x971*x972);
12465 evalcond[0]=(new_r01*x970);
12466 evalcond[1]=(new_r10*x970);
12467 evalcond[2]=(gconst10*x970);
12468 evalcond[3]=(new_r01+(((-1.0)*x973)));
12469 evalcond[4]=(new_r10+(((-1.0)*x973)));
12470 evalcond[5]=(((new_r01*x971))+(((-1.0)*x972)));
12471 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 )
12472 {
12473 continue;
12474 }
12475 }
12476 
12477 {
12478 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12479 vinfos[0].jointtype = 1;
12480 vinfos[0].foffset = j0;
12481 vinfos[0].indices[0] = _ij0[0];
12482 vinfos[0].indices[1] = _ij0[1];
12483 vinfos[0].maxsolutions = _nj0;
12484 vinfos[1].jointtype = 1;
12485 vinfos[1].foffset = j1;
12486 vinfos[1].indices[0] = _ij1[0];
12487 vinfos[1].indices[1] = _ij1[1];
12488 vinfos[1].maxsolutions = _nj1;
12489 vinfos[2].jointtype = 1;
12490 vinfos[2].foffset = j2;
12491 vinfos[2].indices[0] = _ij2[0];
12492 vinfos[2].indices[1] = _ij2[1];
12493 vinfos[2].maxsolutions = _nj2;
12494 vinfos[3].jointtype = 1;
12495 vinfos[3].foffset = j3;
12496 vinfos[3].indices[0] = _ij3[0];
12497 vinfos[3].indices[1] = _ij3[1];
12498 vinfos[3].maxsolutions = _nj3;
12499 vinfos[4].jointtype = 1;
12500 vinfos[4].foffset = j4;
12501 vinfos[4].indices[0] = _ij4[0];
12502 vinfos[4].indices[1] = _ij4[1];
12503 vinfos[4].maxsolutions = _nj4;
12504 vinfos[5].jointtype = 1;
12505 vinfos[5].foffset = j5;
12506 vinfos[5].indices[0] = _ij5[0];
12507 vinfos[5].indices[1] = _ij5[1];
12508 vinfos[5].maxsolutions = _nj5;
12509 std::vector<int> vfree(0);
12510 solutions.AddSolution(vinfos,vfree);
12511 }
12512 }
12513 }
12514 
12515 }
12516 
12517 }
12518 
12519 }
12520 } while(0);
12521 if( bgotonextstatement )
12522 {
12523 bool bgotonextstatement = true;
12524 do
12525 {
12526 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
12527 evalcond[1]=gconst11;
12528 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
12529 {
12530 bgotonextstatement=false;
12531 {
12532 IkReal j3eval[3];
12533 CheckValue<IkReal> x975 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12534 if(!x975.valid){
12535 continue;
12536 }
12537 IkReal x974=((1.0)*(x975.value));
12538 sj4=0;
12539 cj4=-1.0;
12540 j4=3.14159265358979;
12541 sj5=gconst10;
12542 cj5=gconst11;
12543 j5=((3.14159265)+(((-1.0)*x974)));
12544 new_r11=0;
12545 new_r01=0;
12546 new_r22=0;
12547 new_r20=0;
12548 IkReal gconst9=((3.14159265358979)+(((-1.0)*x974)));
12549 IkReal gconst10=((1.0)*new_r10);
12550 IkReal gconst11=((1.0)*new_r00);
12551 j3eval[0]=-1.0;
12552 j3eval[1]=-1.0;
12553 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12554 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12555 {
12556 {
12557 IkReal j3eval[3];
12558 CheckValue<IkReal> x977 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12559 if(!x977.valid){
12560 continue;
12561 }
12562 IkReal x976=((1.0)*(x977.value));
12563 sj4=0;
12564 cj4=-1.0;
12565 j4=3.14159265358979;
12566 sj5=gconst10;
12567 cj5=gconst11;
12568 j5=((3.14159265)+(((-1.0)*x976)));
12569 new_r11=0;
12570 new_r01=0;
12571 new_r22=0;
12572 new_r20=0;
12573 IkReal gconst9=((3.14159265358979)+(((-1.0)*x976)));
12574 IkReal gconst10=((1.0)*new_r10);
12575 IkReal gconst11=((1.0)*new_r00);
12576 j3eval[0]=-1.0;
12577 j3eval[1]=-1.0;
12578 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12579 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12580 {
12581 {
12582 IkReal j3eval[3];
12583 CheckValue<IkReal> x979 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12584 if(!x979.valid){
12585 continue;
12586 }
12587 IkReal x978=((1.0)*(x979.value));
12588 sj4=0;
12589 cj4=-1.0;
12590 j4=3.14159265358979;
12591 sj5=gconst10;
12592 cj5=gconst11;
12593 j5=((3.14159265)+(((-1.0)*x978)));
12594 new_r11=0;
12595 new_r01=0;
12596 new_r22=0;
12597 new_r20=0;
12598 IkReal gconst9=((3.14159265358979)+(((-1.0)*x978)));
12599 IkReal gconst10=((1.0)*new_r10);
12600 IkReal gconst11=((1.0)*new_r00);
12601 j3eval[0]=-1.0;
12602 j3eval[1]=-1.0;
12603 j3eval[2]=((IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
12604 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12605 {
12606 continue; // 3 cases reached
12607 
12608 } else
12609 {
12610 {
12611 IkReal j3array[1], cj3array[1], sj3array[1];
12612 bool j3valid[1]={false};
12613 _nj3 = 1;
12614 CheckValue<IkReal> x980 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
12615 if(!x980.valid){
12616 continue;
12617 }
12618 CheckValue<IkReal> x981=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12619 if(!x981.valid){
12620 continue;
12621 }
12622 j3array[0]=((-1.5707963267949)+(x980.value)+(((1.5707963267949)*(x981.value))));
12623 sj3array[0]=IKsin(j3array[0]);
12624 cj3array[0]=IKcos(j3array[0]);
12625 if( j3array[0] > IKPI )
12626 {
12627  j3array[0]-=IK2PI;
12628 }
12629 else if( j3array[0] < -IKPI )
12630 { j3array[0]+=IK2PI;
12631 }
12632 j3valid[0] = true;
12633 for(int ij3 = 0; ij3 < 1; ++ij3)
12634 {
12635 if( !j3valid[ij3] )
12636 {
12637  continue;
12638 }
12639 _ij3[0] = ij3; _ij3[1] = -1;
12640 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12641 {
12642 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12643 {
12644  j3valid[iij3]=false; _ij3[1] = iij3; break;
12645 }
12646 }
12647 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12648 {
12649 IkReal evalcond[6];
12650 IkReal x982=IKsin(j3);
12651 IkReal x983=IKcos(j3);
12652 IkReal x984=(gconst11*x982);
12653 IkReal x985=(gconst10*x982);
12654 IkReal x986=(gconst11*x983);
12655 IkReal x987=((1.0)*x983);
12656 IkReal x988=(gconst10*x987);
12657 evalcond[0]=((((-1.0)*x988))+x984);
12658 evalcond[1]=(((new_r00*x983))+gconst11+((new_r10*x982)));
12659 evalcond[2]=(new_r00+x985+x986);
12660 evalcond[3]=(((new_r00*x982))+gconst10+(((-1.0)*new_r10*x987)));
12661 evalcond[4]=((((-1.0)*x985))+(((-1.0)*x986)));
12662 evalcond[5]=((((-1.0)*x988))+new_r10+x984);
12663 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 )
12664 {
12665 continue;
12666 }
12667 }
12668 
12669 {
12670 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12671 vinfos[0].jointtype = 1;
12672 vinfos[0].foffset = j0;
12673 vinfos[0].indices[0] = _ij0[0];
12674 vinfos[0].indices[1] = _ij0[1];
12675 vinfos[0].maxsolutions = _nj0;
12676 vinfos[1].jointtype = 1;
12677 vinfos[1].foffset = j1;
12678 vinfos[1].indices[0] = _ij1[0];
12679 vinfos[1].indices[1] = _ij1[1];
12680 vinfos[1].maxsolutions = _nj1;
12681 vinfos[2].jointtype = 1;
12682 vinfos[2].foffset = j2;
12683 vinfos[2].indices[0] = _ij2[0];
12684 vinfos[2].indices[1] = _ij2[1];
12685 vinfos[2].maxsolutions = _nj2;
12686 vinfos[3].jointtype = 1;
12687 vinfos[3].foffset = j3;
12688 vinfos[3].indices[0] = _ij3[0];
12689 vinfos[3].indices[1] = _ij3[1];
12690 vinfos[3].maxsolutions = _nj3;
12691 vinfos[4].jointtype = 1;
12692 vinfos[4].foffset = j4;
12693 vinfos[4].indices[0] = _ij4[0];
12694 vinfos[4].indices[1] = _ij4[1];
12695 vinfos[4].maxsolutions = _nj4;
12696 vinfos[5].jointtype = 1;
12697 vinfos[5].foffset = j5;
12698 vinfos[5].indices[0] = _ij5[0];
12699 vinfos[5].indices[1] = _ij5[1];
12700 vinfos[5].maxsolutions = _nj5;
12701 std::vector<int> vfree(0);
12702 solutions.AddSolution(vinfos,vfree);
12703 }
12704 }
12705 }
12706 
12707 }
12708 
12709 }
12710 
12711 } else
12712 {
12713 {
12714 IkReal j3array[1], cj3array[1], sj3array[1];
12715 bool j3valid[1]={false};
12716 _nj3 = 1;
12717 CheckValue<IkReal> x989=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
12718 if(!x989.valid){
12719 continue;
12720 }
12721 CheckValue<IkReal> x990 = IKatan2WithCheck(IkReal((gconst10*new_r00)),IkReal((gconst11*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12722 if(!x990.valid){
12723 continue;
12724 }
12725 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x989.value)))+(x990.value));
12726 sj3array[0]=IKsin(j3array[0]);
12727 cj3array[0]=IKcos(j3array[0]);
12728 if( j3array[0] > IKPI )
12729 {
12730  j3array[0]-=IK2PI;
12731 }
12732 else if( j3array[0] < -IKPI )
12733 { j3array[0]+=IK2PI;
12734 }
12735 j3valid[0] = true;
12736 for(int ij3 = 0; ij3 < 1; ++ij3)
12737 {
12738 if( !j3valid[ij3] )
12739 {
12740  continue;
12741 }
12742 _ij3[0] = ij3; _ij3[1] = -1;
12743 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12744 {
12745 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12746 {
12747  j3valid[iij3]=false; _ij3[1] = iij3; break;
12748 }
12749 }
12750 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12751 {
12752 IkReal evalcond[6];
12753 IkReal x991=IKsin(j3);
12754 IkReal x992=IKcos(j3);
12755 IkReal x993=(gconst11*x991);
12756 IkReal x994=(gconst10*x991);
12757 IkReal x995=(gconst11*x992);
12758 IkReal x996=((1.0)*x992);
12759 IkReal x997=(gconst10*x996);
12760 evalcond[0]=((((-1.0)*x997))+x993);
12761 evalcond[1]=(((new_r10*x991))+gconst11+((new_r00*x992)));
12762 evalcond[2]=(new_r00+x995+x994);
12763 evalcond[3]=((((-1.0)*new_r10*x996))+gconst10+((new_r00*x991)));
12764 evalcond[4]=((((-1.0)*x994))+(((-1.0)*x995)));
12765 evalcond[5]=((((-1.0)*x997))+new_r10+x993);
12766 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 )
12767 {
12768 continue;
12769 }
12770 }
12771 
12772 {
12773 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12774 vinfos[0].jointtype = 1;
12775 vinfos[0].foffset = j0;
12776 vinfos[0].indices[0] = _ij0[0];
12777 vinfos[0].indices[1] = _ij0[1];
12778 vinfos[0].maxsolutions = _nj0;
12779 vinfos[1].jointtype = 1;
12780 vinfos[1].foffset = j1;
12781 vinfos[1].indices[0] = _ij1[0];
12782 vinfos[1].indices[1] = _ij1[1];
12783 vinfos[1].maxsolutions = _nj1;
12784 vinfos[2].jointtype = 1;
12785 vinfos[2].foffset = j2;
12786 vinfos[2].indices[0] = _ij2[0];
12787 vinfos[2].indices[1] = _ij2[1];
12788 vinfos[2].maxsolutions = _nj2;
12789 vinfos[3].jointtype = 1;
12790 vinfos[3].foffset = j3;
12791 vinfos[3].indices[0] = _ij3[0];
12792 vinfos[3].indices[1] = _ij3[1];
12793 vinfos[3].maxsolutions = _nj3;
12794 vinfos[4].jointtype = 1;
12795 vinfos[4].foffset = j4;
12796 vinfos[4].indices[0] = _ij4[0];
12797 vinfos[4].indices[1] = _ij4[1];
12798 vinfos[4].maxsolutions = _nj4;
12799 vinfos[5].jointtype = 1;
12800 vinfos[5].foffset = j5;
12801 vinfos[5].indices[0] = _ij5[0];
12802 vinfos[5].indices[1] = _ij5[1];
12803 vinfos[5].maxsolutions = _nj5;
12804 std::vector<int> vfree(0);
12805 solutions.AddSolution(vinfos,vfree);
12806 }
12807 }
12808 }
12809 
12810 }
12811 
12812 }
12813 
12814 } else
12815 {
12816 {
12817 IkReal j3array[1], cj3array[1], sj3array[1];
12818 bool j3valid[1]={false};
12819 _nj3 = 1;
12820 CheckValue<IkReal> x998 = IKatan2WithCheck(IkReal((gconst10*gconst11)),IkReal(gconst11*gconst11),IKFAST_ATAN2_MAGTHRESH);
12821 if(!x998.valid){
12822 continue;
12823 }
12824 CheckValue<IkReal> x999=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12825 if(!x999.valid){
12826 continue;
12827 }
12828 j3array[0]=((-1.5707963267949)+(x998.value)+(((1.5707963267949)*(x999.value))));
12829 sj3array[0]=IKsin(j3array[0]);
12830 cj3array[0]=IKcos(j3array[0]);
12831 if( j3array[0] > IKPI )
12832 {
12833  j3array[0]-=IK2PI;
12834 }
12835 else if( j3array[0] < -IKPI )
12836 { j3array[0]+=IK2PI;
12837 }
12838 j3valid[0] = true;
12839 for(int ij3 = 0; ij3 < 1; ++ij3)
12840 {
12841 if( !j3valid[ij3] )
12842 {
12843  continue;
12844 }
12845 _ij3[0] = ij3; _ij3[1] = -1;
12846 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12847 {
12848 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12849 {
12850  j3valid[iij3]=false; _ij3[1] = iij3; break;
12851 }
12852 }
12853 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12854 {
12855 IkReal evalcond[6];
12856 IkReal x1000=IKsin(j3);
12857 IkReal x1001=IKcos(j3);
12858 IkReal x1002=(gconst11*x1000);
12859 IkReal x1003=(gconst10*x1000);
12860 IkReal x1004=(gconst11*x1001);
12861 IkReal x1005=((1.0)*x1001);
12862 IkReal x1006=(gconst10*x1005);
12863 evalcond[0]=(x1002+(((-1.0)*x1006)));
12864 evalcond[1]=(gconst11+((new_r10*x1000))+((new_r00*x1001)));
12865 evalcond[2]=(x1004+x1003+new_r00);
12866 evalcond[3]=(gconst10+(((-1.0)*new_r10*x1005))+((new_r00*x1000)));
12867 evalcond[4]=((((-1.0)*x1003))+(((-1.0)*x1004)));
12868 evalcond[5]=(x1002+(((-1.0)*x1006))+new_r10);
12869 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 )
12870 {
12871 continue;
12872 }
12873 }
12874 
12875 {
12876 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12877 vinfos[0].jointtype = 1;
12878 vinfos[0].foffset = j0;
12879 vinfos[0].indices[0] = _ij0[0];
12880 vinfos[0].indices[1] = _ij0[1];
12881 vinfos[0].maxsolutions = _nj0;
12882 vinfos[1].jointtype = 1;
12883 vinfos[1].foffset = j1;
12884 vinfos[1].indices[0] = _ij1[0];
12885 vinfos[1].indices[1] = _ij1[1];
12886 vinfos[1].maxsolutions = _nj1;
12887 vinfos[2].jointtype = 1;
12888 vinfos[2].foffset = j2;
12889 vinfos[2].indices[0] = _ij2[0];
12890 vinfos[2].indices[1] = _ij2[1];
12891 vinfos[2].maxsolutions = _nj2;
12892 vinfos[3].jointtype = 1;
12893 vinfos[3].foffset = j3;
12894 vinfos[3].indices[0] = _ij3[0];
12895 vinfos[3].indices[1] = _ij3[1];
12896 vinfos[3].maxsolutions = _nj3;
12897 vinfos[4].jointtype = 1;
12898 vinfos[4].foffset = j4;
12899 vinfos[4].indices[0] = _ij4[0];
12900 vinfos[4].indices[1] = _ij4[1];
12901 vinfos[4].maxsolutions = _nj4;
12902 vinfos[5].jointtype = 1;
12903 vinfos[5].foffset = j5;
12904 vinfos[5].indices[0] = _ij5[0];
12905 vinfos[5].indices[1] = _ij5[1];
12906 vinfos[5].maxsolutions = _nj5;
12907 std::vector<int> vfree(0);
12908 solutions.AddSolution(vinfos,vfree);
12909 }
12910 }
12911 }
12912 
12913 }
12914 
12915 }
12916 
12917 }
12918 } while(0);
12919 if( bgotonextstatement )
12920 {
12921 bool bgotonextstatement = true;
12922 do
12923 {
12924 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12925 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12926 {
12927 bgotonextstatement=false;
12928 {
12929 IkReal j3eval[1];
12930 CheckValue<IkReal> x1008 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12931 if(!x1008.valid){
12932 continue;
12933 }
12934 IkReal x1007=((1.0)*(x1008.value));
12935 sj4=0;
12936 cj4=-1.0;
12937 j4=3.14159265358979;
12938 sj5=gconst10;
12939 cj5=gconst11;
12940 j5=((3.14159265)+(((-1.0)*x1007)));
12941 new_r01=0;
12942 new_r10=0;
12943 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1007)));
12944 IkReal gconst10=0;
12945 IkReal x1009 = new_r00*new_r00;
12946 if(IKabs(x1009)==0){
12947 continue;
12948 }
12949 IkReal gconst11=((1.0)*new_r00*(pow(x1009,-0.5)));
12950 j3eval[0]=new_r11;
12951 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12952 {
12953 {
12954 IkReal j3array[2], cj3array[2], sj3array[2];
12955 bool j3valid[2]={false};
12956 _nj3 = 2;
12957 CheckValue<IkReal> x1010=IKPowWithIntegerCheck(gconst11,-1);
12958 if(!x1010.valid){
12959 continue;
12960 }
12961 cj3array[0]=(new_r11*(x1010.value));
12962 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12963 {
12964  j3valid[0] = j3valid[1] = true;
12965  j3array[0] = IKacos(cj3array[0]);
12966  sj3array[0] = IKsin(j3array[0]);
12967  cj3array[1] = cj3array[0];
12968  j3array[1] = -j3array[0];
12969  sj3array[1] = -sj3array[0];
12970 }
12971 else if( isnan(cj3array[0]) )
12972 {
12973  // probably any value will work
12974  j3valid[0] = true;
12975  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12976 }
12977 for(int ij3 = 0; ij3 < 2; ++ij3)
12978 {
12979 if( !j3valid[ij3] )
12980 {
12981  continue;
12982 }
12983 _ij3[0] = ij3; _ij3[1] = -1;
12984 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12985 {
12986 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12987 {
12988  j3valid[iij3]=false; _ij3[1] = iij3; break;
12989 }
12990 }
12991 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12992 {
12993 IkReal evalcond[6];
12994 IkReal x1011=IKsin(j3);
12995 IkReal x1012=IKcos(j3);
12996 evalcond[0]=(new_r00*x1011);
12997 evalcond[1]=(new_r11*x1011);
12998 evalcond[2]=(gconst11*x1011);
12999 evalcond[3]=(gconst11+((new_r00*x1012)));
13000 evalcond[4]=(((gconst11*x1012))+new_r00);
13001 evalcond[5]=((((-1.0)*new_r11*x1012))+gconst11);
13002 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 )
13003 {
13004 continue;
13005 }
13006 }
13007 
13008 {
13009 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13010 vinfos[0].jointtype = 1;
13011 vinfos[0].foffset = j0;
13012 vinfos[0].indices[0] = _ij0[0];
13013 vinfos[0].indices[1] = _ij0[1];
13014 vinfos[0].maxsolutions = _nj0;
13015 vinfos[1].jointtype = 1;
13016 vinfos[1].foffset = j1;
13017 vinfos[1].indices[0] = _ij1[0];
13018 vinfos[1].indices[1] = _ij1[1];
13019 vinfos[1].maxsolutions = _nj1;
13020 vinfos[2].jointtype = 1;
13021 vinfos[2].foffset = j2;
13022 vinfos[2].indices[0] = _ij2[0];
13023 vinfos[2].indices[1] = _ij2[1];
13024 vinfos[2].maxsolutions = _nj2;
13025 vinfos[3].jointtype = 1;
13026 vinfos[3].foffset = j3;
13027 vinfos[3].indices[0] = _ij3[0];
13028 vinfos[3].indices[1] = _ij3[1];
13029 vinfos[3].maxsolutions = _nj3;
13030 vinfos[4].jointtype = 1;
13031 vinfos[4].foffset = j4;
13032 vinfos[4].indices[0] = _ij4[0];
13033 vinfos[4].indices[1] = _ij4[1];
13034 vinfos[4].maxsolutions = _nj4;
13035 vinfos[5].jointtype = 1;
13036 vinfos[5].foffset = j5;
13037 vinfos[5].indices[0] = _ij5[0];
13038 vinfos[5].indices[1] = _ij5[1];
13039 vinfos[5].maxsolutions = _nj5;
13040 std::vector<int> vfree(0);
13041 solutions.AddSolution(vinfos,vfree);
13042 }
13043 }
13044 }
13045 
13046 } else
13047 {
13048 {
13049 IkReal j3array[2], cj3array[2], sj3array[2];
13050 bool j3valid[2]={false};
13051 _nj3 = 2;
13052 CheckValue<IkReal> x1013=IKPowWithIntegerCheck(new_r11,-1);
13053 if(!x1013.valid){
13054 continue;
13055 }
13056 cj3array[0]=(gconst11*(x1013.value));
13057 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
13058 {
13059  j3valid[0] = j3valid[1] = true;
13060  j3array[0] = IKacos(cj3array[0]);
13061  sj3array[0] = IKsin(j3array[0]);
13062  cj3array[1] = cj3array[0];
13063  j3array[1] = -j3array[0];
13064  sj3array[1] = -sj3array[0];
13065 }
13066 else if( isnan(cj3array[0]) )
13067 {
13068  // probably any value will work
13069  j3valid[0] = true;
13070  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13071 }
13072 for(int ij3 = 0; ij3 < 2; ++ij3)
13073 {
13074 if( !j3valid[ij3] )
13075 {
13076  continue;
13077 }
13078 _ij3[0] = ij3; _ij3[1] = -1;
13079 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
13080 {
13081 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13082 {
13083  j3valid[iij3]=false; _ij3[1] = iij3; break;
13084 }
13085 }
13086 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13087 {
13088 IkReal evalcond[6];
13089 IkReal x1014=IKsin(j3);
13090 IkReal x1015=IKcos(j3);
13091 IkReal x1016=(gconst11*x1015);
13092 evalcond[0]=(new_r00*x1014);
13093 evalcond[1]=(new_r11*x1014);
13094 evalcond[2]=(gconst11*x1014);
13095 evalcond[3]=(gconst11+((new_r00*x1015)));
13096 evalcond[4]=(x1016+new_r00);
13097 evalcond[5]=(new_r11+(((-1.0)*x1016)));
13098 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 )
13099 {
13100 continue;
13101 }
13102 }
13103 
13104 {
13105 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13106 vinfos[0].jointtype = 1;
13107 vinfos[0].foffset = j0;
13108 vinfos[0].indices[0] = _ij0[0];
13109 vinfos[0].indices[1] = _ij0[1];
13110 vinfos[0].maxsolutions = _nj0;
13111 vinfos[1].jointtype = 1;
13112 vinfos[1].foffset = j1;
13113 vinfos[1].indices[0] = _ij1[0];
13114 vinfos[1].indices[1] = _ij1[1];
13115 vinfos[1].maxsolutions = _nj1;
13116 vinfos[2].jointtype = 1;
13117 vinfos[2].foffset = j2;
13118 vinfos[2].indices[0] = _ij2[0];
13119 vinfos[2].indices[1] = _ij2[1];
13120 vinfos[2].maxsolutions = _nj2;
13121 vinfos[3].jointtype = 1;
13122 vinfos[3].foffset = j3;
13123 vinfos[3].indices[0] = _ij3[0];
13124 vinfos[3].indices[1] = _ij3[1];
13125 vinfos[3].maxsolutions = _nj3;
13126 vinfos[4].jointtype = 1;
13127 vinfos[4].foffset = j4;
13128 vinfos[4].indices[0] = _ij4[0];
13129 vinfos[4].indices[1] = _ij4[1];
13130 vinfos[4].maxsolutions = _nj4;
13131 vinfos[5].jointtype = 1;
13132 vinfos[5].foffset = j5;
13133 vinfos[5].indices[0] = _ij5[0];
13134 vinfos[5].indices[1] = _ij5[1];
13135 vinfos[5].maxsolutions = _nj5;
13136 std::vector<int> vfree(0);
13137 solutions.AddSolution(vinfos,vfree);
13138 }
13139 }
13140 }
13141 
13142 }
13143 
13144 }
13145 
13146 }
13147 } while(0);
13148 if( bgotonextstatement )
13149 {
13150 bool bgotonextstatement = true;
13151 do
13152 {
13153 evalcond[0]=IKabs(new_r00);
13154 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13155 {
13156 bgotonextstatement=false;
13157 {
13158 IkReal j3eval[1];
13159 CheckValue<IkReal> x1018 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13160 if(!x1018.valid){
13161 continue;
13162 }
13163 IkReal x1017=((1.0)*(x1018.value));
13164 sj4=0;
13165 cj4=-1.0;
13166 j4=3.14159265358979;
13167 sj5=gconst10;
13168 cj5=gconst11;
13169 j5=((3.14159265)+(((-1.0)*x1017)));
13170 new_r00=0;
13171 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1017)));
13172 IkReal x1019 = new_r10*new_r10;
13173 if(IKabs(x1019)==0){
13174 continue;
13175 }
13176 IkReal gconst10=((1.0)*new_r10*(pow(x1019,-0.5)));
13177 IkReal gconst11=0;
13178 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13179 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13180 {
13181 {
13182 IkReal j3eval[1];
13183 CheckValue<IkReal> x1021 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13184 if(!x1021.valid){
13185 continue;
13186 }
13187 IkReal x1020=((1.0)*(x1021.value));
13188 sj4=0;
13189 cj4=-1.0;
13190 j4=3.14159265358979;
13191 sj5=gconst10;
13192 cj5=gconst11;
13193 j5=((3.14159265)+(((-1.0)*x1020)));
13194 new_r00=0;
13195 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1020)));
13196 IkReal x1022 = new_r10*new_r10;
13197 if(IKabs(x1022)==0){
13198 continue;
13199 }
13200 IkReal gconst10=((1.0)*new_r10*(pow(x1022,-0.5)));
13201 IkReal gconst11=0;
13202 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13203 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13204 {
13205 {
13206 IkReal j3eval[1];
13207 CheckValue<IkReal> x1024 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13208 if(!x1024.valid){
13209 continue;
13210 }
13211 IkReal x1023=((1.0)*(x1024.value));
13212 sj4=0;
13213 cj4=-1.0;
13214 j4=3.14159265358979;
13215 sj5=gconst10;
13216 cj5=gconst11;
13217 j5=((3.14159265)+(((-1.0)*x1023)));
13218 new_r00=0;
13219 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1023)));
13220 IkReal x1025 = new_r10*new_r10;
13221 if(IKabs(x1025)==0){
13222 continue;
13223 }
13224 IkReal gconst10=((1.0)*new_r10*(pow(x1025,-0.5)));
13225 IkReal gconst11=0;
13226 j3eval[0]=new_r10;
13227 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13228 {
13229 continue; // 3 cases reached
13230 
13231 } else
13232 {
13233 {
13234 IkReal j3array[1], cj3array[1], sj3array[1];
13235 bool j3valid[1]={false};
13236 _nj3 = 1;
13237 CheckValue<IkReal> x1026=IKPowWithIntegerCheck(gconst10,-1);
13238 if(!x1026.valid){
13239 continue;
13240 }
13241 CheckValue<IkReal> x1027=IKPowWithIntegerCheck(new_r10,-1);
13242 if(!x1027.valid){
13243 continue;
13244 }
13245 if( IKabs((new_r11*(x1026.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst10*(x1027.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x1026.value)))+IKsqr((gconst10*(x1027.value)))-1) <= IKFAST_SINCOS_THRESH )
13246  continue;
13247 j3array[0]=IKatan2((new_r11*(x1026.value)), (gconst10*(x1027.value)));
13248 sj3array[0]=IKsin(j3array[0]);
13249 cj3array[0]=IKcos(j3array[0]);
13250 if( j3array[0] > IKPI )
13251 {
13252  j3array[0]-=IK2PI;
13253 }
13254 else if( j3array[0] < -IKPI )
13255 { j3array[0]+=IK2PI;
13256 }
13257 j3valid[0] = true;
13258 for(int ij3 = 0; ij3 < 1; ++ij3)
13259 {
13260 if( !j3valid[ij3] )
13261 {
13262  continue;
13263 }
13264 _ij3[0] = ij3; _ij3[1] = -1;
13265 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13266 {
13267 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13268 {
13269  j3valid[iij3]=false; _ij3[1] = iij3; break;
13270 }
13271 }
13272 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13273 {
13274 IkReal evalcond[8];
13275 IkReal x1028=IKsin(j3);
13276 IkReal x1029=IKcos(j3);
13277 IkReal x1030=(gconst10*x1028);
13278 IkReal x1031=((1.0)*x1029);
13279 IkReal x1032=(gconst10*x1031);
13280 evalcond[0]=(new_r10*x1028);
13281 evalcond[1]=x1030;
13282 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1031)));
13283 evalcond[3]=((((-1.0)*x1032))+new_r01);
13284 evalcond[4]=((((-1.0)*x1030))+new_r11);
13285 evalcond[5]=((((-1.0)*x1032))+new_r10);
13286 evalcond[6]=((((-1.0)*new_r11*x1031))+((new_r01*x1028)));
13287 evalcond[7]=(((new_r11*x1028))+(((-1.0)*gconst10))+((new_r01*x1029)));
13288 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 )
13289 {
13290 continue;
13291 }
13292 }
13293 
13294 {
13295 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13296 vinfos[0].jointtype = 1;
13297 vinfos[0].foffset = j0;
13298 vinfos[0].indices[0] = _ij0[0];
13299 vinfos[0].indices[1] = _ij0[1];
13300 vinfos[0].maxsolutions = _nj0;
13301 vinfos[1].jointtype = 1;
13302 vinfos[1].foffset = j1;
13303 vinfos[1].indices[0] = _ij1[0];
13304 vinfos[1].indices[1] = _ij1[1];
13305 vinfos[1].maxsolutions = _nj1;
13306 vinfos[2].jointtype = 1;
13307 vinfos[2].foffset = j2;
13308 vinfos[2].indices[0] = _ij2[0];
13309 vinfos[2].indices[1] = _ij2[1];
13310 vinfos[2].maxsolutions = _nj2;
13311 vinfos[3].jointtype = 1;
13312 vinfos[3].foffset = j3;
13313 vinfos[3].indices[0] = _ij3[0];
13314 vinfos[3].indices[1] = _ij3[1];
13315 vinfos[3].maxsolutions = _nj3;
13316 vinfos[4].jointtype = 1;
13317 vinfos[4].foffset = j4;
13318 vinfos[4].indices[0] = _ij4[0];
13319 vinfos[4].indices[1] = _ij4[1];
13320 vinfos[4].maxsolutions = _nj4;
13321 vinfos[5].jointtype = 1;
13322 vinfos[5].foffset = j5;
13323 vinfos[5].indices[0] = _ij5[0];
13324 vinfos[5].indices[1] = _ij5[1];
13325 vinfos[5].maxsolutions = _nj5;
13326 std::vector<int> vfree(0);
13327 solutions.AddSolution(vinfos,vfree);
13328 }
13329 }
13330 }
13331 
13332 }
13333 
13334 }
13335 
13336 } else
13337 {
13338 {
13339 IkReal j3array[1], cj3array[1], sj3array[1];
13340 bool j3valid[1]={false};
13341 _nj3 = 1;
13342 CheckValue<IkReal> x1033=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13343 if(!x1033.valid){
13344 continue;
13345 }
13346 CheckValue<IkReal> x1034 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13347 if(!x1034.valid){
13348 continue;
13349 }
13350 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1033.value)))+(x1034.value));
13351 sj3array[0]=IKsin(j3array[0]);
13352 cj3array[0]=IKcos(j3array[0]);
13353 if( j3array[0] > IKPI )
13354 {
13355  j3array[0]-=IK2PI;
13356 }
13357 else if( j3array[0] < -IKPI )
13358 { j3array[0]+=IK2PI;
13359 }
13360 j3valid[0] = true;
13361 for(int ij3 = 0; ij3 < 1; ++ij3)
13362 {
13363 if( !j3valid[ij3] )
13364 {
13365  continue;
13366 }
13367 _ij3[0] = ij3; _ij3[1] = -1;
13368 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13369 {
13370 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13371 {
13372  j3valid[iij3]=false; _ij3[1] = iij3; break;
13373 }
13374 }
13375 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13376 {
13377 IkReal evalcond[8];
13378 IkReal x1035=IKsin(j3);
13379 IkReal x1036=IKcos(j3);
13380 IkReal x1037=(gconst10*x1035);
13381 IkReal x1038=((1.0)*x1036);
13382 IkReal x1039=(gconst10*x1038);
13383 evalcond[0]=(new_r10*x1035);
13384 evalcond[1]=x1037;
13385 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1038)));
13386 evalcond[3]=((((-1.0)*x1039))+new_r01);
13387 evalcond[4]=((((-1.0)*x1037))+new_r11);
13388 evalcond[5]=((((-1.0)*x1039))+new_r10);
13389 evalcond[6]=((((-1.0)*new_r11*x1038))+((new_r01*x1035)));
13390 evalcond[7]=(((new_r11*x1035))+(((-1.0)*gconst10))+((new_r01*x1036)));
13391 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 )
13392 {
13393 continue;
13394 }
13395 }
13396 
13397 {
13398 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13399 vinfos[0].jointtype = 1;
13400 vinfos[0].foffset = j0;
13401 vinfos[0].indices[0] = _ij0[0];
13402 vinfos[0].indices[1] = _ij0[1];
13403 vinfos[0].maxsolutions = _nj0;
13404 vinfos[1].jointtype = 1;
13405 vinfos[1].foffset = j1;
13406 vinfos[1].indices[0] = _ij1[0];
13407 vinfos[1].indices[1] = _ij1[1];
13408 vinfos[1].maxsolutions = _nj1;
13409 vinfos[2].jointtype = 1;
13410 vinfos[2].foffset = j2;
13411 vinfos[2].indices[0] = _ij2[0];
13412 vinfos[2].indices[1] = _ij2[1];
13413 vinfos[2].maxsolutions = _nj2;
13414 vinfos[3].jointtype = 1;
13415 vinfos[3].foffset = j3;
13416 vinfos[3].indices[0] = _ij3[0];
13417 vinfos[3].indices[1] = _ij3[1];
13418 vinfos[3].maxsolutions = _nj3;
13419 vinfos[4].jointtype = 1;
13420 vinfos[4].foffset = j4;
13421 vinfos[4].indices[0] = _ij4[0];
13422 vinfos[4].indices[1] = _ij4[1];
13423 vinfos[4].maxsolutions = _nj4;
13424 vinfos[5].jointtype = 1;
13425 vinfos[5].foffset = j5;
13426 vinfos[5].indices[0] = _ij5[0];
13427 vinfos[5].indices[1] = _ij5[1];
13428 vinfos[5].maxsolutions = _nj5;
13429 std::vector<int> vfree(0);
13430 solutions.AddSolution(vinfos,vfree);
13431 }
13432 }
13433 }
13434 
13435 }
13436 
13437 }
13438 
13439 } else
13440 {
13441 {
13442 IkReal j3array[1], cj3array[1], sj3array[1];
13443 bool j3valid[1]={false};
13444 _nj3 = 1;
13445 CheckValue<IkReal> x1040=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13446 if(!x1040.valid){
13447 continue;
13448 }
13449 CheckValue<IkReal> x1041 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
13450 if(!x1041.valid){
13451 continue;
13452 }
13453 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1040.value)))+(x1041.value));
13454 sj3array[0]=IKsin(j3array[0]);
13455 cj3array[0]=IKcos(j3array[0]);
13456 if( j3array[0] > IKPI )
13457 {
13458  j3array[0]-=IK2PI;
13459 }
13460 else if( j3array[0] < -IKPI )
13461 { j3array[0]+=IK2PI;
13462 }
13463 j3valid[0] = true;
13464 for(int ij3 = 0; ij3 < 1; ++ij3)
13465 {
13466 if( !j3valid[ij3] )
13467 {
13468  continue;
13469 }
13470 _ij3[0] = ij3; _ij3[1] = -1;
13471 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13472 {
13473 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13474 {
13475  j3valid[iij3]=false; _ij3[1] = iij3; break;
13476 }
13477 }
13478 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13479 {
13480 IkReal evalcond[8];
13481 IkReal x1042=IKsin(j3);
13482 IkReal x1043=IKcos(j3);
13483 IkReal x1044=(gconst10*x1042);
13484 IkReal x1045=((1.0)*x1043);
13485 IkReal x1046=(gconst10*x1045);
13486 evalcond[0]=(new_r10*x1042);
13487 evalcond[1]=x1044;
13488 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1045)));
13489 evalcond[3]=(new_r01+(((-1.0)*x1046)));
13490 evalcond[4]=((((-1.0)*x1044))+new_r11);
13491 evalcond[5]=(new_r10+(((-1.0)*x1046)));
13492 evalcond[6]=(((new_r01*x1042))+(((-1.0)*new_r11*x1045)));
13493 evalcond[7]=(((new_r11*x1042))+((new_r01*x1043))+(((-1.0)*gconst10)));
13494 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 )
13495 {
13496 continue;
13497 }
13498 }
13499 
13500 {
13501 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13502 vinfos[0].jointtype = 1;
13503 vinfos[0].foffset = j0;
13504 vinfos[0].indices[0] = _ij0[0];
13505 vinfos[0].indices[1] = _ij0[1];
13506 vinfos[0].maxsolutions = _nj0;
13507 vinfos[1].jointtype = 1;
13508 vinfos[1].foffset = j1;
13509 vinfos[1].indices[0] = _ij1[0];
13510 vinfos[1].indices[1] = _ij1[1];
13511 vinfos[1].maxsolutions = _nj1;
13512 vinfos[2].jointtype = 1;
13513 vinfos[2].foffset = j2;
13514 vinfos[2].indices[0] = _ij2[0];
13515 vinfos[2].indices[1] = _ij2[1];
13516 vinfos[2].maxsolutions = _nj2;
13517 vinfos[3].jointtype = 1;
13518 vinfos[3].foffset = j3;
13519 vinfos[3].indices[0] = _ij3[0];
13520 vinfos[3].indices[1] = _ij3[1];
13521 vinfos[3].maxsolutions = _nj3;
13522 vinfos[4].jointtype = 1;
13523 vinfos[4].foffset = j4;
13524 vinfos[4].indices[0] = _ij4[0];
13525 vinfos[4].indices[1] = _ij4[1];
13526 vinfos[4].maxsolutions = _nj4;
13527 vinfos[5].jointtype = 1;
13528 vinfos[5].foffset = j5;
13529 vinfos[5].indices[0] = _ij5[0];
13530 vinfos[5].indices[1] = _ij5[1];
13531 vinfos[5].maxsolutions = _nj5;
13532 std::vector<int> vfree(0);
13533 solutions.AddSolution(vinfos,vfree);
13534 }
13535 }
13536 }
13537 
13538 }
13539 
13540 }
13541 
13542 }
13543 } while(0);
13544 if( bgotonextstatement )
13545 {
13546 bool bgotonextstatement = true;
13547 do
13548 {
13549 if( 1 )
13550 {
13551 bgotonextstatement=false;
13552 continue; // branch miss [j3]
13553 
13554 }
13555 } while(0);
13556 if( bgotonextstatement )
13557 {
13558 }
13559 }
13560 }
13561 }
13562 }
13563 }
13564 
13565 } else
13566 {
13567 {
13568 IkReal j3array[1], cj3array[1], sj3array[1];
13569 bool j3valid[1]={false};
13570 _nj3 = 1;
13571 CheckValue<IkReal> x1047 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13572 if(!x1047.valid){
13573 continue;
13574 }
13575 CheckValue<IkReal> x1048=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
13576 if(!x1048.valid){
13577 continue;
13578 }
13579 j3array[0]=((-1.5707963267949)+(x1047.value)+(((1.5707963267949)*(x1048.value))));
13580 sj3array[0]=IKsin(j3array[0]);
13581 cj3array[0]=IKcos(j3array[0]);
13582 if( j3array[0] > IKPI )
13583 {
13584  j3array[0]-=IK2PI;
13585 }
13586 else if( j3array[0] < -IKPI )
13587 { j3array[0]+=IK2PI;
13588 }
13589 j3valid[0] = true;
13590 for(int ij3 = 0; ij3 < 1; ++ij3)
13591 {
13592 if( !j3valid[ij3] )
13593 {
13594  continue;
13595 }
13596 _ij3[0] = ij3; _ij3[1] = -1;
13597 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13598 {
13599 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13600 {
13601  j3valid[iij3]=false; _ij3[1] = iij3; break;
13602 }
13603 }
13604 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13605 {
13606 IkReal evalcond[8];
13607 IkReal x1049=IKcos(j3);
13608 IkReal x1050=IKsin(j3);
13609 IkReal x1051=((1.0)*gconst10);
13610 IkReal x1052=(gconst11*x1050);
13611 IkReal x1053=(gconst10*x1050);
13612 IkReal x1054=(gconst11*x1049);
13613 IkReal x1055=((1.0)*x1049);
13614 IkReal x1056=(x1049*x1051);
13615 evalcond[0]=(gconst11+((new_r00*x1049))+((new_r10*x1050)));
13616 evalcond[1]=(x1053+x1054+new_r00);
13617 evalcond[2]=(gconst10+((new_r00*x1050))+(((-1.0)*new_r10*x1055)));
13618 evalcond[3]=(gconst11+((new_r01*x1050))+(((-1.0)*new_r11*x1055)));
13619 evalcond[4]=((((-1.0)*x1056))+x1052+new_r01);
13620 evalcond[5]=((((-1.0)*x1056))+x1052+new_r10);
13621 evalcond[6]=((((-1.0)*x1051))+((new_r01*x1049))+((new_r11*x1050)));
13622 evalcond[7]=((((-1.0)*x1054))+new_r11+(((-1.0)*x1050*x1051)));
13623 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 )
13624 {
13625 continue;
13626 }
13627 }
13628 
13629 {
13630 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13631 vinfos[0].jointtype = 1;
13632 vinfos[0].foffset = j0;
13633 vinfos[0].indices[0] = _ij0[0];
13634 vinfos[0].indices[1] = _ij0[1];
13635 vinfos[0].maxsolutions = _nj0;
13636 vinfos[1].jointtype = 1;
13637 vinfos[1].foffset = j1;
13638 vinfos[1].indices[0] = _ij1[0];
13639 vinfos[1].indices[1] = _ij1[1];
13640 vinfos[1].maxsolutions = _nj1;
13641 vinfos[2].jointtype = 1;
13642 vinfos[2].foffset = j2;
13643 vinfos[2].indices[0] = _ij2[0];
13644 vinfos[2].indices[1] = _ij2[1];
13645 vinfos[2].maxsolutions = _nj2;
13646 vinfos[3].jointtype = 1;
13647 vinfos[3].foffset = j3;
13648 vinfos[3].indices[0] = _ij3[0];
13649 vinfos[3].indices[1] = _ij3[1];
13650 vinfos[3].maxsolutions = _nj3;
13651 vinfos[4].jointtype = 1;
13652 vinfos[4].foffset = j4;
13653 vinfos[4].indices[0] = _ij4[0];
13654 vinfos[4].indices[1] = _ij4[1];
13655 vinfos[4].maxsolutions = _nj4;
13656 vinfos[5].jointtype = 1;
13657 vinfos[5].foffset = j5;
13658 vinfos[5].indices[0] = _ij5[0];
13659 vinfos[5].indices[1] = _ij5[1];
13660 vinfos[5].maxsolutions = _nj5;
13661 std::vector<int> vfree(0);
13662 solutions.AddSolution(vinfos,vfree);
13663 }
13664 }
13665 }
13666 
13667 }
13668 
13669 }
13670 
13671 } else
13672 {
13673 {
13674 IkReal j3array[1], cj3array[1], sj3array[1];
13675 bool j3valid[1]={false};
13676 _nj3 = 1;
13677 IkReal x1057=((1.0)*new_r10);
13678 CheckValue<IkReal> x1058 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r01)))),IkReal(((((-1.0)*new_r01*x1057))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13679 if(!x1058.valid){
13680 continue;
13681 }
13682 CheckValue<IkReal> x1059=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*x1057))+(((-1.0)*gconst11*new_r00)))),-1);
13683 if(!x1059.valid){
13684 continue;
13685 }
13686 j3array[0]=((-1.5707963267949)+(x1058.value)+(((1.5707963267949)*(x1059.value))));
13687 sj3array[0]=IKsin(j3array[0]);
13688 cj3array[0]=IKcos(j3array[0]);
13689 if( j3array[0] > IKPI )
13690 {
13691  j3array[0]-=IK2PI;
13692 }
13693 else if( j3array[0] < -IKPI )
13694 { j3array[0]+=IK2PI;
13695 }
13696 j3valid[0] = true;
13697 for(int ij3 = 0; ij3 < 1; ++ij3)
13698 {
13699 if( !j3valid[ij3] )
13700 {
13701  continue;
13702 }
13703 _ij3[0] = ij3; _ij3[1] = -1;
13704 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13705 {
13706 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13707 {
13708  j3valid[iij3]=false; _ij3[1] = iij3; break;
13709 }
13710 }
13711 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13712 {
13713 IkReal evalcond[8];
13714 IkReal x1060=IKcos(j3);
13715 IkReal x1061=IKsin(j3);
13716 IkReal x1062=((1.0)*gconst10);
13717 IkReal x1063=(gconst11*x1061);
13718 IkReal x1064=(gconst10*x1061);
13719 IkReal x1065=(gconst11*x1060);
13720 IkReal x1066=((1.0)*x1060);
13721 IkReal x1067=(x1060*x1062);
13722 evalcond[0]=(gconst11+((new_r00*x1060))+((new_r10*x1061)));
13723 evalcond[1]=(x1065+x1064+new_r00);
13724 evalcond[2]=((((-1.0)*new_r10*x1066))+gconst10+((new_r00*x1061)));
13725 evalcond[3]=(gconst11+((new_r01*x1061))+(((-1.0)*new_r11*x1066)));
13726 evalcond[4]=(x1063+new_r01+(((-1.0)*x1067)));
13727 evalcond[5]=(x1063+new_r10+(((-1.0)*x1067)));
13728 evalcond[6]=(((new_r01*x1060))+((new_r11*x1061))+(((-1.0)*x1062)));
13729 evalcond[7]=((((-1.0)*x1061*x1062))+new_r11+(((-1.0)*x1065)));
13730 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 )
13731 {
13732 continue;
13733 }
13734 }
13735 
13736 {
13737 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13738 vinfos[0].jointtype = 1;
13739 vinfos[0].foffset = j0;
13740 vinfos[0].indices[0] = _ij0[0];
13741 vinfos[0].indices[1] = _ij0[1];
13742 vinfos[0].maxsolutions = _nj0;
13743 vinfos[1].jointtype = 1;
13744 vinfos[1].foffset = j1;
13745 vinfos[1].indices[0] = _ij1[0];
13746 vinfos[1].indices[1] = _ij1[1];
13747 vinfos[1].maxsolutions = _nj1;
13748 vinfos[2].jointtype = 1;
13749 vinfos[2].foffset = j2;
13750 vinfos[2].indices[0] = _ij2[0];
13751 vinfos[2].indices[1] = _ij2[1];
13752 vinfos[2].maxsolutions = _nj2;
13753 vinfos[3].jointtype = 1;
13754 vinfos[3].foffset = j3;
13755 vinfos[3].indices[0] = _ij3[0];
13756 vinfos[3].indices[1] = _ij3[1];
13757 vinfos[3].maxsolutions = _nj3;
13758 vinfos[4].jointtype = 1;
13759 vinfos[4].foffset = j4;
13760 vinfos[4].indices[0] = _ij4[0];
13761 vinfos[4].indices[1] = _ij4[1];
13762 vinfos[4].maxsolutions = _nj4;
13763 vinfos[5].jointtype = 1;
13764 vinfos[5].foffset = j5;
13765 vinfos[5].indices[0] = _ij5[0];
13766 vinfos[5].indices[1] = _ij5[1];
13767 vinfos[5].maxsolutions = _nj5;
13768 std::vector<int> vfree(0);
13769 solutions.AddSolution(vinfos,vfree);
13770 }
13771 }
13772 }
13773 
13774 }
13775 
13776 }
13777 
13778 } else
13779 {
13780 {
13781 IkReal j3array[1], cj3array[1], sj3array[1];
13782 bool j3valid[1]={false};
13783 _nj3 = 1;
13784 IkReal x1068=((1.0)*new_r10);
13785 CheckValue<IkReal> x1069=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1068)))),-1);
13786 if(!x1069.valid){
13787 continue;
13788 }
13789 CheckValue<IkReal> x1070 = IKatan2WithCheck(IkReal((((gconst11*new_r00))+((gconst11*new_r11)))),IkReal((((gconst11*new_r01))+(((-1.0)*gconst11*x1068)))),IKFAST_ATAN2_MAGTHRESH);
13790 if(!x1070.valid){
13791 continue;
13792 }
13793 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1069.value)))+(x1070.value));
13794 sj3array[0]=IKsin(j3array[0]);
13795 cj3array[0]=IKcos(j3array[0]);
13796 if( j3array[0] > IKPI )
13797 {
13798  j3array[0]-=IK2PI;
13799 }
13800 else if( j3array[0] < -IKPI )
13801 { j3array[0]+=IK2PI;
13802 }
13803 j3valid[0] = true;
13804 for(int ij3 = 0; ij3 < 1; ++ij3)
13805 {
13806 if( !j3valid[ij3] )
13807 {
13808  continue;
13809 }
13810 _ij3[0] = ij3; _ij3[1] = -1;
13811 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13812 {
13813 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13814 {
13815  j3valid[iij3]=false; _ij3[1] = iij3; break;
13816 }
13817 }
13818 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13819 {
13820 IkReal evalcond[8];
13821 IkReal x1071=IKcos(j3);
13822 IkReal x1072=IKsin(j3);
13823 IkReal x1073=((1.0)*gconst10);
13824 IkReal x1074=(gconst11*x1072);
13825 IkReal x1075=(gconst10*x1072);
13826 IkReal x1076=(gconst11*x1071);
13827 IkReal x1077=((1.0)*x1071);
13828 IkReal x1078=(x1071*x1073);
13829 evalcond[0]=(gconst11+((new_r00*x1071))+((new_r10*x1072)));
13830 evalcond[1]=(x1076+x1075+new_r00);
13831 evalcond[2]=(gconst10+((new_r00*x1072))+(((-1.0)*new_r10*x1077)));
13832 evalcond[3]=((((-1.0)*new_r11*x1077))+gconst11+((new_r01*x1072)));
13833 evalcond[4]=(x1074+new_r01+(((-1.0)*x1078)));
13834 evalcond[5]=(x1074+new_r10+(((-1.0)*x1078)));
13835 evalcond[6]=(((new_r01*x1071))+((new_r11*x1072))+(((-1.0)*x1073)));
13836 evalcond[7]=((((-1.0)*x1076))+(((-1.0)*x1072*x1073))+new_r11);
13837 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 )
13838 {
13839 continue;
13840 }
13841 }
13842 
13843 {
13844 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13845 vinfos[0].jointtype = 1;
13846 vinfos[0].foffset = j0;
13847 vinfos[0].indices[0] = _ij0[0];
13848 vinfos[0].indices[1] = _ij0[1];
13849 vinfos[0].maxsolutions = _nj0;
13850 vinfos[1].jointtype = 1;
13851 vinfos[1].foffset = j1;
13852 vinfos[1].indices[0] = _ij1[0];
13853 vinfos[1].indices[1] = _ij1[1];
13854 vinfos[1].maxsolutions = _nj1;
13855 vinfos[2].jointtype = 1;
13856 vinfos[2].foffset = j2;
13857 vinfos[2].indices[0] = _ij2[0];
13858 vinfos[2].indices[1] = _ij2[1];
13859 vinfos[2].maxsolutions = _nj2;
13860 vinfos[3].jointtype = 1;
13861 vinfos[3].foffset = j3;
13862 vinfos[3].indices[0] = _ij3[0];
13863 vinfos[3].indices[1] = _ij3[1];
13864 vinfos[3].maxsolutions = _nj3;
13865 vinfos[4].jointtype = 1;
13866 vinfos[4].foffset = j4;
13867 vinfos[4].indices[0] = _ij4[0];
13868 vinfos[4].indices[1] = _ij4[1];
13869 vinfos[4].maxsolutions = _nj4;
13870 vinfos[5].jointtype = 1;
13871 vinfos[5].foffset = j5;
13872 vinfos[5].indices[0] = _ij5[0];
13873 vinfos[5].indices[1] = _ij5[1];
13874 vinfos[5].maxsolutions = _nj5;
13875 std::vector<int> vfree(0);
13876 solutions.AddSolution(vinfos,vfree);
13877 }
13878 }
13879 }
13880 
13881 }
13882 
13883 }
13884 
13885 }
13886 } while(0);
13887 if( bgotonextstatement )
13888 {
13889 bool bgotonextstatement = true;
13890 do
13891 {
13892 IkReal x1079=((-1.0)*new_r10);
13893 IkReal x1081 = ((new_r10*new_r10)+(new_r00*new_r00));
13894 if(IKabs(x1081)==0){
13895 continue;
13896 }
13897 IkReal x1080=pow(x1081,-0.5);
13898 CheckValue<IkReal> x1082 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1079),IKFAST_ATAN2_MAGTHRESH);
13899 if(!x1082.valid){
13900 continue;
13901 }
13902 IkReal gconst12=((-1.0)*(x1082.value));
13903 IkReal gconst13=(new_r00*x1080);
13904 IkReal gconst14=(x1079*x1080);
13905 CheckValue<IkReal> x1083 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
13906 if(!x1083.valid){
13907 continue;
13908 }
13909 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x1083.value)+j5)))), 6.28318530717959)));
13910 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13911 {
13912 bgotonextstatement=false;
13913 {
13914 IkReal j3eval[3];
13915 IkReal x1084=((-1.0)*new_r10);
13916 CheckValue<IkReal> x1087 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1084),IKFAST_ATAN2_MAGTHRESH);
13917 if(!x1087.valid){
13918 continue;
13919 }
13920 IkReal x1085=((-1.0)*(x1087.value));
13921 IkReal x1086=x1080;
13922 sj4=0;
13923 cj4=-1.0;
13924 j4=3.14159265358979;
13925 sj5=gconst13;
13926 cj5=gconst14;
13927 j5=x1085;
13928 IkReal gconst12=x1085;
13929 IkReal gconst13=(new_r00*x1086);
13930 IkReal gconst14=(x1084*x1086);
13931 IkReal x1088=new_r10*new_r10;
13932 IkReal x1089=((1.0)*new_r00);
13933 IkReal x1090=((1.0)*new_r10*new_r11);
13934 IkReal x1091=((((-1.0)*x1090))+(((-1.0)*new_r01*x1089)));
13935 IkReal x1092=x1080;
13936 IkReal x1093=(new_r10*x1092);
13937 j3eval[0]=x1091;
13938 j3eval[1]=((IKabs((((x1088*x1092))+(((-1.0)*new_r01*x1093)))))+(IKabs(((((-1.0)*x1090*x1092))+(((-1.0)*x1089*x1093))))));
13939 j3eval[2]=IKsign(x1091);
13940 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
13941 {
13942 {
13943 IkReal j3eval[1];
13944 IkReal x1094=((-1.0)*new_r10);
13945 CheckValue<IkReal> x1097 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1094),IKFAST_ATAN2_MAGTHRESH);
13946 if(!x1097.valid){
13947 continue;
13948 }
13949 IkReal x1095=((-1.0)*(x1097.value));
13950 IkReal x1096=x1080;
13951 sj4=0;
13952 cj4=-1.0;
13953 j4=3.14159265358979;
13954 sj5=gconst13;
13955 cj5=gconst14;
13956 j5=x1095;
13957 IkReal gconst12=x1095;
13958 IkReal gconst13=(new_r00*x1096);
13959 IkReal gconst14=(x1094*x1096);
13960 IkReal x1098=new_r10*new_r10;
13961 CheckValue<IkReal> x1101=IKPowWithIntegerCheck((x1098+(new_r00*new_r00)),-1);
13962 if(!x1101.valid){
13963 continue;
13964 }
13965 IkReal x1099=x1101.value;
13966 IkReal x1100=(new_r00*x1099);
13967 j3eval[0]=((IKabs((((new_r00*new_r11))+((x1098*x1099)))))+(IKabs((((new_r01*x1098*x1100))+((new_r10*x1100))+((new_r01*x1100*(new_r00*new_r00)))))));
13968 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13969 {
13970 {
13971 IkReal j3eval[1];
13972 IkReal x1102=((-1.0)*new_r10);
13973 CheckValue<IkReal> x1105 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1102),IKFAST_ATAN2_MAGTHRESH);
13974 if(!x1105.valid){
13975 continue;
13976 }
13977 IkReal x1103=((-1.0)*(x1105.value));
13978 IkReal x1104=x1080;
13979 sj4=0;
13980 cj4=-1.0;
13981 j4=3.14159265358979;
13982 sj5=gconst13;
13983 cj5=gconst14;
13984 j5=x1103;
13985 IkReal gconst12=x1103;
13986 IkReal gconst13=(new_r00*x1104);
13987 IkReal gconst14=(x1102*x1104);
13988 IkReal x1106=new_r10*new_r10;
13989 IkReal x1107=new_r00*new_r00;
13990 CheckValue<IkReal> x1111=IKPowWithIntegerCheck((x1106+x1107),-1);
13991 if(!x1111.valid){
13992 continue;
13993 }
13994 IkReal x1108=x1111.value;
13995 IkReal x1109=(new_r10*x1108);
13996 IkReal x1110=(x1106*x1108);
13997 j3eval[0]=((IKabs((x1110+(((-1.0)*x1108*(x1107*x1107)))+(((-1.0)*x1107*x1110)))))+(IKabs((((x1109*(new_r00*new_r00*new_r00)))+((new_r00*x1109))+((new_r00*x1109*(new_r10*new_r10)))))));
13998 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13999 {
14000 {
14001 IkReal evalcond[2];
14002 bool bgotonextstatement = true;
14003 do
14004 {
14005 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
14006 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14007 {
14008 bgotonextstatement=false;
14009 {
14010 IkReal j3eval[1];
14011 IkReal x1112=((-1.0)*new_r10);
14012 CheckValue<IkReal> x1114 = IKatan2WithCheck(IkReal(0),IkReal(x1112),IKFAST_ATAN2_MAGTHRESH);
14013 if(!x1114.valid){
14014 continue;
14015 }
14016 IkReal x1113=((-1.0)*(x1114.value));
14017 sj4=0;
14018 cj4=-1.0;
14019 j4=3.14159265358979;
14020 sj5=gconst13;
14021 cj5=gconst14;
14022 j5=x1113;
14023 new_r11=0;
14024 new_r00=0;
14025 IkReal gconst12=x1113;
14026 IkReal gconst13=0;
14027 IkReal x1115 = new_r10*new_r10;
14028 if(IKabs(x1115)==0){
14029 continue;
14030 }
14031 IkReal gconst14=(x1112*(pow(x1115,-0.5)));
14032 j3eval[0]=new_r01;
14033 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14034 {
14035 {
14036 IkReal j3array[2], cj3array[2], sj3array[2];
14037 bool j3valid[2]={false};
14038 _nj3 = 2;
14039 CheckValue<IkReal> x1116=IKPowWithIntegerCheck(gconst14,-1);
14040 if(!x1116.valid){
14041 continue;
14042 }
14043 sj3array[0]=((-1.0)*new_r01*(x1116.value));
14044 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14045 {
14046  j3valid[0] = j3valid[1] = true;
14047  j3array[0] = IKasin(sj3array[0]);
14048  cj3array[0] = IKcos(j3array[0]);
14049  sj3array[1] = sj3array[0];
14050  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14051  cj3array[1] = -cj3array[0];
14052 }
14053 else if( isnan(sj3array[0]) )
14054 {
14055  // probably any value will work
14056  j3valid[0] = true;
14057  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14058 }
14059 for(int ij3 = 0; ij3 < 2; ++ij3)
14060 {
14061 if( !j3valid[ij3] )
14062 {
14063  continue;
14064 }
14065 _ij3[0] = ij3; _ij3[1] = -1;
14066 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14067 {
14068 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14069 {
14070  j3valid[iij3]=false; _ij3[1] = iij3; break;
14071 }
14072 }
14073 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14074 {
14075 IkReal evalcond[6];
14076 IkReal x1117=IKcos(j3);
14077 IkReal x1118=IKsin(j3);
14078 evalcond[0]=(new_r01*x1117);
14079 evalcond[1]=(gconst14*x1117);
14080 evalcond[2]=((-1.0)*new_r10*x1117);
14081 evalcond[3]=(gconst14+((new_r01*x1118)));
14082 evalcond[4]=(gconst14+((new_r10*x1118)));
14083 evalcond[5]=(((gconst14*x1118))+new_r10);
14084 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 )
14085 {
14086 continue;
14087 }
14088 }
14089 
14090 {
14091 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14092 vinfos[0].jointtype = 1;
14093 vinfos[0].foffset = j0;
14094 vinfos[0].indices[0] = _ij0[0];
14095 vinfos[0].indices[1] = _ij0[1];
14096 vinfos[0].maxsolutions = _nj0;
14097 vinfos[1].jointtype = 1;
14098 vinfos[1].foffset = j1;
14099 vinfos[1].indices[0] = _ij1[0];
14100 vinfos[1].indices[1] = _ij1[1];
14101 vinfos[1].maxsolutions = _nj1;
14102 vinfos[2].jointtype = 1;
14103 vinfos[2].foffset = j2;
14104 vinfos[2].indices[0] = _ij2[0];
14105 vinfos[2].indices[1] = _ij2[1];
14106 vinfos[2].maxsolutions = _nj2;
14107 vinfos[3].jointtype = 1;
14108 vinfos[3].foffset = j3;
14109 vinfos[3].indices[0] = _ij3[0];
14110 vinfos[3].indices[1] = _ij3[1];
14111 vinfos[3].maxsolutions = _nj3;
14112 vinfos[4].jointtype = 1;
14113 vinfos[4].foffset = j4;
14114 vinfos[4].indices[0] = _ij4[0];
14115 vinfos[4].indices[1] = _ij4[1];
14116 vinfos[4].maxsolutions = _nj4;
14117 vinfos[5].jointtype = 1;
14118 vinfos[5].foffset = j5;
14119 vinfos[5].indices[0] = _ij5[0];
14120 vinfos[5].indices[1] = _ij5[1];
14121 vinfos[5].maxsolutions = _nj5;
14122 std::vector<int> vfree(0);
14123 solutions.AddSolution(vinfos,vfree);
14124 }
14125 }
14126 }
14127 
14128 } else
14129 {
14130 {
14131 IkReal j3array[2], cj3array[2], sj3array[2];
14132 bool j3valid[2]={false};
14133 _nj3 = 2;
14134 CheckValue<IkReal> x1119=IKPowWithIntegerCheck(new_r01,-1);
14135 if(!x1119.valid){
14136 continue;
14137 }
14138 sj3array[0]=((-1.0)*gconst14*(x1119.value));
14139 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14140 {
14141  j3valid[0] = j3valid[1] = true;
14142  j3array[0] = IKasin(sj3array[0]);
14143  cj3array[0] = IKcos(j3array[0]);
14144  sj3array[1] = sj3array[0];
14145  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14146  cj3array[1] = -cj3array[0];
14147 }
14148 else if( isnan(sj3array[0]) )
14149 {
14150  // probably any value will work
14151  j3valid[0] = true;
14152  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14153 }
14154 for(int ij3 = 0; ij3 < 2; ++ij3)
14155 {
14156 if( !j3valid[ij3] )
14157 {
14158  continue;
14159 }
14160 _ij3[0] = ij3; _ij3[1] = -1;
14161 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14162 {
14163 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14164 {
14165  j3valid[iij3]=false; _ij3[1] = iij3; break;
14166 }
14167 }
14168 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14169 {
14170 IkReal evalcond[6];
14171 IkReal x1120=IKcos(j3);
14172 IkReal x1121=IKsin(j3);
14173 IkReal x1122=(gconst14*x1121);
14174 evalcond[0]=(new_r01*x1120);
14175 evalcond[1]=(gconst14*x1120);
14176 evalcond[2]=((-1.0)*new_r10*x1120);
14177 evalcond[3]=(x1122+new_r01);
14178 evalcond[4]=(gconst14+((new_r10*x1121)));
14179 evalcond[5]=(x1122+new_r10);
14180 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 )
14181 {
14182 continue;
14183 }
14184 }
14185 
14186 {
14187 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14188 vinfos[0].jointtype = 1;
14189 vinfos[0].foffset = j0;
14190 vinfos[0].indices[0] = _ij0[0];
14191 vinfos[0].indices[1] = _ij0[1];
14192 vinfos[0].maxsolutions = _nj0;
14193 vinfos[1].jointtype = 1;
14194 vinfos[1].foffset = j1;
14195 vinfos[1].indices[0] = _ij1[0];
14196 vinfos[1].indices[1] = _ij1[1];
14197 vinfos[1].maxsolutions = _nj1;
14198 vinfos[2].jointtype = 1;
14199 vinfos[2].foffset = j2;
14200 vinfos[2].indices[0] = _ij2[0];
14201 vinfos[2].indices[1] = _ij2[1];
14202 vinfos[2].maxsolutions = _nj2;
14203 vinfos[3].jointtype = 1;
14204 vinfos[3].foffset = j3;
14205 vinfos[3].indices[0] = _ij3[0];
14206 vinfos[3].indices[1] = _ij3[1];
14207 vinfos[3].maxsolutions = _nj3;
14208 vinfos[4].jointtype = 1;
14209 vinfos[4].foffset = j4;
14210 vinfos[4].indices[0] = _ij4[0];
14211 vinfos[4].indices[1] = _ij4[1];
14212 vinfos[4].maxsolutions = _nj4;
14213 vinfos[5].jointtype = 1;
14214 vinfos[5].foffset = j5;
14215 vinfos[5].indices[0] = _ij5[0];
14216 vinfos[5].indices[1] = _ij5[1];
14217 vinfos[5].maxsolutions = _nj5;
14218 std::vector<int> vfree(0);
14219 solutions.AddSolution(vinfos,vfree);
14220 }
14221 }
14222 }
14223 
14224 }
14225 
14226 }
14227 
14228 }
14229 } while(0);
14230 if( bgotonextstatement )
14231 {
14232 bool bgotonextstatement = true;
14233 do
14234 {
14235 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14236 evalcond[1]=gconst14;
14237 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
14238 {
14239 bgotonextstatement=false;
14240 {
14241 IkReal j3eval[3];
14242 IkReal x1123=((-1.0)*new_r10);
14243 CheckValue<IkReal> x1125 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1123),IKFAST_ATAN2_MAGTHRESH);
14244 if(!x1125.valid){
14245 continue;
14246 }
14247 IkReal x1124=((-1.0)*(x1125.value));
14248 sj4=0;
14249 cj4=-1.0;
14250 j4=3.14159265358979;
14251 sj5=gconst13;
14252 cj5=gconst14;
14253 j5=x1124;
14254 new_r11=0;
14255 new_r01=0;
14256 new_r22=0;
14257 new_r20=0;
14258 IkReal gconst12=x1124;
14259 IkReal gconst13=new_r00;
14260 IkReal gconst14=x1123;
14261 j3eval[0]=-1.0;
14262 j3eval[1]=-1.0;
14263 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14264 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14265 {
14266 {
14267 IkReal j3eval[3];
14268 IkReal x1126=((-1.0)*new_r10);
14269 CheckValue<IkReal> x1128 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1126),IKFAST_ATAN2_MAGTHRESH);
14270 if(!x1128.valid){
14271 continue;
14272 }
14273 IkReal x1127=((-1.0)*(x1128.value));
14274 sj4=0;
14275 cj4=-1.0;
14276 j4=3.14159265358979;
14277 sj5=gconst13;
14278 cj5=gconst14;
14279 j5=x1127;
14280 new_r11=0;
14281 new_r01=0;
14282 new_r22=0;
14283 new_r20=0;
14284 IkReal gconst12=x1127;
14285 IkReal gconst13=new_r00;
14286 IkReal gconst14=x1126;
14287 j3eval[0]=-1.0;
14288 j3eval[1]=-1.0;
14289 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14290 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14291 {
14292 {
14293 IkReal j3eval[3];
14294 IkReal x1129=((-1.0)*new_r10);
14295 CheckValue<IkReal> x1131 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1129),IKFAST_ATAN2_MAGTHRESH);
14296 if(!x1131.valid){
14297 continue;
14298 }
14299 IkReal x1130=((-1.0)*(x1131.value));
14300 sj4=0;
14301 cj4=-1.0;
14302 j4=3.14159265358979;
14303 sj5=gconst13;
14304 cj5=gconst14;
14305 j5=x1130;
14306 new_r11=0;
14307 new_r01=0;
14308 new_r22=0;
14309 new_r20=0;
14310 IkReal gconst12=x1130;
14311 IkReal gconst13=new_r00;
14312 IkReal gconst14=x1129;
14313 j3eval[0]=1.0;
14314 j3eval[1]=1.0;
14315 j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
14316 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14317 {
14318 continue; // 3 cases reached
14319 
14320 } else
14321 {
14322 {
14323 IkReal j3array[1], cj3array[1], sj3array[1];
14324 bool j3valid[1]={false};
14325 _nj3 = 1;
14326 IkReal x1132=((1.0)*gconst14);
14327 CheckValue<IkReal> x1133=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x1132))+((gconst13*new_r00)))),-1);
14328 if(!x1133.valid){
14329 continue;
14330 }
14331 CheckValue<IkReal> x1134 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal(((((-1.0)*gconst13*x1132))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
14332 if(!x1134.valid){
14333 continue;
14334 }
14335 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1133.value)))+(x1134.value));
14336 sj3array[0]=IKsin(j3array[0]);
14337 cj3array[0]=IKcos(j3array[0]);
14338 if( j3array[0] > IKPI )
14339 {
14340  j3array[0]-=IK2PI;
14341 }
14342 else if( j3array[0] < -IKPI )
14343 { j3array[0]+=IK2PI;
14344 }
14345 j3valid[0] = true;
14346 for(int ij3 = 0; ij3 < 1; ++ij3)
14347 {
14348 if( !j3valid[ij3] )
14349 {
14350  continue;
14351 }
14352 _ij3[0] = ij3; _ij3[1] = -1;
14353 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14354 {
14355 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14356 {
14357  j3valid[iij3]=false; _ij3[1] = iij3; break;
14358 }
14359 }
14360 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14361 {
14362 IkReal evalcond[6];
14363 IkReal x1135=IKsin(j3);
14364 IkReal x1136=IKcos(j3);
14365 IkReal x1137=((1.0)*gconst13);
14366 IkReal x1138=(gconst14*x1135);
14367 IkReal x1139=(gconst14*x1136);
14368 IkReal x1140=(x1136*x1137);
14369 evalcond[0]=(x1138+(((-1.0)*x1140)));
14370 evalcond[1]=(gconst14+((new_r00*x1136))+((new_r10*x1135)));
14371 evalcond[2]=(x1139+((gconst13*x1135))+new_r00);
14372 evalcond[3]=(gconst13+((new_r00*x1135))+(((-1.0)*new_r10*x1136)));
14373 evalcond[4]=((((-1.0)*x1139))+(((-1.0)*x1135*x1137)));
14374 evalcond[5]=(x1138+(((-1.0)*x1140))+new_r10);
14375 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 )
14376 {
14377 continue;
14378 }
14379 }
14380 
14381 {
14382 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14383 vinfos[0].jointtype = 1;
14384 vinfos[0].foffset = j0;
14385 vinfos[0].indices[0] = _ij0[0];
14386 vinfos[0].indices[1] = _ij0[1];
14387 vinfos[0].maxsolutions = _nj0;
14388 vinfos[1].jointtype = 1;
14389 vinfos[1].foffset = j1;
14390 vinfos[1].indices[0] = _ij1[0];
14391 vinfos[1].indices[1] = _ij1[1];
14392 vinfos[1].maxsolutions = _nj1;
14393 vinfos[2].jointtype = 1;
14394 vinfos[2].foffset = j2;
14395 vinfos[2].indices[0] = _ij2[0];
14396 vinfos[2].indices[1] = _ij2[1];
14397 vinfos[2].maxsolutions = _nj2;
14398 vinfos[3].jointtype = 1;
14399 vinfos[3].foffset = j3;
14400 vinfos[3].indices[0] = _ij3[0];
14401 vinfos[3].indices[1] = _ij3[1];
14402 vinfos[3].maxsolutions = _nj3;
14403 vinfos[4].jointtype = 1;
14404 vinfos[4].foffset = j4;
14405 vinfos[4].indices[0] = _ij4[0];
14406 vinfos[4].indices[1] = _ij4[1];
14407 vinfos[4].maxsolutions = _nj4;
14408 vinfos[5].jointtype = 1;
14409 vinfos[5].foffset = j5;
14410 vinfos[5].indices[0] = _ij5[0];
14411 vinfos[5].indices[1] = _ij5[1];
14412 vinfos[5].maxsolutions = _nj5;
14413 std::vector<int> vfree(0);
14414 solutions.AddSolution(vinfos,vfree);
14415 }
14416 }
14417 }
14418 
14419 }
14420 
14421 }
14422 
14423 } else
14424 {
14425 {
14426 IkReal j3array[1], cj3array[1], sj3array[1];
14427 bool j3valid[1]={false};
14428 _nj3 = 1;
14429 CheckValue<IkReal> x1141=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst14*gconst14)))+(((-1.0)*(gconst13*gconst13))))),-1);
14430 if(!x1141.valid){
14431 continue;
14432 }
14433 CheckValue<IkReal> x1142 = IKatan2WithCheck(IkReal((gconst13*new_r00)),IkReal((gconst14*new_r00)),IKFAST_ATAN2_MAGTHRESH);
14434 if(!x1142.valid){
14435 continue;
14436 }
14437 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1141.value)))+(x1142.value));
14438 sj3array[0]=IKsin(j3array[0]);
14439 cj3array[0]=IKcos(j3array[0]);
14440 if( j3array[0] > IKPI )
14441 {
14442  j3array[0]-=IK2PI;
14443 }
14444 else if( j3array[0] < -IKPI )
14445 { j3array[0]+=IK2PI;
14446 }
14447 j3valid[0] = true;
14448 for(int ij3 = 0; ij3 < 1; ++ij3)
14449 {
14450 if( !j3valid[ij3] )
14451 {
14452  continue;
14453 }
14454 _ij3[0] = ij3; _ij3[1] = -1;
14455 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14456 {
14457 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14458 {
14459  j3valid[iij3]=false; _ij3[1] = iij3; break;
14460 }
14461 }
14462 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14463 {
14464 IkReal evalcond[6];
14465 IkReal x1143=IKsin(j3);
14466 IkReal x1144=IKcos(j3);
14467 IkReal x1145=((1.0)*gconst13);
14468 IkReal x1146=(gconst14*x1143);
14469 IkReal x1147=(gconst14*x1144);
14470 IkReal x1148=(x1144*x1145);
14471 evalcond[0]=(x1146+(((-1.0)*x1148)));
14472 evalcond[1]=(((new_r00*x1144))+gconst14+((new_r10*x1143)));
14473 evalcond[2]=(x1147+((gconst13*x1143))+new_r00);
14474 evalcond[3]=((((-1.0)*new_r10*x1144))+((new_r00*x1143))+gconst13);
14475 evalcond[4]=((((-1.0)*x1143*x1145))+(((-1.0)*x1147)));
14476 evalcond[5]=(x1146+(((-1.0)*x1148))+new_r10);
14477 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 )
14478 {
14479 continue;
14480 }
14481 }
14482 
14483 {
14484 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14485 vinfos[0].jointtype = 1;
14486 vinfos[0].foffset = j0;
14487 vinfos[0].indices[0] = _ij0[0];
14488 vinfos[0].indices[1] = _ij0[1];
14489 vinfos[0].maxsolutions = _nj0;
14490 vinfos[1].jointtype = 1;
14491 vinfos[1].foffset = j1;
14492 vinfos[1].indices[0] = _ij1[0];
14493 vinfos[1].indices[1] = _ij1[1];
14494 vinfos[1].maxsolutions = _nj1;
14495 vinfos[2].jointtype = 1;
14496 vinfos[2].foffset = j2;
14497 vinfos[2].indices[0] = _ij2[0];
14498 vinfos[2].indices[1] = _ij2[1];
14499 vinfos[2].maxsolutions = _nj2;
14500 vinfos[3].jointtype = 1;
14501 vinfos[3].foffset = j3;
14502 vinfos[3].indices[0] = _ij3[0];
14503 vinfos[3].indices[1] = _ij3[1];
14504 vinfos[3].maxsolutions = _nj3;
14505 vinfos[4].jointtype = 1;
14506 vinfos[4].foffset = j4;
14507 vinfos[4].indices[0] = _ij4[0];
14508 vinfos[4].indices[1] = _ij4[1];
14509 vinfos[4].maxsolutions = _nj4;
14510 vinfos[5].jointtype = 1;
14511 vinfos[5].foffset = j5;
14512 vinfos[5].indices[0] = _ij5[0];
14513 vinfos[5].indices[1] = _ij5[1];
14514 vinfos[5].maxsolutions = _nj5;
14515 std::vector<int> vfree(0);
14516 solutions.AddSolution(vinfos,vfree);
14517 }
14518 }
14519 }
14520 
14521 }
14522 
14523 }
14524 
14525 } else
14526 {
14527 {
14528 IkReal j3array[1], cj3array[1], sj3array[1];
14529 bool j3valid[1]={false};
14530 _nj3 = 1;
14531 CheckValue<IkReal> x1149 = IKatan2WithCheck(IkReal(gconst13*gconst13),IkReal((gconst13*gconst14)),IKFAST_ATAN2_MAGTHRESH);
14532 if(!x1149.valid){
14533 continue;
14534 }
14535 CheckValue<IkReal> x1150=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r00))+((gconst14*new_r10)))),-1);
14536 if(!x1150.valid){
14537 continue;
14538 }
14539 j3array[0]=((-1.5707963267949)+(x1149.value)+(((1.5707963267949)*(x1150.value))));
14540 sj3array[0]=IKsin(j3array[0]);
14541 cj3array[0]=IKcos(j3array[0]);
14542 if( j3array[0] > IKPI )
14543 {
14544  j3array[0]-=IK2PI;
14545 }
14546 else if( j3array[0] < -IKPI )
14547 { j3array[0]+=IK2PI;
14548 }
14549 j3valid[0] = true;
14550 for(int ij3 = 0; ij3 < 1; ++ij3)
14551 {
14552 if( !j3valid[ij3] )
14553 {
14554  continue;
14555 }
14556 _ij3[0] = ij3; _ij3[1] = -1;
14557 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14558 {
14559 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14560 {
14561  j3valid[iij3]=false; _ij3[1] = iij3; break;
14562 }
14563 }
14564 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14565 {
14566 IkReal evalcond[6];
14567 IkReal x1151=IKsin(j3);
14568 IkReal x1152=IKcos(j3);
14569 IkReal x1153=((1.0)*gconst13);
14570 IkReal x1154=(gconst14*x1151);
14571 IkReal x1155=(gconst14*x1152);
14572 IkReal x1156=(x1152*x1153);
14573 evalcond[0]=(x1154+(((-1.0)*x1156)));
14574 evalcond[1]=(((new_r10*x1151))+gconst14+((new_r00*x1152)));
14575 evalcond[2]=(x1155+((gconst13*x1151))+new_r00);
14576 evalcond[3]=(gconst13+((new_r00*x1151))+(((-1.0)*new_r10*x1152)));
14577 evalcond[4]=((((-1.0)*x1155))+(((-1.0)*x1151*x1153)));
14578 evalcond[5]=(x1154+(((-1.0)*x1156))+new_r10);
14579 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 )
14580 {
14581 continue;
14582 }
14583 }
14584 
14585 {
14586 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14587 vinfos[0].jointtype = 1;
14588 vinfos[0].foffset = j0;
14589 vinfos[0].indices[0] = _ij0[0];
14590 vinfos[0].indices[1] = _ij0[1];
14591 vinfos[0].maxsolutions = _nj0;
14592 vinfos[1].jointtype = 1;
14593 vinfos[1].foffset = j1;
14594 vinfos[1].indices[0] = _ij1[0];
14595 vinfos[1].indices[1] = _ij1[1];
14596 vinfos[1].maxsolutions = _nj1;
14597 vinfos[2].jointtype = 1;
14598 vinfos[2].foffset = j2;
14599 vinfos[2].indices[0] = _ij2[0];
14600 vinfos[2].indices[1] = _ij2[1];
14601 vinfos[2].maxsolutions = _nj2;
14602 vinfos[3].jointtype = 1;
14603 vinfos[3].foffset = j3;
14604 vinfos[3].indices[0] = _ij3[0];
14605 vinfos[3].indices[1] = _ij3[1];
14606 vinfos[3].maxsolutions = _nj3;
14607 vinfos[4].jointtype = 1;
14608 vinfos[4].foffset = j4;
14609 vinfos[4].indices[0] = _ij4[0];
14610 vinfos[4].indices[1] = _ij4[1];
14611 vinfos[4].maxsolutions = _nj4;
14612 vinfos[5].jointtype = 1;
14613 vinfos[5].foffset = j5;
14614 vinfos[5].indices[0] = _ij5[0];
14615 vinfos[5].indices[1] = _ij5[1];
14616 vinfos[5].maxsolutions = _nj5;
14617 std::vector<int> vfree(0);
14618 solutions.AddSolution(vinfos,vfree);
14619 }
14620 }
14621 }
14622 
14623 }
14624 
14625 }
14626 
14627 }
14628 } while(0);
14629 if( bgotonextstatement )
14630 {
14631 bool bgotonextstatement = true;
14632 do
14633 {
14634 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
14635 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14636 {
14637 bgotonextstatement=false;
14638 {
14639 IkReal j3array[2], cj3array[2], sj3array[2];
14640 bool j3valid[2]={false};
14641 _nj3 = 2;
14642 CheckValue<IkReal> x1157=IKPowWithIntegerCheck(gconst13,-1);
14643 if(!x1157.valid){
14644 continue;
14645 }
14646 sj3array[0]=(new_r11*(x1157.value));
14647 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14648 {
14649  j3valid[0] = j3valid[1] = true;
14650  j3array[0] = IKasin(sj3array[0]);
14651  cj3array[0] = IKcos(j3array[0]);
14652  sj3array[1] = sj3array[0];
14653  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14654  cj3array[1] = -cj3array[0];
14655 }
14656 else if( isnan(sj3array[0]) )
14657 {
14658  // probably any value will work
14659  j3valid[0] = true;
14660  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14661 }
14662 for(int ij3 = 0; ij3 < 2; ++ij3)
14663 {
14664 if( !j3valid[ij3] )
14665 {
14666  continue;
14667 }
14668 _ij3[0] = ij3; _ij3[1] = -1;
14669 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14670 {
14671 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14672 {
14673  j3valid[iij3]=false; _ij3[1] = iij3; break;
14674 }
14675 }
14676 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14677 {
14678 IkReal evalcond[6];
14679 IkReal x1158=IKcos(j3);
14680 IkReal x1159=IKsin(j3);
14681 IkReal x1160=((-1.0)*x1158);
14682 evalcond[0]=(new_r00*x1158);
14683 evalcond[1]=(new_r11*x1160);
14684 evalcond[2]=(gconst13*x1160);
14685 evalcond[3]=(gconst13+((new_r00*x1159)));
14686 evalcond[4]=(((gconst13*x1159))+new_r00);
14687 evalcond[5]=(((new_r11*x1159))+(((-1.0)*gconst13)));
14688 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 )
14689 {
14690 continue;
14691 }
14692 }
14693 
14694 {
14695 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14696 vinfos[0].jointtype = 1;
14697 vinfos[0].foffset = j0;
14698 vinfos[0].indices[0] = _ij0[0];
14699 vinfos[0].indices[1] = _ij0[1];
14700 vinfos[0].maxsolutions = _nj0;
14701 vinfos[1].jointtype = 1;
14702 vinfos[1].foffset = j1;
14703 vinfos[1].indices[0] = _ij1[0];
14704 vinfos[1].indices[1] = _ij1[1];
14705 vinfos[1].maxsolutions = _nj1;
14706 vinfos[2].jointtype = 1;
14707 vinfos[2].foffset = j2;
14708 vinfos[2].indices[0] = _ij2[0];
14709 vinfos[2].indices[1] = _ij2[1];
14710 vinfos[2].maxsolutions = _nj2;
14711 vinfos[3].jointtype = 1;
14712 vinfos[3].foffset = j3;
14713 vinfos[3].indices[0] = _ij3[0];
14714 vinfos[3].indices[1] = _ij3[1];
14715 vinfos[3].maxsolutions = _nj3;
14716 vinfos[4].jointtype = 1;
14717 vinfos[4].foffset = j4;
14718 vinfos[4].indices[0] = _ij4[0];
14719 vinfos[4].indices[1] = _ij4[1];
14720 vinfos[4].maxsolutions = _nj4;
14721 vinfos[5].jointtype = 1;
14722 vinfos[5].foffset = j5;
14723 vinfos[5].indices[0] = _ij5[0];
14724 vinfos[5].indices[1] = _ij5[1];
14725 vinfos[5].maxsolutions = _nj5;
14726 std::vector<int> vfree(0);
14727 solutions.AddSolution(vinfos,vfree);
14728 }
14729 }
14730 }
14731 
14732 }
14733 } while(0);
14734 if( bgotonextstatement )
14735 {
14736 bool bgotonextstatement = true;
14737 do
14738 {
14739 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
14740 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14741 {
14742 bgotonextstatement=false;
14743 {
14744 IkReal j3eval[1];
14745 CheckValue<IkReal> x1162 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14746 if(!x1162.valid){
14747 continue;
14748 }
14749 IkReal x1161=((-1.0)*(x1162.value));
14750 sj4=0;
14751 cj4=-1.0;
14752 j4=3.14159265358979;
14753 sj5=gconst13;
14754 cj5=gconst14;
14755 j5=x1161;
14756 new_r11=0;
14757 new_r10=0;
14758 new_r22=0;
14759 new_r02=0;
14760 IkReal gconst12=x1161;
14761 IkReal x1163 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14762 if(IKabs(x1163)==0){
14763 continue;
14764 }
14765 IkReal gconst13=(new_r00*(pow(x1163,-0.5)));
14766 IkReal gconst14=0;
14767 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
14768 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14769 {
14770 {
14771 IkReal j3eval[1];
14772 CheckValue<IkReal> x1165 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14773 if(!x1165.valid){
14774 continue;
14775 }
14776 IkReal x1164=((-1.0)*(x1165.value));
14777 sj4=0;
14778 cj4=-1.0;
14779 j4=3.14159265358979;
14780 sj5=gconst13;
14781 cj5=gconst14;
14782 j5=x1164;
14783 new_r11=0;
14784 new_r10=0;
14785 new_r22=0;
14786 new_r02=0;
14787 IkReal gconst12=x1164;
14788 IkReal x1166 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14789 if(IKabs(x1166)==0){
14790 continue;
14791 }
14792 IkReal gconst13=(new_r00*(pow(x1166,-0.5)));
14793 IkReal gconst14=0;
14794 j3eval[0]=new_r00;
14795 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14796 {
14797 {
14798 IkReal j3eval[2];
14799 CheckValue<IkReal> x1168 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14800 if(!x1168.valid){
14801 continue;
14802 }
14803 IkReal x1167=((-1.0)*(x1168.value));
14804 sj4=0;
14805 cj4=-1.0;
14806 j4=3.14159265358979;
14807 sj5=gconst13;
14808 cj5=gconst14;
14809 j5=x1167;
14810 new_r11=0;
14811 new_r10=0;
14812 new_r22=0;
14813 new_r02=0;
14814 IkReal gconst12=x1167;
14815 IkReal x1169 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14816 if(IKabs(x1169)==0){
14817 continue;
14818 }
14819 IkReal gconst13=(new_r00*(pow(x1169,-0.5)));
14820 IkReal gconst14=0;
14821 j3eval[0]=new_r00;
14822 j3eval[1]=new_r01;
14823 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
14824 {
14825 continue; // 3 cases reached
14826 
14827 } else
14828 {
14829 {
14830 IkReal j3array[1], cj3array[1], sj3array[1];
14831 bool j3valid[1]={false};
14832 _nj3 = 1;
14833 CheckValue<IkReal> x1170=IKPowWithIntegerCheck(new_r00,-1);
14834 if(!x1170.valid){
14835 continue;
14836 }
14837 CheckValue<IkReal> x1171=IKPowWithIntegerCheck(new_r01,-1);
14838 if(!x1171.valid){
14839 continue;
14840 }
14841 if( IKabs(((-1.0)*gconst13*(x1170.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst13*(x1171.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1170.value)))+IKsqr((gconst13*(x1171.value)))-1) <= IKFAST_SINCOS_THRESH )
14842  continue;
14843 j3array[0]=IKatan2(((-1.0)*gconst13*(x1170.value)), (gconst13*(x1171.value)));
14844 sj3array[0]=IKsin(j3array[0]);
14845 cj3array[0]=IKcos(j3array[0]);
14846 if( j3array[0] > IKPI )
14847 {
14848  j3array[0]-=IK2PI;
14849 }
14850 else if( j3array[0] < -IKPI )
14851 { j3array[0]+=IK2PI;
14852 }
14853 j3valid[0] = true;
14854 for(int ij3 = 0; ij3 < 1; ++ij3)
14855 {
14856 if( !j3valid[ij3] )
14857 {
14858  continue;
14859 }
14860 _ij3[0] = ij3; _ij3[1] = -1;
14861 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14862 {
14863 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14864 {
14865  j3valid[iij3]=false; _ij3[1] = iij3; break;
14866 }
14867 }
14868 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14869 {
14870 IkReal evalcond[8];
14871 IkReal x1172=IKsin(j3);
14872 IkReal x1173=IKcos(j3);
14873 IkReal x1174=(gconst13*x1173);
14874 IkReal x1175=(gconst13*x1172);
14875 evalcond[0]=(new_r01*x1172);
14876 evalcond[1]=(new_r00*x1173);
14877 evalcond[2]=((-1.0)*x1175);
14878 evalcond[3]=((-1.0)*x1174);
14879 evalcond[4]=(gconst13+((new_r00*x1172)));
14880 evalcond[5]=(x1175+new_r00);
14881 evalcond[6]=(new_r01+(((-1.0)*x1174)));
14882 evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1173)));
14883 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 )
14884 {
14885 continue;
14886 }
14887 }
14888 
14889 {
14890 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14891 vinfos[0].jointtype = 1;
14892 vinfos[0].foffset = j0;
14893 vinfos[0].indices[0] = _ij0[0];
14894 vinfos[0].indices[1] = _ij0[1];
14895 vinfos[0].maxsolutions = _nj0;
14896 vinfos[1].jointtype = 1;
14897 vinfos[1].foffset = j1;
14898 vinfos[1].indices[0] = _ij1[0];
14899 vinfos[1].indices[1] = _ij1[1];
14900 vinfos[1].maxsolutions = _nj1;
14901 vinfos[2].jointtype = 1;
14902 vinfos[2].foffset = j2;
14903 vinfos[2].indices[0] = _ij2[0];
14904 vinfos[2].indices[1] = _ij2[1];
14905 vinfos[2].maxsolutions = _nj2;
14906 vinfos[3].jointtype = 1;
14907 vinfos[3].foffset = j3;
14908 vinfos[3].indices[0] = _ij3[0];
14909 vinfos[3].indices[1] = _ij3[1];
14910 vinfos[3].maxsolutions = _nj3;
14911 vinfos[4].jointtype = 1;
14912 vinfos[4].foffset = j4;
14913 vinfos[4].indices[0] = _ij4[0];
14914 vinfos[4].indices[1] = _ij4[1];
14915 vinfos[4].maxsolutions = _nj4;
14916 vinfos[5].jointtype = 1;
14917 vinfos[5].foffset = j5;
14918 vinfos[5].indices[0] = _ij5[0];
14919 vinfos[5].indices[1] = _ij5[1];
14920 vinfos[5].maxsolutions = _nj5;
14921 std::vector<int> vfree(0);
14922 solutions.AddSolution(vinfos,vfree);
14923 }
14924 }
14925 }
14926 
14927 }
14928 
14929 }
14930 
14931 } else
14932 {
14933 {
14934 IkReal j3array[1], cj3array[1], sj3array[1];
14935 bool j3valid[1]={false};
14936 _nj3 = 1;
14937 CheckValue<IkReal> x1176=IKPowWithIntegerCheck(new_r00,-1);
14938 if(!x1176.valid){
14939 continue;
14940 }
14941 CheckValue<IkReal> x1177=IKPowWithIntegerCheck(gconst13,-1);
14942 if(!x1177.valid){
14943 continue;
14944 }
14945 if( IKabs(((-1.0)*gconst13*(x1176.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1177.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1176.value)))+IKsqr((new_r01*(x1177.value)))-1) <= IKFAST_SINCOS_THRESH )
14946  continue;
14947 j3array[0]=IKatan2(((-1.0)*gconst13*(x1176.value)), (new_r01*(x1177.value)));
14948 sj3array[0]=IKsin(j3array[0]);
14949 cj3array[0]=IKcos(j3array[0]);
14950 if( j3array[0] > IKPI )
14951 {
14952  j3array[0]-=IK2PI;
14953 }
14954 else if( j3array[0] < -IKPI )
14955 { j3array[0]+=IK2PI;
14956 }
14957 j3valid[0] = true;
14958 for(int ij3 = 0; ij3 < 1; ++ij3)
14959 {
14960 if( !j3valid[ij3] )
14961 {
14962  continue;
14963 }
14964 _ij3[0] = ij3; _ij3[1] = -1;
14965 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14966 {
14967 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14968 {
14969  j3valid[iij3]=false; _ij3[1] = iij3; break;
14970 }
14971 }
14972 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14973 {
14974 IkReal evalcond[8];
14975 IkReal x1178=IKsin(j3);
14976 IkReal x1179=IKcos(j3);
14977 IkReal x1180=(gconst13*x1179);
14978 IkReal x1181=(gconst13*x1178);
14979 evalcond[0]=(new_r01*x1178);
14980 evalcond[1]=(new_r00*x1179);
14981 evalcond[2]=((-1.0)*x1181);
14982 evalcond[3]=((-1.0)*x1180);
14983 evalcond[4]=(gconst13+((new_r00*x1178)));
14984 evalcond[5]=(x1181+new_r00);
14985 evalcond[6]=(new_r01+(((-1.0)*x1180)));
14986 evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1179)));
14987 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 )
14988 {
14989 continue;
14990 }
14991 }
14992 
14993 {
14994 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14995 vinfos[0].jointtype = 1;
14996 vinfos[0].foffset = j0;
14997 vinfos[0].indices[0] = _ij0[0];
14998 vinfos[0].indices[1] = _ij0[1];
14999 vinfos[0].maxsolutions = _nj0;
15000 vinfos[1].jointtype = 1;
15001 vinfos[1].foffset = j1;
15002 vinfos[1].indices[0] = _ij1[0];
15003 vinfos[1].indices[1] = _ij1[1];
15004 vinfos[1].maxsolutions = _nj1;
15005 vinfos[2].jointtype = 1;
15006 vinfos[2].foffset = j2;
15007 vinfos[2].indices[0] = _ij2[0];
15008 vinfos[2].indices[1] = _ij2[1];
15009 vinfos[2].maxsolutions = _nj2;
15010 vinfos[3].jointtype = 1;
15011 vinfos[3].foffset = j3;
15012 vinfos[3].indices[0] = _ij3[0];
15013 vinfos[3].indices[1] = _ij3[1];
15014 vinfos[3].maxsolutions = _nj3;
15015 vinfos[4].jointtype = 1;
15016 vinfos[4].foffset = j4;
15017 vinfos[4].indices[0] = _ij4[0];
15018 vinfos[4].indices[1] = _ij4[1];
15019 vinfos[4].maxsolutions = _nj4;
15020 vinfos[5].jointtype = 1;
15021 vinfos[5].foffset = j5;
15022 vinfos[5].indices[0] = _ij5[0];
15023 vinfos[5].indices[1] = _ij5[1];
15024 vinfos[5].maxsolutions = _nj5;
15025 std::vector<int> vfree(0);
15026 solutions.AddSolution(vinfos,vfree);
15027 }
15028 }
15029 }
15030 
15031 }
15032 
15033 }
15034 
15035 } else
15036 {
15037 {
15038 IkReal j3array[1], cj3array[1], sj3array[1];
15039 bool j3valid[1]={false};
15040 _nj3 = 1;
15041 CheckValue<IkReal> x1182 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15042 if(!x1182.valid){
15043 continue;
15044 }
15045 CheckValue<IkReal> x1183=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15046 if(!x1183.valid){
15047 continue;
15048 }
15049 j3array[0]=((-1.5707963267949)+(x1182.value)+(((1.5707963267949)*(x1183.value))));
15050 sj3array[0]=IKsin(j3array[0]);
15051 cj3array[0]=IKcos(j3array[0]);
15052 if( j3array[0] > IKPI )
15053 {
15054  j3array[0]-=IK2PI;
15055 }
15056 else if( j3array[0] < -IKPI )
15057 { j3array[0]+=IK2PI;
15058 }
15059 j3valid[0] = true;
15060 for(int ij3 = 0; ij3 < 1; ++ij3)
15061 {
15062 if( !j3valid[ij3] )
15063 {
15064  continue;
15065 }
15066 _ij3[0] = ij3; _ij3[1] = -1;
15067 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15068 {
15069 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15070 {
15071  j3valid[iij3]=false; _ij3[1] = iij3; break;
15072 }
15073 }
15074 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15075 {
15076 IkReal evalcond[8];
15077 IkReal x1184=IKsin(j3);
15078 IkReal x1185=IKcos(j3);
15079 IkReal x1186=(gconst13*x1185);
15080 IkReal x1187=(gconst13*x1184);
15081 evalcond[0]=(new_r01*x1184);
15082 evalcond[1]=(new_r00*x1185);
15083 evalcond[2]=((-1.0)*x1187);
15084 evalcond[3]=((-1.0)*x1186);
15085 evalcond[4]=(gconst13+((new_r00*x1184)));
15086 evalcond[5]=(x1187+new_r00);
15087 evalcond[6]=(new_r01+(((-1.0)*x1186)));
15088 evalcond[7]=(((new_r01*x1185))+(((-1.0)*gconst13)));
15089 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 )
15090 {
15091 continue;
15092 }
15093 }
15094 
15095 {
15096 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15097 vinfos[0].jointtype = 1;
15098 vinfos[0].foffset = j0;
15099 vinfos[0].indices[0] = _ij0[0];
15100 vinfos[0].indices[1] = _ij0[1];
15101 vinfos[0].maxsolutions = _nj0;
15102 vinfos[1].jointtype = 1;
15103 vinfos[1].foffset = j1;
15104 vinfos[1].indices[0] = _ij1[0];
15105 vinfos[1].indices[1] = _ij1[1];
15106 vinfos[1].maxsolutions = _nj1;
15107 vinfos[2].jointtype = 1;
15108 vinfos[2].foffset = j2;
15109 vinfos[2].indices[0] = _ij2[0];
15110 vinfos[2].indices[1] = _ij2[1];
15111 vinfos[2].maxsolutions = _nj2;
15112 vinfos[3].jointtype = 1;
15113 vinfos[3].foffset = j3;
15114 vinfos[3].indices[0] = _ij3[0];
15115 vinfos[3].indices[1] = _ij3[1];
15116 vinfos[3].maxsolutions = _nj3;
15117 vinfos[4].jointtype = 1;
15118 vinfos[4].foffset = j4;
15119 vinfos[4].indices[0] = _ij4[0];
15120 vinfos[4].indices[1] = _ij4[1];
15121 vinfos[4].maxsolutions = _nj4;
15122 vinfos[5].jointtype = 1;
15123 vinfos[5].foffset = j5;
15124 vinfos[5].indices[0] = _ij5[0];
15125 vinfos[5].indices[1] = _ij5[1];
15126 vinfos[5].maxsolutions = _nj5;
15127 std::vector<int> vfree(0);
15128 solutions.AddSolution(vinfos,vfree);
15129 }
15130 }
15131 }
15132 
15133 }
15134 
15135 }
15136 
15137 }
15138 } while(0);
15139 if( bgotonextstatement )
15140 {
15141 bool bgotonextstatement = true;
15142 do
15143 {
15144 evalcond[0]=IKabs(new_r10);
15145 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15146 {
15147 bgotonextstatement=false;
15148 {
15149 IkReal j3eval[1];
15150 CheckValue<IkReal> x1189 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15151 if(!x1189.valid){
15152 continue;
15153 }
15154 IkReal x1188=((-1.0)*(x1189.value));
15155 sj4=0;
15156 cj4=-1.0;
15157 j4=3.14159265358979;
15158 sj5=gconst13;
15159 cj5=gconst14;
15160 j5=x1188;
15161 new_r10=0;
15162 IkReal gconst12=x1188;
15163 IkReal x1190 = new_r00*new_r00;
15164 if(IKabs(x1190)==0){
15165 continue;
15166 }
15167 IkReal gconst13=(new_r00*(pow(x1190,-0.5)));
15168 IkReal gconst14=0;
15169 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
15170 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15171 {
15172 {
15173 IkReal j3eval[1];
15174 CheckValue<IkReal> x1192 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15175 if(!x1192.valid){
15176 continue;
15177 }
15178 IkReal x1191=((-1.0)*(x1192.value));
15179 sj4=0;
15180 cj4=-1.0;
15181 j4=3.14159265358979;
15182 sj5=gconst13;
15183 cj5=gconst14;
15184 j5=x1191;
15185 new_r10=0;
15186 IkReal gconst12=x1191;
15187 IkReal x1193 = new_r00*new_r00;
15188 if(IKabs(x1193)==0){
15189 continue;
15190 }
15191 IkReal gconst13=(new_r00*(pow(x1193,-0.5)));
15192 IkReal gconst14=0;
15193 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
15194 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15195 {
15196 {
15197 IkReal j3eval[1];
15198 CheckValue<IkReal> x1195 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15199 if(!x1195.valid){
15200 continue;
15201 }
15202 IkReal x1194=((-1.0)*(x1195.value));
15203 sj4=0;
15204 cj4=-1.0;
15205 j4=3.14159265358979;
15206 sj5=gconst13;
15207 cj5=gconst14;
15208 j5=x1194;
15209 new_r10=0;
15210 IkReal gconst12=x1194;
15211 IkReal x1196 = new_r00*new_r00;
15212 if(IKabs(x1196)==0){
15213 continue;
15214 }
15215 IkReal gconst13=(new_r00*(pow(x1196,-0.5)));
15216 IkReal gconst14=0;
15217 j3eval[0]=new_r00;
15218 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15219 {
15220 continue; // 3 cases reached
15221 
15222 } else
15223 {
15224 {
15225 IkReal j3array[1], cj3array[1], sj3array[1];
15226 bool j3valid[1]={false};
15227 _nj3 = 1;
15228 CheckValue<IkReal> x1197=IKPowWithIntegerCheck(new_r00,-1);
15229 if(!x1197.valid){
15230 continue;
15231 }
15232 CheckValue<IkReal> x1198=IKPowWithIntegerCheck(gconst13,-1);
15233 if(!x1198.valid){
15234 continue;
15235 }
15236 if( IKabs(((-1.0)*gconst13*(x1197.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1198.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1197.value)))+IKsqr((new_r01*(x1198.value)))-1) <= IKFAST_SINCOS_THRESH )
15237  continue;
15238 j3array[0]=IKatan2(((-1.0)*gconst13*(x1197.value)), (new_r01*(x1198.value)));
15239 sj3array[0]=IKsin(j3array[0]);
15240 cj3array[0]=IKcos(j3array[0]);
15241 if( j3array[0] > IKPI )
15242 {
15243  j3array[0]-=IK2PI;
15244 }
15245 else if( j3array[0] < -IKPI )
15246 { j3array[0]+=IK2PI;
15247 }
15248 j3valid[0] = true;
15249 for(int ij3 = 0; ij3 < 1; ++ij3)
15250 {
15251 if( !j3valid[ij3] )
15252 {
15253  continue;
15254 }
15255 _ij3[0] = ij3; _ij3[1] = -1;
15256 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15257 {
15258 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15259 {
15260  j3valid[iij3]=false; _ij3[1] = iij3; break;
15261 }
15262 }
15263 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15264 {
15265 IkReal evalcond[8];
15266 IkReal x1199=IKcos(j3);
15267 IkReal x1200=IKsin(j3);
15268 IkReal x1201=(gconst13*x1199);
15269 IkReal x1202=(gconst13*x1200);
15270 evalcond[0]=(new_r00*x1199);
15271 evalcond[1]=((-1.0)*x1201);
15272 evalcond[2]=(((new_r00*x1200))+gconst13);
15273 evalcond[3]=(x1202+new_r00);
15274 evalcond[4]=((((-1.0)*x1201))+new_r01);
15275 evalcond[5]=((((-1.0)*x1202))+new_r11);
15276 evalcond[6]=(((new_r01*x1200))+(((-1.0)*new_r11*x1199)));
15277 evalcond[7]=(((new_r11*x1200))+(((-1.0)*gconst13))+((new_r01*x1199)));
15278 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 )
15279 {
15280 continue;
15281 }
15282 }
15283 
15284 {
15285 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15286 vinfos[0].jointtype = 1;
15287 vinfos[0].foffset = j0;
15288 vinfos[0].indices[0] = _ij0[0];
15289 vinfos[0].indices[1] = _ij0[1];
15290 vinfos[0].maxsolutions = _nj0;
15291 vinfos[1].jointtype = 1;
15292 vinfos[1].foffset = j1;
15293 vinfos[1].indices[0] = _ij1[0];
15294 vinfos[1].indices[1] = _ij1[1];
15295 vinfos[1].maxsolutions = _nj1;
15296 vinfos[2].jointtype = 1;
15297 vinfos[2].foffset = j2;
15298 vinfos[2].indices[0] = _ij2[0];
15299 vinfos[2].indices[1] = _ij2[1];
15300 vinfos[2].maxsolutions = _nj2;
15301 vinfos[3].jointtype = 1;
15302 vinfos[3].foffset = j3;
15303 vinfos[3].indices[0] = _ij3[0];
15304 vinfos[3].indices[1] = _ij3[1];
15305 vinfos[3].maxsolutions = _nj3;
15306 vinfos[4].jointtype = 1;
15307 vinfos[4].foffset = j4;
15308 vinfos[4].indices[0] = _ij4[0];
15309 vinfos[4].indices[1] = _ij4[1];
15310 vinfos[4].maxsolutions = _nj4;
15311 vinfos[5].jointtype = 1;
15312 vinfos[5].foffset = j5;
15313 vinfos[5].indices[0] = _ij5[0];
15314 vinfos[5].indices[1] = _ij5[1];
15315 vinfos[5].maxsolutions = _nj5;
15316 std::vector<int> vfree(0);
15317 solutions.AddSolution(vinfos,vfree);
15318 }
15319 }
15320 }
15321 
15322 }
15323 
15324 }
15325 
15326 } else
15327 {
15328 {
15329 IkReal j3array[1], cj3array[1], sj3array[1];
15330 bool j3valid[1]={false};
15331 _nj3 = 1;
15332 CheckValue<IkReal> x1203 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15333 if(!x1203.valid){
15334 continue;
15335 }
15336 CheckValue<IkReal> x1204=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15337 if(!x1204.valid){
15338 continue;
15339 }
15340 j3array[0]=((-1.5707963267949)+(x1203.value)+(((1.5707963267949)*(x1204.value))));
15341 sj3array[0]=IKsin(j3array[0]);
15342 cj3array[0]=IKcos(j3array[0]);
15343 if( j3array[0] > IKPI )
15344 {
15345  j3array[0]-=IK2PI;
15346 }
15347 else if( j3array[0] < -IKPI )
15348 { j3array[0]+=IK2PI;
15349 }
15350 j3valid[0] = true;
15351 for(int ij3 = 0; ij3 < 1; ++ij3)
15352 {
15353 if( !j3valid[ij3] )
15354 {
15355  continue;
15356 }
15357 _ij3[0] = ij3; _ij3[1] = -1;
15358 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15359 {
15360 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15361 {
15362  j3valid[iij3]=false; _ij3[1] = iij3; break;
15363 }
15364 }
15365 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15366 {
15367 IkReal evalcond[8];
15368 IkReal x1205=IKcos(j3);
15369 IkReal x1206=IKsin(j3);
15370 IkReal x1207=(gconst13*x1205);
15371 IkReal x1208=(gconst13*x1206);
15372 evalcond[0]=(new_r00*x1205);
15373 evalcond[1]=((-1.0)*x1207);
15374 evalcond[2]=(((new_r00*x1206))+gconst13);
15375 evalcond[3]=(x1208+new_r00);
15376 evalcond[4]=((((-1.0)*x1207))+new_r01);
15377 evalcond[5]=((((-1.0)*x1208))+new_r11);
15378 evalcond[6]=(((new_r01*x1206))+(((-1.0)*new_r11*x1205)));
15379 evalcond[7]=(((new_r11*x1206))+((new_r01*x1205))+(((-1.0)*gconst13)));
15380 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 )
15381 {
15382 continue;
15383 }
15384 }
15385 
15386 {
15387 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15388 vinfos[0].jointtype = 1;
15389 vinfos[0].foffset = j0;
15390 vinfos[0].indices[0] = _ij0[0];
15391 vinfos[0].indices[1] = _ij0[1];
15392 vinfos[0].maxsolutions = _nj0;
15393 vinfos[1].jointtype = 1;
15394 vinfos[1].foffset = j1;
15395 vinfos[1].indices[0] = _ij1[0];
15396 vinfos[1].indices[1] = _ij1[1];
15397 vinfos[1].maxsolutions = _nj1;
15398 vinfos[2].jointtype = 1;
15399 vinfos[2].foffset = j2;
15400 vinfos[2].indices[0] = _ij2[0];
15401 vinfos[2].indices[1] = _ij2[1];
15402 vinfos[2].maxsolutions = _nj2;
15403 vinfos[3].jointtype = 1;
15404 vinfos[3].foffset = j3;
15405 vinfos[3].indices[0] = _ij3[0];
15406 vinfos[3].indices[1] = _ij3[1];
15407 vinfos[3].maxsolutions = _nj3;
15408 vinfos[4].jointtype = 1;
15409 vinfos[4].foffset = j4;
15410 vinfos[4].indices[0] = _ij4[0];
15411 vinfos[4].indices[1] = _ij4[1];
15412 vinfos[4].maxsolutions = _nj4;
15413 vinfos[5].jointtype = 1;
15414 vinfos[5].foffset = j5;
15415 vinfos[5].indices[0] = _ij5[0];
15416 vinfos[5].indices[1] = _ij5[1];
15417 vinfos[5].maxsolutions = _nj5;
15418 std::vector<int> vfree(0);
15419 solutions.AddSolution(vinfos,vfree);
15420 }
15421 }
15422 }
15423 
15424 }
15425 
15426 }
15427 
15428 } else
15429 {
15430 {
15431 IkReal j3array[1], cj3array[1], sj3array[1];
15432 bool j3valid[1]={false};
15433 _nj3 = 1;
15434 CheckValue<IkReal> x1209 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15435 if(!x1209.valid){
15436 continue;
15437 }
15438 CheckValue<IkReal> x1210=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15439 if(!x1210.valid){
15440 continue;
15441 }
15442 j3array[0]=((-1.5707963267949)+(x1209.value)+(((1.5707963267949)*(x1210.value))));
15443 sj3array[0]=IKsin(j3array[0]);
15444 cj3array[0]=IKcos(j3array[0]);
15445 if( j3array[0] > IKPI )
15446 {
15447  j3array[0]-=IK2PI;
15448 }
15449 else if( j3array[0] < -IKPI )
15450 { j3array[0]+=IK2PI;
15451 }
15452 j3valid[0] = true;
15453 for(int ij3 = 0; ij3 < 1; ++ij3)
15454 {
15455 if( !j3valid[ij3] )
15456 {
15457  continue;
15458 }
15459 _ij3[0] = ij3; _ij3[1] = -1;
15460 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15461 {
15462 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15463 {
15464  j3valid[iij3]=false; _ij3[1] = iij3; break;
15465 }
15466 }
15467 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15468 {
15469 IkReal evalcond[8];
15470 IkReal x1211=IKcos(j3);
15471 IkReal x1212=IKsin(j3);
15472 IkReal x1213=(gconst13*x1211);
15473 IkReal x1214=(gconst13*x1212);
15474 evalcond[0]=(new_r00*x1211);
15475 evalcond[1]=((-1.0)*x1213);
15476 evalcond[2]=(gconst13+((new_r00*x1212)));
15477 evalcond[3]=(x1214+new_r00);
15478 evalcond[4]=(new_r01+(((-1.0)*x1213)));
15479 evalcond[5]=(new_r11+(((-1.0)*x1214)));
15480 evalcond[6]=(((new_r01*x1212))+(((-1.0)*new_r11*x1211)));
15481 evalcond[7]=(((new_r11*x1212))+((new_r01*x1211))+(((-1.0)*gconst13)));
15482 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 )
15483 {
15484 continue;
15485 }
15486 }
15487 
15488 {
15489 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15490 vinfos[0].jointtype = 1;
15491 vinfos[0].foffset = j0;
15492 vinfos[0].indices[0] = _ij0[0];
15493 vinfos[0].indices[1] = _ij0[1];
15494 vinfos[0].maxsolutions = _nj0;
15495 vinfos[1].jointtype = 1;
15496 vinfos[1].foffset = j1;
15497 vinfos[1].indices[0] = _ij1[0];
15498 vinfos[1].indices[1] = _ij1[1];
15499 vinfos[1].maxsolutions = _nj1;
15500 vinfos[2].jointtype = 1;
15501 vinfos[2].foffset = j2;
15502 vinfos[2].indices[0] = _ij2[0];
15503 vinfos[2].indices[1] = _ij2[1];
15504 vinfos[2].maxsolutions = _nj2;
15505 vinfos[3].jointtype = 1;
15506 vinfos[3].foffset = j3;
15507 vinfos[3].indices[0] = _ij3[0];
15508 vinfos[3].indices[1] = _ij3[1];
15509 vinfos[3].maxsolutions = _nj3;
15510 vinfos[4].jointtype = 1;
15511 vinfos[4].foffset = j4;
15512 vinfos[4].indices[0] = _ij4[0];
15513 vinfos[4].indices[1] = _ij4[1];
15514 vinfos[4].maxsolutions = _nj4;
15515 vinfos[5].jointtype = 1;
15516 vinfos[5].foffset = j5;
15517 vinfos[5].indices[0] = _ij5[0];
15518 vinfos[5].indices[1] = _ij5[1];
15519 vinfos[5].maxsolutions = _nj5;
15520 std::vector<int> vfree(0);
15521 solutions.AddSolution(vinfos,vfree);
15522 }
15523 }
15524 }
15525 
15526 }
15527 
15528 }
15529 
15530 }
15531 } while(0);
15532 if( bgotonextstatement )
15533 {
15534 bool bgotonextstatement = true;
15535 do
15536 {
15537 if( 1 )
15538 {
15539 bgotonextstatement=false;
15540 continue; // branch miss [j3]
15541 
15542 }
15543 } while(0);
15544 if( bgotonextstatement )
15545 {
15546 }
15547 }
15548 }
15549 }
15550 }
15551 }
15552 }
15553 
15554 } else
15555 {
15556 {
15557 IkReal j3array[1], cj3array[1], sj3array[1];
15558 bool j3valid[1]={false};
15559 _nj3 = 1;
15560 IkReal x1215=((1.0)*gconst14);
15561 CheckValue<IkReal> x1216=IKPowWithIntegerCheck(IKsign((((gconst13*new_r00))+(((-1.0)*new_r10*x1215)))),-1);
15562 if(!x1216.valid){
15563 continue;
15564 }
15565 CheckValue<IkReal> x1217 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst13*x1215)))),IKFAST_ATAN2_MAGTHRESH);
15566 if(!x1217.valid){
15567 continue;
15568 }
15569 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1216.value)))+(x1217.value));
15570 sj3array[0]=IKsin(j3array[0]);
15571 cj3array[0]=IKcos(j3array[0]);
15572 if( j3array[0] > IKPI )
15573 {
15574  j3array[0]-=IK2PI;
15575 }
15576 else if( j3array[0] < -IKPI )
15577 { j3array[0]+=IK2PI;
15578 }
15579 j3valid[0] = true;
15580 for(int ij3 = 0; ij3 < 1; ++ij3)
15581 {
15582 if( !j3valid[ij3] )
15583 {
15584  continue;
15585 }
15586 _ij3[0] = ij3; _ij3[1] = -1;
15587 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15588 {
15589 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15590 {
15591  j3valid[iij3]=false; _ij3[1] = iij3; break;
15592 }
15593 }
15594 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15595 {
15596 IkReal evalcond[8];
15597 IkReal x1218=IKsin(j3);
15598 IkReal x1219=IKcos(j3);
15599 IkReal x1220=(gconst14*x1218);
15600 IkReal x1221=((1.0)*x1219);
15601 IkReal x1222=(gconst13*x1218);
15602 IkReal x1223=(gconst13*x1221);
15603 evalcond[0]=(((new_r10*x1218))+gconst14+((new_r00*x1219)));
15604 evalcond[1]=(x1222+((gconst14*x1219))+new_r00);
15605 evalcond[2]=(gconst13+((new_r00*x1218))+(((-1.0)*new_r10*x1221)));
15606 evalcond[3]=(gconst14+((new_r01*x1218))+(((-1.0)*new_r11*x1221)));
15607 evalcond[4]=(x1220+(((-1.0)*x1223))+new_r01);
15608 evalcond[5]=(x1220+(((-1.0)*x1223))+new_r10);
15609 evalcond[6]=(((new_r11*x1218))+((new_r01*x1219))+(((-1.0)*gconst13)));
15610 evalcond[7]=((((-1.0)*x1222))+new_r11+(((-1.0)*gconst14*x1221)));
15611 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 )
15612 {
15613 continue;
15614 }
15615 }
15616 
15617 {
15618 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15619 vinfos[0].jointtype = 1;
15620 vinfos[0].foffset = j0;
15621 vinfos[0].indices[0] = _ij0[0];
15622 vinfos[0].indices[1] = _ij0[1];
15623 vinfos[0].maxsolutions = _nj0;
15624 vinfos[1].jointtype = 1;
15625 vinfos[1].foffset = j1;
15626 vinfos[1].indices[0] = _ij1[0];
15627 vinfos[1].indices[1] = _ij1[1];
15628 vinfos[1].maxsolutions = _nj1;
15629 vinfos[2].jointtype = 1;
15630 vinfos[2].foffset = j2;
15631 vinfos[2].indices[0] = _ij2[0];
15632 vinfos[2].indices[1] = _ij2[1];
15633 vinfos[2].maxsolutions = _nj2;
15634 vinfos[3].jointtype = 1;
15635 vinfos[3].foffset = j3;
15636 vinfos[3].indices[0] = _ij3[0];
15637 vinfos[3].indices[1] = _ij3[1];
15638 vinfos[3].maxsolutions = _nj3;
15639 vinfos[4].jointtype = 1;
15640 vinfos[4].foffset = j4;
15641 vinfos[4].indices[0] = _ij4[0];
15642 vinfos[4].indices[1] = _ij4[1];
15643 vinfos[4].maxsolutions = _nj4;
15644 vinfos[5].jointtype = 1;
15645 vinfos[5].foffset = j5;
15646 vinfos[5].indices[0] = _ij5[0];
15647 vinfos[5].indices[1] = _ij5[1];
15648 vinfos[5].maxsolutions = _nj5;
15649 std::vector<int> vfree(0);
15650 solutions.AddSolution(vinfos,vfree);
15651 }
15652 }
15653 }
15654 
15655 }
15656 
15657 }
15658 
15659 } else
15660 {
15661 {
15662 IkReal j3array[1], cj3array[1], sj3array[1];
15663 bool j3valid[1]={false};
15664 _nj3 = 1;
15665 IkReal x1224=((1.0)*gconst14);
15666 CheckValue<IkReal> x1225 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+((new_r00*new_r11)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst13*x1224)))),IKFAST_ATAN2_MAGTHRESH);
15667 if(!x1225.valid){
15668 continue;
15669 }
15670 CheckValue<IkReal> x1226=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r11))+(((-1.0)*new_r01*x1224)))),-1);
15671 if(!x1226.valid){
15672 continue;
15673 }
15674 j3array[0]=((-1.5707963267949)+(x1225.value)+(((1.5707963267949)*(x1226.value))));
15675 sj3array[0]=IKsin(j3array[0]);
15676 cj3array[0]=IKcos(j3array[0]);
15677 if( j3array[0] > IKPI )
15678 {
15679  j3array[0]-=IK2PI;
15680 }
15681 else if( j3array[0] < -IKPI )
15682 { j3array[0]+=IK2PI;
15683 }
15684 j3valid[0] = true;
15685 for(int ij3 = 0; ij3 < 1; ++ij3)
15686 {
15687 if( !j3valid[ij3] )
15688 {
15689  continue;
15690 }
15691 _ij3[0] = ij3; _ij3[1] = -1;
15692 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15693 {
15694 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15695 {
15696  j3valid[iij3]=false; _ij3[1] = iij3; break;
15697 }
15698 }
15699 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15700 {
15701 IkReal evalcond[8];
15702 IkReal x1227=IKsin(j3);
15703 IkReal x1228=IKcos(j3);
15704 IkReal x1229=(gconst14*x1227);
15705 IkReal x1230=((1.0)*x1228);
15706 IkReal x1231=(gconst13*x1227);
15707 IkReal x1232=(gconst13*x1230);
15708 evalcond[0]=(gconst14+((new_r00*x1228))+((new_r10*x1227)));
15709 evalcond[1]=(x1231+new_r00+((gconst14*x1228)));
15710 evalcond[2]=(gconst13+((new_r00*x1227))+(((-1.0)*new_r10*x1230)));
15711 evalcond[3]=(gconst14+((new_r01*x1227))+(((-1.0)*new_r11*x1230)));
15712 evalcond[4]=(x1229+(((-1.0)*x1232))+new_r01);
15713 evalcond[5]=(x1229+(((-1.0)*x1232))+new_r10);
15714 evalcond[6]=(((new_r11*x1227))+((new_r01*x1228))+(((-1.0)*gconst13)));
15715 evalcond[7]=((((-1.0)*x1231))+new_r11+(((-1.0)*gconst14*x1230)));
15716 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 )
15717 {
15718 continue;
15719 }
15720 }
15721 
15722 {
15723 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15724 vinfos[0].jointtype = 1;
15725 vinfos[0].foffset = j0;
15726 vinfos[0].indices[0] = _ij0[0];
15727 vinfos[0].indices[1] = _ij0[1];
15728 vinfos[0].maxsolutions = _nj0;
15729 vinfos[1].jointtype = 1;
15730 vinfos[1].foffset = j1;
15731 vinfos[1].indices[0] = _ij1[0];
15732 vinfos[1].indices[1] = _ij1[1];
15733 vinfos[1].maxsolutions = _nj1;
15734 vinfos[2].jointtype = 1;
15735 vinfos[2].foffset = j2;
15736 vinfos[2].indices[0] = _ij2[0];
15737 vinfos[2].indices[1] = _ij2[1];
15738 vinfos[2].maxsolutions = _nj2;
15739 vinfos[3].jointtype = 1;
15740 vinfos[3].foffset = j3;
15741 vinfos[3].indices[0] = _ij3[0];
15742 vinfos[3].indices[1] = _ij3[1];
15743 vinfos[3].maxsolutions = _nj3;
15744 vinfos[4].jointtype = 1;
15745 vinfos[4].foffset = j4;
15746 vinfos[4].indices[0] = _ij4[0];
15747 vinfos[4].indices[1] = _ij4[1];
15748 vinfos[4].maxsolutions = _nj4;
15749 vinfos[5].jointtype = 1;
15750 vinfos[5].foffset = j5;
15751 vinfos[5].indices[0] = _ij5[0];
15752 vinfos[5].indices[1] = _ij5[1];
15753 vinfos[5].maxsolutions = _nj5;
15754 std::vector<int> vfree(0);
15755 solutions.AddSolution(vinfos,vfree);
15756 }
15757 }
15758 }
15759 
15760 }
15761 
15762 }
15763 
15764 } else
15765 {
15766 {
15767 IkReal j3array[1], cj3array[1], sj3array[1];
15768 bool j3valid[1]={false};
15769 _nj3 = 1;
15770 IkReal x1233=((1.0)*new_r10);
15771 CheckValue<IkReal> x1234=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1233)))),-1);
15772 if(!x1234.valid){
15773 continue;
15774 }
15775 CheckValue<IkReal> x1235 = IKatan2WithCheck(IkReal((((gconst14*new_r00))+((gconst14*new_r11)))),IkReal((((gconst14*new_r01))+(((-1.0)*gconst14*x1233)))),IKFAST_ATAN2_MAGTHRESH);
15776 if(!x1235.valid){
15777 continue;
15778 }
15779 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1234.value)))+(x1235.value));
15780 sj3array[0]=IKsin(j3array[0]);
15781 cj3array[0]=IKcos(j3array[0]);
15782 if( j3array[0] > IKPI )
15783 {
15784  j3array[0]-=IK2PI;
15785 }
15786 else if( j3array[0] < -IKPI )
15787 { j3array[0]+=IK2PI;
15788 }
15789 j3valid[0] = true;
15790 for(int ij3 = 0; ij3 < 1; ++ij3)
15791 {
15792 if( !j3valid[ij3] )
15793 {
15794  continue;
15795 }
15796 _ij3[0] = ij3; _ij3[1] = -1;
15797 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15798 {
15799 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15800 {
15801  j3valid[iij3]=false; _ij3[1] = iij3; break;
15802 }
15803 }
15804 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15805 {
15806 IkReal evalcond[8];
15807 IkReal x1236=IKsin(j3);
15808 IkReal x1237=IKcos(j3);
15809 IkReal x1238=(gconst14*x1236);
15810 IkReal x1239=((1.0)*x1237);
15811 IkReal x1240=(gconst13*x1236);
15812 IkReal x1241=(gconst13*x1239);
15813 evalcond[0]=(gconst14+((new_r10*x1236))+((new_r00*x1237)));
15814 evalcond[1]=(((gconst14*x1237))+x1240+new_r00);
15815 evalcond[2]=(gconst13+((new_r00*x1236))+(((-1.0)*new_r10*x1239)));
15816 evalcond[3]=(gconst14+((new_r01*x1236))+(((-1.0)*new_r11*x1239)));
15817 evalcond[4]=(x1238+(((-1.0)*x1241))+new_r01);
15818 evalcond[5]=(x1238+(((-1.0)*x1241))+new_r10);
15819 evalcond[6]=(((new_r01*x1237))+((new_r11*x1236))+(((-1.0)*gconst13)));
15820 evalcond[7]=((((-1.0)*x1240))+new_r11+(((-1.0)*gconst14*x1239)));
15821 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 )
15822 {
15823 continue;
15824 }
15825 }
15826 
15827 {
15828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15829 vinfos[0].jointtype = 1;
15830 vinfos[0].foffset = j0;
15831 vinfos[0].indices[0] = _ij0[0];
15832 vinfos[0].indices[1] = _ij0[1];
15833 vinfos[0].maxsolutions = _nj0;
15834 vinfos[1].jointtype = 1;
15835 vinfos[1].foffset = j1;
15836 vinfos[1].indices[0] = _ij1[0];
15837 vinfos[1].indices[1] = _ij1[1];
15838 vinfos[1].maxsolutions = _nj1;
15839 vinfos[2].jointtype = 1;
15840 vinfos[2].foffset = j2;
15841 vinfos[2].indices[0] = _ij2[0];
15842 vinfos[2].indices[1] = _ij2[1];
15843 vinfos[2].maxsolutions = _nj2;
15844 vinfos[3].jointtype = 1;
15845 vinfos[3].foffset = j3;
15846 vinfos[3].indices[0] = _ij3[0];
15847 vinfos[3].indices[1] = _ij3[1];
15848 vinfos[3].maxsolutions = _nj3;
15849 vinfos[4].jointtype = 1;
15850 vinfos[4].foffset = j4;
15851 vinfos[4].indices[0] = _ij4[0];
15852 vinfos[4].indices[1] = _ij4[1];
15853 vinfos[4].maxsolutions = _nj4;
15854 vinfos[5].jointtype = 1;
15855 vinfos[5].foffset = j5;
15856 vinfos[5].indices[0] = _ij5[0];
15857 vinfos[5].indices[1] = _ij5[1];
15858 vinfos[5].maxsolutions = _nj5;
15859 std::vector<int> vfree(0);
15860 solutions.AddSolution(vinfos,vfree);
15861 }
15862 }
15863 }
15864 
15865 }
15866 
15867 }
15868 
15869 }
15870 } while(0);
15871 if( bgotonextstatement )
15872 {
15873 bool bgotonextstatement = true;
15874 do
15875 {
15876 IkReal x1242=((-1.0)*new_r00);
15877 IkReal x1244 = ((new_r10*new_r10)+(new_r00*new_r00));
15878 if(IKabs(x1244)==0){
15879 continue;
15880 }
15881 IkReal x1243=pow(x1244,-0.5);
15882 CheckValue<IkReal> x1245 = IKatan2WithCheck(IkReal(x1242),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15883 if(!x1245.valid){
15884 continue;
15885 }
15886 IkReal gconst15=((3.14159265358979)+(((-1.0)*(x1245.value))));
15887 IkReal gconst16=(x1242*x1243);
15888 IkReal gconst17=((1.0)*new_r10*x1243);
15889 CheckValue<IkReal> x1246 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15890 if(!x1246.valid){
15891 continue;
15892 }
15893 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x1246.value)+j5)))), 6.28318530717959)));
15894 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15895 {
15896 bgotonextstatement=false;
15897 {
15898 IkReal j3eval[3];
15899 IkReal x1247=((-1.0)*new_r00);
15900 CheckValue<IkReal> x1250 = IKatan2WithCheck(IkReal(x1247),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15901 if(!x1250.valid){
15902 continue;
15903 }
15904 IkReal x1248=((1.0)*(x1250.value));
15905 IkReal x1249=x1243;
15906 sj4=0;
15907 cj4=-1.0;
15908 j4=3.14159265358979;
15909 sj5=gconst16;
15910 cj5=gconst17;
15911 j5=((3.14159265)+(((-1.0)*x1248)));
15912 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1248)));
15913 IkReal gconst16=(x1247*x1249);
15914 IkReal gconst17=((1.0)*new_r10*x1249);
15915 IkReal x1251=new_r10*new_r10;
15916 IkReal x1252=(new_r10*new_r11);
15917 IkReal x1253=((((-1.0)*new_r00*new_r01))+(((-1.0)*x1252)));
15918 IkReal x1254=x1243;
15919 IkReal x1255=(new_r10*x1254);
15920 j3eval[0]=x1253;
15921 j3eval[1]=((IKabs((((new_r01*x1255))+(((-1.0)*x1251*x1254)))))+(IKabs((((new_r00*x1255))+((x1252*x1254))))));
15922 j3eval[2]=IKsign(x1253);
15923 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
15924 {
15925 {
15926 IkReal j3eval[1];
15927 IkReal x1256=((-1.0)*new_r00);
15928 CheckValue<IkReal> x1259 = IKatan2WithCheck(IkReal(x1256),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15929 if(!x1259.valid){
15930 continue;
15931 }
15932 IkReal x1257=((1.0)*(x1259.value));
15933 IkReal x1258=x1243;
15934 sj4=0;
15935 cj4=-1.0;
15936 j4=3.14159265358979;
15937 sj5=gconst16;
15938 cj5=gconst17;
15939 j5=((3.14159265)+(((-1.0)*x1257)));
15940 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1257)));
15941 IkReal gconst16=(x1256*x1258);
15942 IkReal gconst17=((1.0)*new_r10*x1258);
15943 IkReal x1260=new_r10*new_r10;
15944 IkReal x1261=new_r00*new_r00*new_r00;
15945 CheckValue<IkReal> x1265=IKPowWithIntegerCheck((x1260+(new_r00*new_r00)),-1);
15946 if(!x1265.valid){
15947 continue;
15948 }
15949 IkReal x1262=x1265.value;
15950 IkReal x1263=(x1260*x1262);
15951 IkReal x1264=(x1261*x1262);
15952 j3eval[0]=((IKabs((((new_r00*new_r10*x1262))+((new_r01*x1264))+((new_r00*new_r01*x1263)))))+(IKabs((x1263+((new_r11*x1264))+((new_r00*new_r11*x1263))))));
15953 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15954 {
15955 {
15956 IkReal j3eval[1];
15957 IkReal x1266=((-1.0)*new_r00);
15958 CheckValue<IkReal> x1269 = IKatan2WithCheck(IkReal(x1266),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15959 if(!x1269.valid){
15960 continue;
15961 }
15962 IkReal x1267=((1.0)*(x1269.value));
15963 IkReal x1268=x1243;
15964 sj4=0;
15965 cj4=-1.0;
15966 j4=3.14159265358979;
15967 sj5=gconst16;
15968 cj5=gconst17;
15969 j5=((3.14159265)+(((-1.0)*x1267)));
15970 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1267)));
15971 IkReal gconst16=(x1266*x1268);
15972 IkReal gconst17=((1.0)*new_r10*x1268);
15973 IkReal x1270=new_r10*new_r10;
15974 IkReal x1271=new_r00*new_r00;
15975 CheckValue<IkReal> x1275=IKPowWithIntegerCheck((x1270+x1271),-1);
15976 if(!x1275.valid){
15977 continue;
15978 }
15979 IkReal x1272=x1275.value;
15980 IkReal x1273=(new_r10*x1272);
15981 IkReal x1274=(x1270*x1272);
15982 j3eval[0]=((IKabs(((((-1.0)*x1271*x1274))+x1274+(((-1.0)*x1272*(x1271*x1271))))))+(IKabs((((new_r00*x1273))+((x1273*(new_r00*new_r00*new_r00)))+((new_r00*x1273*(new_r10*new_r10)))))));
15983 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15984 {
15985 {
15986 IkReal evalcond[2];
15987 bool bgotonextstatement = true;
15988 do
15989 {
15990 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
15991 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15992 {
15993 bgotonextstatement=false;
15994 {
15995 IkReal j3eval[1];
15996 CheckValue<IkReal> x1277 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15997 if(!x1277.valid){
15998 continue;
15999 }
16000 IkReal x1276=((1.0)*(x1277.value));
16001 sj4=0;
16002 cj4=-1.0;
16003 j4=3.14159265358979;
16004 sj5=gconst16;
16005 cj5=gconst17;
16006 j5=((3.14159265)+(((-1.0)*x1276)));
16007 new_r11=0;
16008 new_r00=0;
16009 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1276)));
16010 IkReal gconst16=0;
16011 IkReal x1278 = new_r10*new_r10;
16012 if(IKabs(x1278)==0){
16013 continue;
16014 }
16015 IkReal gconst17=((1.0)*new_r10*(pow(x1278,-0.5)));
16016 j3eval[0]=new_r01;
16017 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16018 {
16019 {
16020 IkReal j3array[2], cj3array[2], sj3array[2];
16021 bool j3valid[2]={false};
16022 _nj3 = 2;
16023 CheckValue<IkReal> x1279=IKPowWithIntegerCheck(gconst17,-1);
16024 if(!x1279.valid){
16025 continue;
16026 }
16027 sj3array[0]=((-1.0)*new_r01*(x1279.value));
16028 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16029 {
16030  j3valid[0] = j3valid[1] = true;
16031  j3array[0] = IKasin(sj3array[0]);
16032  cj3array[0] = IKcos(j3array[0]);
16033  sj3array[1] = sj3array[0];
16034  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16035  cj3array[1] = -cj3array[0];
16036 }
16037 else if( isnan(sj3array[0]) )
16038 {
16039  // probably any value will work
16040  j3valid[0] = true;
16041  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16042 }
16043 for(int ij3 = 0; ij3 < 2; ++ij3)
16044 {
16045 if( !j3valid[ij3] )
16046 {
16047  continue;
16048 }
16049 _ij3[0] = ij3; _ij3[1] = -1;
16050 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16051 {
16052 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16053 {
16054  j3valid[iij3]=false; _ij3[1] = iij3; break;
16055 }
16056 }
16057 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16058 {
16059 IkReal evalcond[6];
16060 IkReal x1280=IKcos(j3);
16061 IkReal x1281=IKsin(j3);
16062 evalcond[0]=(new_r01*x1280);
16063 evalcond[1]=(gconst17*x1280);
16064 evalcond[2]=((-1.0)*new_r10*x1280);
16065 evalcond[3]=(gconst17+((new_r01*x1281)));
16066 evalcond[4]=(((new_r10*x1281))+gconst17);
16067 evalcond[5]=(new_r10+((gconst17*x1281)));
16068 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 )
16069 {
16070 continue;
16071 }
16072 }
16073 
16074 {
16075 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16076 vinfos[0].jointtype = 1;
16077 vinfos[0].foffset = j0;
16078 vinfos[0].indices[0] = _ij0[0];
16079 vinfos[0].indices[1] = _ij0[1];
16080 vinfos[0].maxsolutions = _nj0;
16081 vinfos[1].jointtype = 1;
16082 vinfos[1].foffset = j1;
16083 vinfos[1].indices[0] = _ij1[0];
16084 vinfos[1].indices[1] = _ij1[1];
16085 vinfos[1].maxsolutions = _nj1;
16086 vinfos[2].jointtype = 1;
16087 vinfos[2].foffset = j2;
16088 vinfos[2].indices[0] = _ij2[0];
16089 vinfos[2].indices[1] = _ij2[1];
16090 vinfos[2].maxsolutions = _nj2;
16091 vinfos[3].jointtype = 1;
16092 vinfos[3].foffset = j3;
16093 vinfos[3].indices[0] = _ij3[0];
16094 vinfos[3].indices[1] = _ij3[1];
16095 vinfos[3].maxsolutions = _nj3;
16096 vinfos[4].jointtype = 1;
16097 vinfos[4].foffset = j4;
16098 vinfos[4].indices[0] = _ij4[0];
16099 vinfos[4].indices[1] = _ij4[1];
16100 vinfos[4].maxsolutions = _nj4;
16101 vinfos[5].jointtype = 1;
16102 vinfos[5].foffset = j5;
16103 vinfos[5].indices[0] = _ij5[0];
16104 vinfos[5].indices[1] = _ij5[1];
16105 vinfos[5].maxsolutions = _nj5;
16106 std::vector<int> vfree(0);
16107 solutions.AddSolution(vinfos,vfree);
16108 }
16109 }
16110 }
16111 
16112 } else
16113 {
16114 {
16115 IkReal j3array[2], cj3array[2], sj3array[2];
16116 bool j3valid[2]={false};
16117 _nj3 = 2;
16118 CheckValue<IkReal> x1282=IKPowWithIntegerCheck(new_r01,-1);
16119 if(!x1282.valid){
16120 continue;
16121 }
16122 sj3array[0]=((-1.0)*gconst17*(x1282.value));
16123 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16124 {
16125  j3valid[0] = j3valid[1] = true;
16126  j3array[0] = IKasin(sj3array[0]);
16127  cj3array[0] = IKcos(j3array[0]);
16128  sj3array[1] = sj3array[0];
16129  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16130  cj3array[1] = -cj3array[0];
16131 }
16132 else if( isnan(sj3array[0]) )
16133 {
16134  // probably any value will work
16135  j3valid[0] = true;
16136  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16137 }
16138 for(int ij3 = 0; ij3 < 2; ++ij3)
16139 {
16140 if( !j3valid[ij3] )
16141 {
16142  continue;
16143 }
16144 _ij3[0] = ij3; _ij3[1] = -1;
16145 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16146 {
16147 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16148 {
16149  j3valid[iij3]=false; _ij3[1] = iij3; break;
16150 }
16151 }
16152 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16153 {
16154 IkReal evalcond[6];
16155 IkReal x1283=IKcos(j3);
16156 IkReal x1284=IKsin(j3);
16157 IkReal x1285=(gconst17*x1284);
16158 evalcond[0]=(new_r01*x1283);
16159 evalcond[1]=(gconst17*x1283);
16160 evalcond[2]=((-1.0)*new_r10*x1283);
16161 evalcond[3]=(x1285+new_r01);
16162 evalcond[4]=(((new_r10*x1284))+gconst17);
16163 evalcond[5]=(x1285+new_r10);
16164 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 )
16165 {
16166 continue;
16167 }
16168 }
16169 
16170 {
16171 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16172 vinfos[0].jointtype = 1;
16173 vinfos[0].foffset = j0;
16174 vinfos[0].indices[0] = _ij0[0];
16175 vinfos[0].indices[1] = _ij0[1];
16176 vinfos[0].maxsolutions = _nj0;
16177 vinfos[1].jointtype = 1;
16178 vinfos[1].foffset = j1;
16179 vinfos[1].indices[0] = _ij1[0];
16180 vinfos[1].indices[1] = _ij1[1];
16181 vinfos[1].maxsolutions = _nj1;
16182 vinfos[2].jointtype = 1;
16183 vinfos[2].foffset = j2;
16184 vinfos[2].indices[0] = _ij2[0];
16185 vinfos[2].indices[1] = _ij2[1];
16186 vinfos[2].maxsolutions = _nj2;
16187 vinfos[3].jointtype = 1;
16188 vinfos[3].foffset = j3;
16189 vinfos[3].indices[0] = _ij3[0];
16190 vinfos[3].indices[1] = _ij3[1];
16191 vinfos[3].maxsolutions = _nj3;
16192 vinfos[4].jointtype = 1;
16193 vinfos[4].foffset = j4;
16194 vinfos[4].indices[0] = _ij4[0];
16195 vinfos[4].indices[1] = _ij4[1];
16196 vinfos[4].maxsolutions = _nj4;
16197 vinfos[5].jointtype = 1;
16198 vinfos[5].foffset = j5;
16199 vinfos[5].indices[0] = _ij5[0];
16200 vinfos[5].indices[1] = _ij5[1];
16201 vinfos[5].maxsolutions = _nj5;
16202 std::vector<int> vfree(0);
16203 solutions.AddSolution(vinfos,vfree);
16204 }
16205 }
16206 }
16207 
16208 }
16209 
16210 }
16211 
16212 }
16213 } while(0);
16214 if( bgotonextstatement )
16215 {
16216 bool bgotonextstatement = true;
16217 do
16218 {
16219 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
16220 evalcond[1]=gconst17;
16221 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
16222 {
16223 bgotonextstatement=false;
16224 {
16225 IkReal j3eval[3];
16226 IkReal x1286=((-1.0)*new_r00);
16227 CheckValue<IkReal> x1288 = IKatan2WithCheck(IkReal(x1286),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16228 if(!x1288.valid){
16229 continue;
16230 }
16231 IkReal x1287=((1.0)*(x1288.value));
16232 sj4=0;
16233 cj4=-1.0;
16234 j4=3.14159265358979;
16235 sj5=gconst16;
16236 cj5=gconst17;
16237 j5=((3.14159265)+(((-1.0)*x1287)));
16238 new_r11=0;
16239 new_r01=0;
16240 new_r22=0;
16241 new_r20=0;
16242 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1287)));
16243 IkReal gconst16=x1286;
16244 IkReal gconst17=((1.0)*new_r10);
16245 j3eval[0]=1.0;
16246 j3eval[1]=1.0;
16247 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
16248 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16249 {
16250 {
16251 IkReal j3eval[3];
16252 IkReal x1289=((-1.0)*new_r00);
16253 CheckValue<IkReal> x1291 = IKatan2WithCheck(IkReal(x1289),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16254 if(!x1291.valid){
16255 continue;
16256 }
16257 IkReal x1290=((1.0)*(x1291.value));
16258 sj4=0;
16259 cj4=-1.0;
16260 j4=3.14159265358979;
16261 sj5=gconst16;
16262 cj5=gconst17;
16263 j5=((3.14159265)+(((-1.0)*x1290)));
16264 new_r11=0;
16265 new_r01=0;
16266 new_r22=0;
16267 new_r20=0;
16268 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1290)));
16269 IkReal gconst16=x1289;
16270 IkReal gconst17=((1.0)*new_r10);
16271 j3eval[0]=-1.0;
16272 j3eval[1]=-1.0;
16273 j3eval[2]=((IKabs(((1.0)*new_r00*new_r10)))+(IKabs(((-1.0)+(new_r10*new_r10)))));
16274 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16275 {
16276 {
16277 IkReal j3eval[3];
16278 IkReal x1292=((-1.0)*new_r00);
16279 CheckValue<IkReal> x1294 = IKatan2WithCheck(IkReal(x1292),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16280 if(!x1294.valid){
16281 continue;
16282 }
16283 IkReal x1293=((1.0)*(x1294.value));
16284 sj4=0;
16285 cj4=-1.0;
16286 j4=3.14159265358979;
16287 sj5=gconst16;
16288 cj5=gconst17;
16289 j5=((3.14159265)+(((-1.0)*x1293)));
16290 new_r11=0;
16291 new_r01=0;
16292 new_r22=0;
16293 new_r20=0;
16294 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1293)));
16295 IkReal gconst16=x1292;
16296 IkReal gconst17=((1.0)*new_r10);
16297 j3eval[0]=-1.0;
16298 j3eval[1]=-1.0;
16299 j3eval[2]=((IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
16300 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16301 {
16302 continue; // 3 cases reached
16303 
16304 } else
16305 {
16306 {
16307 IkReal j3array[1], cj3array[1], sj3array[1];
16308 bool j3valid[1]={false};
16309 _nj3 = 1;
16310 IkReal x1295=((1.0)*gconst17);
16311 CheckValue<IkReal> x1296 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1295))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
16312 if(!x1296.valid){
16313 continue;
16314 }
16315 CheckValue<IkReal> x1297=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1295)))),-1);
16316 if(!x1297.valid){
16317 continue;
16318 }
16319 j3array[0]=((-1.5707963267949)+(x1296.value)+(((1.5707963267949)*(x1297.value))));
16320 sj3array[0]=IKsin(j3array[0]);
16321 cj3array[0]=IKcos(j3array[0]);
16322 if( j3array[0] > IKPI )
16323 {
16324  j3array[0]-=IK2PI;
16325 }
16326 else if( j3array[0] < -IKPI )
16327 { j3array[0]+=IK2PI;
16328 }
16329 j3valid[0] = true;
16330 for(int ij3 = 0; ij3 < 1; ++ij3)
16331 {
16332 if( !j3valid[ij3] )
16333 {
16334  continue;
16335 }
16336 _ij3[0] = ij3; _ij3[1] = -1;
16337 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16338 {
16339 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16340 {
16341  j3valid[iij3]=false; _ij3[1] = iij3; break;
16342 }
16343 }
16344 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16345 {
16346 IkReal evalcond[6];
16347 IkReal x1298=IKsin(j3);
16348 IkReal x1299=IKcos(j3);
16349 IkReal x1300=(gconst17*x1298);
16350 IkReal x1301=(gconst16*x1298);
16351 IkReal x1302=(gconst17*x1299);
16352 IkReal x1303=((1.0)*x1299);
16353 IkReal x1304=(gconst16*x1303);
16354 evalcond[0]=(x1300+(((-1.0)*x1304)));
16355 evalcond[1]=(gconst17+((new_r10*x1298))+((new_r00*x1299)));
16356 evalcond[2]=(x1301+x1302+new_r00);
16357 evalcond[3]=((((-1.0)*new_r10*x1303))+gconst16+((new_r00*x1298)));
16358 evalcond[4]=((((-1.0)*x1301))+(((-1.0)*x1302)));
16359 evalcond[5]=(x1300+(((-1.0)*x1304))+new_r10);
16360 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 )
16361 {
16362 continue;
16363 }
16364 }
16365 
16366 {
16367 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16368 vinfos[0].jointtype = 1;
16369 vinfos[0].foffset = j0;
16370 vinfos[0].indices[0] = _ij0[0];
16371 vinfos[0].indices[1] = _ij0[1];
16372 vinfos[0].maxsolutions = _nj0;
16373 vinfos[1].jointtype = 1;
16374 vinfos[1].foffset = j1;
16375 vinfos[1].indices[0] = _ij1[0];
16376 vinfos[1].indices[1] = _ij1[1];
16377 vinfos[1].maxsolutions = _nj1;
16378 vinfos[2].jointtype = 1;
16379 vinfos[2].foffset = j2;
16380 vinfos[2].indices[0] = _ij2[0];
16381 vinfos[2].indices[1] = _ij2[1];
16382 vinfos[2].maxsolutions = _nj2;
16383 vinfos[3].jointtype = 1;
16384 vinfos[3].foffset = j3;
16385 vinfos[3].indices[0] = _ij3[0];
16386 vinfos[3].indices[1] = _ij3[1];
16387 vinfos[3].maxsolutions = _nj3;
16388 vinfos[4].jointtype = 1;
16389 vinfos[4].foffset = j4;
16390 vinfos[4].indices[0] = _ij4[0];
16391 vinfos[4].indices[1] = _ij4[1];
16392 vinfos[4].maxsolutions = _nj4;
16393 vinfos[5].jointtype = 1;
16394 vinfos[5].foffset = j5;
16395 vinfos[5].indices[0] = _ij5[0];
16396 vinfos[5].indices[1] = _ij5[1];
16397 vinfos[5].maxsolutions = _nj5;
16398 std::vector<int> vfree(0);
16399 solutions.AddSolution(vinfos,vfree);
16400 }
16401 }
16402 }
16403 
16404 }
16405 
16406 }
16407 
16408 } else
16409 {
16410 {
16411 IkReal j3array[1], cj3array[1], sj3array[1];
16412 bool j3valid[1]={false};
16413 _nj3 = 1;
16414 CheckValue<IkReal> x1305=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst17*gconst17)))+(((-1.0)*(gconst16*gconst16))))),-1);
16415 if(!x1305.valid){
16416 continue;
16417 }
16418 CheckValue<IkReal> x1306 = IKatan2WithCheck(IkReal((gconst16*new_r00)),IkReal((gconst17*new_r00)),IKFAST_ATAN2_MAGTHRESH);
16419 if(!x1306.valid){
16420 continue;
16421 }
16422 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1305.value)))+(x1306.value));
16423 sj3array[0]=IKsin(j3array[0]);
16424 cj3array[0]=IKcos(j3array[0]);
16425 if( j3array[0] > IKPI )
16426 {
16427  j3array[0]-=IK2PI;
16428 }
16429 else if( j3array[0] < -IKPI )
16430 { j3array[0]+=IK2PI;
16431 }
16432 j3valid[0] = true;
16433 for(int ij3 = 0; ij3 < 1; ++ij3)
16434 {
16435 if( !j3valid[ij3] )
16436 {
16437  continue;
16438 }
16439 _ij3[0] = ij3; _ij3[1] = -1;
16440 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16441 {
16442 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16443 {
16444  j3valid[iij3]=false; _ij3[1] = iij3; break;
16445 }
16446 }
16447 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16448 {
16449 IkReal evalcond[6];
16450 IkReal x1307=IKsin(j3);
16451 IkReal x1308=IKcos(j3);
16452 IkReal x1309=(gconst17*x1307);
16453 IkReal x1310=(gconst16*x1307);
16454 IkReal x1311=(gconst17*x1308);
16455 IkReal x1312=((1.0)*x1308);
16456 IkReal x1313=(gconst16*x1312);
16457 evalcond[0]=(x1309+(((-1.0)*x1313)));
16458 evalcond[1]=(gconst17+((new_r10*x1307))+((new_r00*x1308)));
16459 evalcond[2]=(x1311+x1310+new_r00);
16460 evalcond[3]=((((-1.0)*new_r10*x1312))+gconst16+((new_r00*x1307)));
16461 evalcond[4]=((((-1.0)*x1310))+(((-1.0)*x1311)));
16462 evalcond[5]=(x1309+(((-1.0)*x1313))+new_r10);
16463 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 )
16464 {
16465 continue;
16466 }
16467 }
16468 
16469 {
16470 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16471 vinfos[0].jointtype = 1;
16472 vinfos[0].foffset = j0;
16473 vinfos[0].indices[0] = _ij0[0];
16474 vinfos[0].indices[1] = _ij0[1];
16475 vinfos[0].maxsolutions = _nj0;
16476 vinfos[1].jointtype = 1;
16477 vinfos[1].foffset = j1;
16478 vinfos[1].indices[0] = _ij1[0];
16479 vinfos[1].indices[1] = _ij1[1];
16480 vinfos[1].maxsolutions = _nj1;
16481 vinfos[2].jointtype = 1;
16482 vinfos[2].foffset = j2;
16483 vinfos[2].indices[0] = _ij2[0];
16484 vinfos[2].indices[1] = _ij2[1];
16485 vinfos[2].maxsolutions = _nj2;
16486 vinfos[3].jointtype = 1;
16487 vinfos[3].foffset = j3;
16488 vinfos[3].indices[0] = _ij3[0];
16489 vinfos[3].indices[1] = _ij3[1];
16490 vinfos[3].maxsolutions = _nj3;
16491 vinfos[4].jointtype = 1;
16492 vinfos[4].foffset = j4;
16493 vinfos[4].indices[0] = _ij4[0];
16494 vinfos[4].indices[1] = _ij4[1];
16495 vinfos[4].maxsolutions = _nj4;
16496 vinfos[5].jointtype = 1;
16497 vinfos[5].foffset = j5;
16498 vinfos[5].indices[0] = _ij5[0];
16499 vinfos[5].indices[1] = _ij5[1];
16500 vinfos[5].maxsolutions = _nj5;
16501 std::vector<int> vfree(0);
16502 solutions.AddSolution(vinfos,vfree);
16503 }
16504 }
16505 }
16506 
16507 }
16508 
16509 }
16510 
16511 } else
16512 {
16513 {
16514 IkReal j3array[1], cj3array[1], sj3array[1];
16515 bool j3valid[1]={false};
16516 _nj3 = 1;
16517 CheckValue<IkReal> x1314=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r00))+((gconst17*new_r10)))),-1);
16518 if(!x1314.valid){
16519 continue;
16520 }
16521 CheckValue<IkReal> x1315 = IKatan2WithCheck(IkReal(gconst16*gconst16),IkReal((gconst16*gconst17)),IKFAST_ATAN2_MAGTHRESH);
16522 if(!x1315.valid){
16523 continue;
16524 }
16525 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1314.value)))+(x1315.value));
16526 sj3array[0]=IKsin(j3array[0]);
16527 cj3array[0]=IKcos(j3array[0]);
16528 if( j3array[0] > IKPI )
16529 {
16530  j3array[0]-=IK2PI;
16531 }
16532 else if( j3array[0] < -IKPI )
16533 { j3array[0]+=IK2PI;
16534 }
16535 j3valid[0] = true;
16536 for(int ij3 = 0; ij3 < 1; ++ij3)
16537 {
16538 if( !j3valid[ij3] )
16539 {
16540  continue;
16541 }
16542 _ij3[0] = ij3; _ij3[1] = -1;
16543 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16544 {
16545 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16546 {
16547  j3valid[iij3]=false; _ij3[1] = iij3; break;
16548 }
16549 }
16550 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16551 {
16552 IkReal evalcond[6];
16553 IkReal x1316=IKsin(j3);
16554 IkReal x1317=IKcos(j3);
16555 IkReal x1318=(gconst17*x1316);
16556 IkReal x1319=(gconst16*x1316);
16557 IkReal x1320=(gconst17*x1317);
16558 IkReal x1321=((1.0)*x1317);
16559 IkReal x1322=(gconst16*x1321);
16560 evalcond[0]=(x1318+(((-1.0)*x1322)));
16561 evalcond[1]=(((new_r00*x1317))+gconst17+((new_r10*x1316)));
16562 evalcond[2]=(x1319+x1320+new_r00);
16563 evalcond[3]=(((new_r00*x1316))+(((-1.0)*new_r10*x1321))+gconst16);
16564 evalcond[4]=((((-1.0)*x1319))+(((-1.0)*x1320)));
16565 evalcond[5]=(x1318+(((-1.0)*x1322))+new_r10);
16566 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 )
16567 {
16568 continue;
16569 }
16570 }
16571 
16572 {
16573 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16574 vinfos[0].jointtype = 1;
16575 vinfos[0].foffset = j0;
16576 vinfos[0].indices[0] = _ij0[0];
16577 vinfos[0].indices[1] = _ij0[1];
16578 vinfos[0].maxsolutions = _nj0;
16579 vinfos[1].jointtype = 1;
16580 vinfos[1].foffset = j1;
16581 vinfos[1].indices[0] = _ij1[0];
16582 vinfos[1].indices[1] = _ij1[1];
16583 vinfos[1].maxsolutions = _nj1;
16584 vinfos[2].jointtype = 1;
16585 vinfos[2].foffset = j2;
16586 vinfos[2].indices[0] = _ij2[0];
16587 vinfos[2].indices[1] = _ij2[1];
16588 vinfos[2].maxsolutions = _nj2;
16589 vinfos[3].jointtype = 1;
16590 vinfos[3].foffset = j3;
16591 vinfos[3].indices[0] = _ij3[0];
16592 vinfos[3].indices[1] = _ij3[1];
16593 vinfos[3].maxsolutions = _nj3;
16594 vinfos[4].jointtype = 1;
16595 vinfos[4].foffset = j4;
16596 vinfos[4].indices[0] = _ij4[0];
16597 vinfos[4].indices[1] = _ij4[1];
16598 vinfos[4].maxsolutions = _nj4;
16599 vinfos[5].jointtype = 1;
16600 vinfos[5].foffset = j5;
16601 vinfos[5].indices[0] = _ij5[0];
16602 vinfos[5].indices[1] = _ij5[1];
16603 vinfos[5].maxsolutions = _nj5;
16604 std::vector<int> vfree(0);
16605 solutions.AddSolution(vinfos,vfree);
16606 }
16607 }
16608 }
16609 
16610 }
16611 
16612 }
16613 
16614 }
16615 } while(0);
16616 if( bgotonextstatement )
16617 {
16618 bool bgotonextstatement = true;
16619 do
16620 {
16621 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
16622 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16623 {
16624 bgotonextstatement=false;
16625 {
16626 IkReal j3array[2], cj3array[2], sj3array[2];
16627 bool j3valid[2]={false};
16628 _nj3 = 2;
16629 CheckValue<IkReal> x1323=IKPowWithIntegerCheck(gconst16,-1);
16630 if(!x1323.valid){
16631 continue;
16632 }
16633 sj3array[0]=(new_r11*(x1323.value));
16634 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16635 {
16636  j3valid[0] = j3valid[1] = true;
16637  j3array[0] = IKasin(sj3array[0]);
16638  cj3array[0] = IKcos(j3array[0]);
16639  sj3array[1] = sj3array[0];
16640  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16641  cj3array[1] = -cj3array[0];
16642 }
16643 else if( isnan(sj3array[0]) )
16644 {
16645  // probably any value will work
16646  j3valid[0] = true;
16647  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16648 }
16649 for(int ij3 = 0; ij3 < 2; ++ij3)
16650 {
16651 if( !j3valid[ij3] )
16652 {
16653  continue;
16654 }
16655 _ij3[0] = ij3; _ij3[1] = -1;
16656 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16657 {
16658 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16659 {
16660  j3valid[iij3]=false; _ij3[1] = iij3; break;
16661 }
16662 }
16663 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16664 {
16665 IkReal evalcond[6];
16666 IkReal x1324=IKcos(j3);
16667 IkReal x1325=IKsin(j3);
16668 IkReal x1326=((-1.0)*x1324);
16669 evalcond[0]=(new_r00*x1324);
16670 evalcond[1]=(new_r11*x1326);
16671 evalcond[2]=(gconst16*x1326);
16672 evalcond[3]=(((new_r00*x1325))+gconst16);
16673 evalcond[4]=(((gconst16*x1325))+new_r00);
16674 evalcond[5]=(((new_r11*x1325))+(((-1.0)*gconst16)));
16675 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 )
16676 {
16677 continue;
16678 }
16679 }
16680 
16681 {
16682 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16683 vinfos[0].jointtype = 1;
16684 vinfos[0].foffset = j0;
16685 vinfos[0].indices[0] = _ij0[0];
16686 vinfos[0].indices[1] = _ij0[1];
16687 vinfos[0].maxsolutions = _nj0;
16688 vinfos[1].jointtype = 1;
16689 vinfos[1].foffset = j1;
16690 vinfos[1].indices[0] = _ij1[0];
16691 vinfos[1].indices[1] = _ij1[1];
16692 vinfos[1].maxsolutions = _nj1;
16693 vinfos[2].jointtype = 1;
16694 vinfos[2].foffset = j2;
16695 vinfos[2].indices[0] = _ij2[0];
16696 vinfos[2].indices[1] = _ij2[1];
16697 vinfos[2].maxsolutions = _nj2;
16698 vinfos[3].jointtype = 1;
16699 vinfos[3].foffset = j3;
16700 vinfos[3].indices[0] = _ij3[0];
16701 vinfos[3].indices[1] = _ij3[1];
16702 vinfos[3].maxsolutions = _nj3;
16703 vinfos[4].jointtype = 1;
16704 vinfos[4].foffset = j4;
16705 vinfos[4].indices[0] = _ij4[0];
16706 vinfos[4].indices[1] = _ij4[1];
16707 vinfos[4].maxsolutions = _nj4;
16708 vinfos[5].jointtype = 1;
16709 vinfos[5].foffset = j5;
16710 vinfos[5].indices[0] = _ij5[0];
16711 vinfos[5].indices[1] = _ij5[1];
16712 vinfos[5].maxsolutions = _nj5;
16713 std::vector<int> vfree(0);
16714 solutions.AddSolution(vinfos,vfree);
16715 }
16716 }
16717 }
16718 
16719 }
16720 } while(0);
16721 if( bgotonextstatement )
16722 {
16723 bool bgotonextstatement = true;
16724 do
16725 {
16726 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
16727 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16728 {
16729 bgotonextstatement=false;
16730 {
16731 IkReal j3eval[1];
16732 IkReal x1327=((-1.0)*new_r00);
16733 CheckValue<IkReal> x1329 = IKatan2WithCheck(IkReal(x1327),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16734 if(!x1329.valid){
16735 continue;
16736 }
16737 IkReal x1328=((1.0)*(x1329.value));
16738 sj4=0;
16739 cj4=-1.0;
16740 j4=3.14159265358979;
16741 sj5=gconst16;
16742 cj5=gconst17;
16743 j5=((3.14159265)+(((-1.0)*x1328)));
16744 new_r11=0;
16745 new_r10=0;
16746 new_r22=0;
16747 new_r02=0;
16748 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1328)));
16749 IkReal x1330 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16750 if(IKabs(x1330)==0){
16751 continue;
16752 }
16753 IkReal gconst16=(x1327*(pow(x1330,-0.5)));
16754 IkReal gconst17=0;
16755 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
16756 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16757 {
16758 {
16759 IkReal j3eval[1];
16760 IkReal x1331=((-1.0)*new_r00);
16761 CheckValue<IkReal> x1333 = IKatan2WithCheck(IkReal(x1331),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16762 if(!x1333.valid){
16763 continue;
16764 }
16765 IkReal x1332=((1.0)*(x1333.value));
16766 sj4=0;
16767 cj4=-1.0;
16768 j4=3.14159265358979;
16769 sj5=gconst16;
16770 cj5=gconst17;
16771 j5=((3.14159265)+(((-1.0)*x1332)));
16772 new_r11=0;
16773 new_r10=0;
16774 new_r22=0;
16775 new_r02=0;
16776 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1332)));
16777 IkReal x1334 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16778 if(IKabs(x1334)==0){
16779 continue;
16780 }
16781 IkReal gconst16=(x1331*(pow(x1334,-0.5)));
16782 IkReal gconst17=0;
16783 j3eval[0]=new_r00;
16784 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16785 {
16786 {
16787 IkReal j3eval[2];
16788 IkReal x1335=((-1.0)*new_r00);
16789 CheckValue<IkReal> x1337 = IKatan2WithCheck(IkReal(x1335),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16790 if(!x1337.valid){
16791 continue;
16792 }
16793 IkReal x1336=((1.0)*(x1337.value));
16794 sj4=0;
16795 cj4=-1.0;
16796 j4=3.14159265358979;
16797 sj5=gconst16;
16798 cj5=gconst17;
16799 j5=((3.14159265)+(((-1.0)*x1336)));
16800 new_r11=0;
16801 new_r10=0;
16802 new_r22=0;
16803 new_r02=0;
16804 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1336)));
16805 IkReal x1338 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16806 if(IKabs(x1338)==0){
16807 continue;
16808 }
16809 IkReal gconst16=(x1335*(pow(x1338,-0.5)));
16810 IkReal gconst17=0;
16811 j3eval[0]=new_r00;
16812 j3eval[1]=new_r01;
16813 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
16814 {
16815 continue; // 3 cases reached
16816 
16817 } else
16818 {
16819 {
16820 IkReal j3array[1], cj3array[1], sj3array[1];
16821 bool j3valid[1]={false};
16822 _nj3 = 1;
16823 CheckValue<IkReal> x1339=IKPowWithIntegerCheck(new_r00,-1);
16824 if(!x1339.valid){
16825 continue;
16826 }
16827 CheckValue<IkReal> x1340=IKPowWithIntegerCheck(new_r01,-1);
16828 if(!x1340.valid){
16829 continue;
16830 }
16831 if( IKabs(((-1.0)*gconst16*(x1339.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst16*(x1340.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1339.value)))+IKsqr((gconst16*(x1340.value)))-1) <= IKFAST_SINCOS_THRESH )
16832  continue;
16833 j3array[0]=IKatan2(((-1.0)*gconst16*(x1339.value)), (gconst16*(x1340.value)));
16834 sj3array[0]=IKsin(j3array[0]);
16835 cj3array[0]=IKcos(j3array[0]);
16836 if( j3array[0] > IKPI )
16837 {
16838  j3array[0]-=IK2PI;
16839 }
16840 else if( j3array[0] < -IKPI )
16841 { j3array[0]+=IK2PI;
16842 }
16843 j3valid[0] = true;
16844 for(int ij3 = 0; ij3 < 1; ++ij3)
16845 {
16846 if( !j3valid[ij3] )
16847 {
16848  continue;
16849 }
16850 _ij3[0] = ij3; _ij3[1] = -1;
16851 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16852 {
16853 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16854 {
16855  j3valid[iij3]=false; _ij3[1] = iij3; break;
16856 }
16857 }
16858 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16859 {
16860 IkReal evalcond[8];
16861 IkReal x1341=IKsin(j3);
16862 IkReal x1342=IKcos(j3);
16863 IkReal x1343=((1.0)*gconst16);
16864 IkReal x1344=((-1.0)*gconst16);
16865 evalcond[0]=(new_r01*x1341);
16866 evalcond[1]=(new_r00*x1342);
16867 evalcond[2]=(x1341*x1344);
16868 evalcond[3]=(x1342*x1344);
16869 evalcond[4]=(gconst16+((new_r00*x1341)));
16870 evalcond[5]=(((gconst16*x1341))+new_r00);
16871 evalcond[6]=((((-1.0)*x1342*x1343))+new_r01);
16872 evalcond[7]=((((-1.0)*x1343))+((new_r01*x1342)));
16873 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 )
16874 {
16875 continue;
16876 }
16877 }
16878 
16879 {
16880 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16881 vinfos[0].jointtype = 1;
16882 vinfos[0].foffset = j0;
16883 vinfos[0].indices[0] = _ij0[0];
16884 vinfos[0].indices[1] = _ij0[1];
16885 vinfos[0].maxsolutions = _nj0;
16886 vinfos[1].jointtype = 1;
16887 vinfos[1].foffset = j1;
16888 vinfos[1].indices[0] = _ij1[0];
16889 vinfos[1].indices[1] = _ij1[1];
16890 vinfos[1].maxsolutions = _nj1;
16891 vinfos[2].jointtype = 1;
16892 vinfos[2].foffset = j2;
16893 vinfos[2].indices[0] = _ij2[0];
16894 vinfos[2].indices[1] = _ij2[1];
16895 vinfos[2].maxsolutions = _nj2;
16896 vinfos[3].jointtype = 1;
16897 vinfos[3].foffset = j3;
16898 vinfos[3].indices[0] = _ij3[0];
16899 vinfos[3].indices[1] = _ij3[1];
16900 vinfos[3].maxsolutions = _nj3;
16901 vinfos[4].jointtype = 1;
16902 vinfos[4].foffset = j4;
16903 vinfos[4].indices[0] = _ij4[0];
16904 vinfos[4].indices[1] = _ij4[1];
16905 vinfos[4].maxsolutions = _nj4;
16906 vinfos[5].jointtype = 1;
16907 vinfos[5].foffset = j5;
16908 vinfos[5].indices[0] = _ij5[0];
16909 vinfos[5].indices[1] = _ij5[1];
16910 vinfos[5].maxsolutions = _nj5;
16911 std::vector<int> vfree(0);
16912 solutions.AddSolution(vinfos,vfree);
16913 }
16914 }
16915 }
16916 
16917 }
16918 
16919 }
16920 
16921 } else
16922 {
16923 {
16924 IkReal j3array[1], cj3array[1], sj3array[1];
16925 bool j3valid[1]={false};
16926 _nj3 = 1;
16927 CheckValue<IkReal> x1345=IKPowWithIntegerCheck(new_r00,-1);
16928 if(!x1345.valid){
16929 continue;
16930 }
16931 CheckValue<IkReal> x1346=IKPowWithIntegerCheck(gconst16,-1);
16932 if(!x1346.valid){
16933 continue;
16934 }
16935 if( IKabs(((-1.0)*gconst16*(x1345.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1346.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1345.value)))+IKsqr((new_r01*(x1346.value)))-1) <= IKFAST_SINCOS_THRESH )
16936  continue;
16937 j3array[0]=IKatan2(((-1.0)*gconst16*(x1345.value)), (new_r01*(x1346.value)));
16938 sj3array[0]=IKsin(j3array[0]);
16939 cj3array[0]=IKcos(j3array[0]);
16940 if( j3array[0] > IKPI )
16941 {
16942  j3array[0]-=IK2PI;
16943 }
16944 else if( j3array[0] < -IKPI )
16945 { j3array[0]+=IK2PI;
16946 }
16947 j3valid[0] = true;
16948 for(int ij3 = 0; ij3 < 1; ++ij3)
16949 {
16950 if( !j3valid[ij3] )
16951 {
16952  continue;
16953 }
16954 _ij3[0] = ij3; _ij3[1] = -1;
16955 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16956 {
16957 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16958 {
16959  j3valid[iij3]=false; _ij3[1] = iij3; break;
16960 }
16961 }
16962 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16963 {
16964 IkReal evalcond[8];
16965 IkReal x1347=IKsin(j3);
16966 IkReal x1348=IKcos(j3);
16967 IkReal x1349=((1.0)*gconst16);
16968 IkReal x1350=((-1.0)*gconst16);
16969 evalcond[0]=(new_r01*x1347);
16970 evalcond[1]=(new_r00*x1348);
16971 evalcond[2]=(x1347*x1350);
16972 evalcond[3]=(x1348*x1350);
16973 evalcond[4]=(gconst16+((new_r00*x1347)));
16974 evalcond[5]=(((gconst16*x1347))+new_r00);
16975 evalcond[6]=((((-1.0)*x1348*x1349))+new_r01);
16976 evalcond[7]=((((-1.0)*x1349))+((new_r01*x1348)));
16977 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 )
16978 {
16979 continue;
16980 }
16981 }
16982 
16983 {
16984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16985 vinfos[0].jointtype = 1;
16986 vinfos[0].foffset = j0;
16987 vinfos[0].indices[0] = _ij0[0];
16988 vinfos[0].indices[1] = _ij0[1];
16989 vinfos[0].maxsolutions = _nj0;
16990 vinfos[1].jointtype = 1;
16991 vinfos[1].foffset = j1;
16992 vinfos[1].indices[0] = _ij1[0];
16993 vinfos[1].indices[1] = _ij1[1];
16994 vinfos[1].maxsolutions = _nj1;
16995 vinfos[2].jointtype = 1;
16996 vinfos[2].foffset = j2;
16997 vinfos[2].indices[0] = _ij2[0];
16998 vinfos[2].indices[1] = _ij2[1];
16999 vinfos[2].maxsolutions = _nj2;
17000 vinfos[3].jointtype = 1;
17001 vinfos[3].foffset = j3;
17002 vinfos[3].indices[0] = _ij3[0];
17003 vinfos[3].indices[1] = _ij3[1];
17004 vinfos[3].maxsolutions = _nj3;
17005 vinfos[4].jointtype = 1;
17006 vinfos[4].foffset = j4;
17007 vinfos[4].indices[0] = _ij4[0];
17008 vinfos[4].indices[1] = _ij4[1];
17009 vinfos[4].maxsolutions = _nj4;
17010 vinfos[5].jointtype = 1;
17011 vinfos[5].foffset = j5;
17012 vinfos[5].indices[0] = _ij5[0];
17013 vinfos[5].indices[1] = _ij5[1];
17014 vinfos[5].maxsolutions = _nj5;
17015 std::vector<int> vfree(0);
17016 solutions.AddSolution(vinfos,vfree);
17017 }
17018 }
17019 }
17020 
17021 }
17022 
17023 }
17024 
17025 } else
17026 {
17027 {
17028 IkReal j3array[1], cj3array[1], sj3array[1];
17029 bool j3valid[1]={false};
17030 _nj3 = 1;
17031 CheckValue<IkReal> x1351 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17032 if(!x1351.valid){
17033 continue;
17034 }
17035 CheckValue<IkReal> x1352=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17036 if(!x1352.valid){
17037 continue;
17038 }
17039 j3array[0]=((-1.5707963267949)+(x1351.value)+(((1.5707963267949)*(x1352.value))));
17040 sj3array[0]=IKsin(j3array[0]);
17041 cj3array[0]=IKcos(j3array[0]);
17042 if( j3array[0] > IKPI )
17043 {
17044  j3array[0]-=IK2PI;
17045 }
17046 else if( j3array[0] < -IKPI )
17047 { j3array[0]+=IK2PI;
17048 }
17049 j3valid[0] = true;
17050 for(int ij3 = 0; ij3 < 1; ++ij3)
17051 {
17052 if( !j3valid[ij3] )
17053 {
17054  continue;
17055 }
17056 _ij3[0] = ij3; _ij3[1] = -1;
17057 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17058 {
17059 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17060 {
17061  j3valid[iij3]=false; _ij3[1] = iij3; break;
17062 }
17063 }
17064 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17065 {
17066 IkReal evalcond[8];
17067 IkReal x1353=IKsin(j3);
17068 IkReal x1354=IKcos(j3);
17069 IkReal x1355=((1.0)*gconst16);
17070 IkReal x1356=((-1.0)*gconst16);
17071 evalcond[0]=(new_r01*x1353);
17072 evalcond[1]=(new_r00*x1354);
17073 evalcond[2]=(x1353*x1356);
17074 evalcond[3]=(x1354*x1356);
17075 evalcond[4]=(gconst16+((new_r00*x1353)));
17076 evalcond[5]=(new_r00+((gconst16*x1353)));
17077 evalcond[6]=((((-1.0)*x1354*x1355))+new_r01);
17078 evalcond[7]=((((-1.0)*x1355))+((new_r01*x1354)));
17079 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 )
17080 {
17081 continue;
17082 }
17083 }
17084 
17085 {
17086 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17087 vinfos[0].jointtype = 1;
17088 vinfos[0].foffset = j0;
17089 vinfos[0].indices[0] = _ij0[0];
17090 vinfos[0].indices[1] = _ij0[1];
17091 vinfos[0].maxsolutions = _nj0;
17092 vinfos[1].jointtype = 1;
17093 vinfos[1].foffset = j1;
17094 vinfos[1].indices[0] = _ij1[0];
17095 vinfos[1].indices[1] = _ij1[1];
17096 vinfos[1].maxsolutions = _nj1;
17097 vinfos[2].jointtype = 1;
17098 vinfos[2].foffset = j2;
17099 vinfos[2].indices[0] = _ij2[0];
17100 vinfos[2].indices[1] = _ij2[1];
17101 vinfos[2].maxsolutions = _nj2;
17102 vinfos[3].jointtype = 1;
17103 vinfos[3].foffset = j3;
17104 vinfos[3].indices[0] = _ij3[0];
17105 vinfos[3].indices[1] = _ij3[1];
17106 vinfos[3].maxsolutions = _nj3;
17107 vinfos[4].jointtype = 1;
17108 vinfos[4].foffset = j4;
17109 vinfos[4].indices[0] = _ij4[0];
17110 vinfos[4].indices[1] = _ij4[1];
17111 vinfos[4].maxsolutions = _nj4;
17112 vinfos[5].jointtype = 1;
17113 vinfos[5].foffset = j5;
17114 vinfos[5].indices[0] = _ij5[0];
17115 vinfos[5].indices[1] = _ij5[1];
17116 vinfos[5].maxsolutions = _nj5;
17117 std::vector<int> vfree(0);
17118 solutions.AddSolution(vinfos,vfree);
17119 }
17120 }
17121 }
17122 
17123 }
17124 
17125 }
17126 
17127 }
17128 } while(0);
17129 if( bgotonextstatement )
17130 {
17131 bool bgotonextstatement = true;
17132 do
17133 {
17134 evalcond[0]=IKabs(new_r10);
17135 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17136 {
17137 bgotonextstatement=false;
17138 {
17139 IkReal j3eval[1];
17140 IkReal x1357=((-1.0)*new_r00);
17141 CheckValue<IkReal> x1359 = IKatan2WithCheck(IkReal(x1357),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17142 if(!x1359.valid){
17143 continue;
17144 }
17145 IkReal x1358=((1.0)*(x1359.value));
17146 sj4=0;
17147 cj4=-1.0;
17148 j4=3.14159265358979;
17149 sj5=gconst16;
17150 cj5=gconst17;
17151 j5=((3.14159265)+(((-1.0)*x1358)));
17152 new_r10=0;
17153 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1358)));
17154 IkReal x1360 = new_r00*new_r00;
17155 if(IKabs(x1360)==0){
17156 continue;
17157 }
17158 IkReal gconst16=(x1357*(pow(x1360,-0.5)));
17159 IkReal gconst17=0;
17160 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
17161 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17162 {
17163 {
17164 IkReal j3eval[1];
17165 IkReal x1361=((-1.0)*new_r00);
17166 CheckValue<IkReal> x1363 = IKatan2WithCheck(IkReal(x1361),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17167 if(!x1363.valid){
17168 continue;
17169 }
17170 IkReal x1362=((1.0)*(x1363.value));
17171 sj4=0;
17172 cj4=-1.0;
17173 j4=3.14159265358979;
17174 sj5=gconst16;
17175 cj5=gconst17;
17176 j5=((3.14159265)+(((-1.0)*x1362)));
17177 new_r10=0;
17178 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1362)));
17179 IkReal x1364 = new_r00*new_r00;
17180 if(IKabs(x1364)==0){
17181 continue;
17182 }
17183 IkReal gconst16=(x1361*(pow(x1364,-0.5)));
17184 IkReal gconst17=0;
17185 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
17186 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17187 {
17188 {
17189 IkReal j3eval[1];
17190 IkReal x1365=((-1.0)*new_r00);
17191 CheckValue<IkReal> x1367 = IKatan2WithCheck(IkReal(x1365),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17192 if(!x1367.valid){
17193 continue;
17194 }
17195 IkReal x1366=((1.0)*(x1367.value));
17196 sj4=0;
17197 cj4=-1.0;
17198 j4=3.14159265358979;
17199 sj5=gconst16;
17200 cj5=gconst17;
17201 j5=((3.14159265)+(((-1.0)*x1366)));
17202 new_r10=0;
17203 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1366)));
17204 IkReal x1368 = new_r00*new_r00;
17205 if(IKabs(x1368)==0){
17206 continue;
17207 }
17208 IkReal gconst16=(x1365*(pow(x1368,-0.5)));
17209 IkReal gconst17=0;
17210 j3eval[0]=new_r00;
17211 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17212 {
17213 continue; // 3 cases reached
17214 
17215 } else
17216 {
17217 {
17218 IkReal j3array[1], cj3array[1], sj3array[1];
17219 bool j3valid[1]={false};
17220 _nj3 = 1;
17221 CheckValue<IkReal> x1369=IKPowWithIntegerCheck(new_r00,-1);
17222 if(!x1369.valid){
17223 continue;
17224 }
17225 CheckValue<IkReal> x1370=IKPowWithIntegerCheck(gconst16,-1);
17226 if(!x1370.valid){
17227 continue;
17228 }
17229 if( IKabs(((-1.0)*gconst16*(x1369.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1370.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1369.value)))+IKsqr((new_r01*(x1370.value)))-1) <= IKFAST_SINCOS_THRESH )
17230  continue;
17231 j3array[0]=IKatan2(((-1.0)*gconst16*(x1369.value)), (new_r01*(x1370.value)));
17232 sj3array[0]=IKsin(j3array[0]);
17233 cj3array[0]=IKcos(j3array[0]);
17234 if( j3array[0] > IKPI )
17235 {
17236  j3array[0]-=IK2PI;
17237 }
17238 else if( j3array[0] < -IKPI )
17239 { j3array[0]+=IK2PI;
17240 }
17241 j3valid[0] = true;
17242 for(int ij3 = 0; ij3 < 1; ++ij3)
17243 {
17244 if( !j3valid[ij3] )
17245 {
17246  continue;
17247 }
17248 _ij3[0] = ij3; _ij3[1] = -1;
17249 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17250 {
17251 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17252 {
17253  j3valid[iij3]=false; _ij3[1] = iij3; break;
17254 }
17255 }
17256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17257 {
17258 IkReal evalcond[8];
17259 IkReal x1371=IKcos(j3);
17260 IkReal x1372=IKsin(j3);
17261 IkReal x1373=((1.0)*gconst16);
17262 evalcond[0]=(new_r00*x1371);
17263 evalcond[1]=((-1.0)*gconst16*x1371);
17264 evalcond[2]=(gconst16+((new_r00*x1372)));
17265 evalcond[3]=(new_r00+((gconst16*x1372)));
17266 evalcond[4]=((((-1.0)*x1371*x1373))+new_r01);
17267 evalcond[5]=((((-1.0)*x1372*x1373))+new_r11);
17268 evalcond[6]=(((new_r01*x1372))+(((-1.0)*new_r11*x1371)));
17269 evalcond[7]=((((-1.0)*x1373))+((new_r11*x1372))+((new_r01*x1371)));
17270 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 )
17271 {
17272 continue;
17273 }
17274 }
17275 
17276 {
17277 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17278 vinfos[0].jointtype = 1;
17279 vinfos[0].foffset = j0;
17280 vinfos[0].indices[0] = _ij0[0];
17281 vinfos[0].indices[1] = _ij0[1];
17282 vinfos[0].maxsolutions = _nj0;
17283 vinfos[1].jointtype = 1;
17284 vinfos[1].foffset = j1;
17285 vinfos[1].indices[0] = _ij1[0];
17286 vinfos[1].indices[1] = _ij1[1];
17287 vinfos[1].maxsolutions = _nj1;
17288 vinfos[2].jointtype = 1;
17289 vinfos[2].foffset = j2;
17290 vinfos[2].indices[0] = _ij2[0];
17291 vinfos[2].indices[1] = _ij2[1];
17292 vinfos[2].maxsolutions = _nj2;
17293 vinfos[3].jointtype = 1;
17294 vinfos[3].foffset = j3;
17295 vinfos[3].indices[0] = _ij3[0];
17296 vinfos[3].indices[1] = _ij3[1];
17297 vinfos[3].maxsolutions = _nj3;
17298 vinfos[4].jointtype = 1;
17299 vinfos[4].foffset = j4;
17300 vinfos[4].indices[0] = _ij4[0];
17301 vinfos[4].indices[1] = _ij4[1];
17302 vinfos[4].maxsolutions = _nj4;
17303 vinfos[5].jointtype = 1;
17304 vinfos[5].foffset = j5;
17305 vinfos[5].indices[0] = _ij5[0];
17306 vinfos[5].indices[1] = _ij5[1];
17307 vinfos[5].maxsolutions = _nj5;
17308 std::vector<int> vfree(0);
17309 solutions.AddSolution(vinfos,vfree);
17310 }
17311 }
17312 }
17313 
17314 }
17315 
17316 }
17317 
17318 } else
17319 {
17320 {
17321 IkReal j3array[1], cj3array[1], sj3array[1];
17322 bool j3valid[1]={false};
17323 _nj3 = 1;
17324 CheckValue<IkReal> x1374 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17325 if(!x1374.valid){
17326 continue;
17327 }
17328 CheckValue<IkReal> x1375=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17329 if(!x1375.valid){
17330 continue;
17331 }
17332 j3array[0]=((-1.5707963267949)+(x1374.value)+(((1.5707963267949)*(x1375.value))));
17333 sj3array[0]=IKsin(j3array[0]);
17334 cj3array[0]=IKcos(j3array[0]);
17335 if( j3array[0] > IKPI )
17336 {
17337  j3array[0]-=IK2PI;
17338 }
17339 else if( j3array[0] < -IKPI )
17340 { j3array[0]+=IK2PI;
17341 }
17342 j3valid[0] = true;
17343 for(int ij3 = 0; ij3 < 1; ++ij3)
17344 {
17345 if( !j3valid[ij3] )
17346 {
17347  continue;
17348 }
17349 _ij3[0] = ij3; _ij3[1] = -1;
17350 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17351 {
17352 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17353 {
17354  j3valid[iij3]=false; _ij3[1] = iij3; break;
17355 }
17356 }
17357 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17358 {
17359 IkReal evalcond[8];
17360 IkReal x1376=IKcos(j3);
17361 IkReal x1377=IKsin(j3);
17362 IkReal x1378=((1.0)*gconst16);
17363 evalcond[0]=(new_r00*x1376);
17364 evalcond[1]=((-1.0)*gconst16*x1376);
17365 evalcond[2]=(gconst16+((new_r00*x1377)));
17366 evalcond[3]=(new_r00+((gconst16*x1377)));
17367 evalcond[4]=((((-1.0)*x1376*x1378))+new_r01);
17368 evalcond[5]=((((-1.0)*x1377*x1378))+new_r11);
17369 evalcond[6]=(((new_r01*x1377))+(((-1.0)*new_r11*x1376)));
17370 evalcond[7]=((((-1.0)*x1378))+((new_r11*x1377))+((new_r01*x1376)));
17371 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 )
17372 {
17373 continue;
17374 }
17375 }
17376 
17377 {
17378 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17379 vinfos[0].jointtype = 1;
17380 vinfos[0].foffset = j0;
17381 vinfos[0].indices[0] = _ij0[0];
17382 vinfos[0].indices[1] = _ij0[1];
17383 vinfos[0].maxsolutions = _nj0;
17384 vinfos[1].jointtype = 1;
17385 vinfos[1].foffset = j1;
17386 vinfos[1].indices[0] = _ij1[0];
17387 vinfos[1].indices[1] = _ij1[1];
17388 vinfos[1].maxsolutions = _nj1;
17389 vinfos[2].jointtype = 1;
17390 vinfos[2].foffset = j2;
17391 vinfos[2].indices[0] = _ij2[0];
17392 vinfos[2].indices[1] = _ij2[1];
17393 vinfos[2].maxsolutions = _nj2;
17394 vinfos[3].jointtype = 1;
17395 vinfos[3].foffset = j3;
17396 vinfos[3].indices[0] = _ij3[0];
17397 vinfos[3].indices[1] = _ij3[1];
17398 vinfos[3].maxsolutions = _nj3;
17399 vinfos[4].jointtype = 1;
17400 vinfos[4].foffset = j4;
17401 vinfos[4].indices[0] = _ij4[0];
17402 vinfos[4].indices[1] = _ij4[1];
17403 vinfos[4].maxsolutions = _nj4;
17404 vinfos[5].jointtype = 1;
17405 vinfos[5].foffset = j5;
17406 vinfos[5].indices[0] = _ij5[0];
17407 vinfos[5].indices[1] = _ij5[1];
17408 vinfos[5].maxsolutions = _nj5;
17409 std::vector<int> vfree(0);
17410 solutions.AddSolution(vinfos,vfree);
17411 }
17412 }
17413 }
17414 
17415 }
17416 
17417 }
17418 
17419 } else
17420 {
17421 {
17422 IkReal j3array[1], cj3array[1], sj3array[1];
17423 bool j3valid[1]={false};
17424 _nj3 = 1;
17425 CheckValue<IkReal> x1379=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17426 if(!x1379.valid){
17427 continue;
17428 }
17429 CheckValue<IkReal> x1380 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17430 if(!x1380.valid){
17431 continue;
17432 }
17433 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1379.value)))+(x1380.value));
17434 sj3array[0]=IKsin(j3array[0]);
17435 cj3array[0]=IKcos(j3array[0]);
17436 if( j3array[0] > IKPI )
17437 {
17438  j3array[0]-=IK2PI;
17439 }
17440 else if( j3array[0] < -IKPI )
17441 { j3array[0]+=IK2PI;
17442 }
17443 j3valid[0] = true;
17444 for(int ij3 = 0; ij3 < 1; ++ij3)
17445 {
17446 if( !j3valid[ij3] )
17447 {
17448  continue;
17449 }
17450 _ij3[0] = ij3; _ij3[1] = -1;
17451 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17452 {
17453 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17454 {
17455  j3valid[iij3]=false; _ij3[1] = iij3; break;
17456 }
17457 }
17458 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17459 {
17460 IkReal evalcond[8];
17461 IkReal x1381=IKcos(j3);
17462 IkReal x1382=IKsin(j3);
17463 IkReal x1383=((1.0)*gconst16);
17464 evalcond[0]=(new_r00*x1381);
17465 evalcond[1]=((-1.0)*gconst16*x1381);
17466 evalcond[2]=(gconst16+((new_r00*x1382)));
17467 evalcond[3]=(((gconst16*x1382))+new_r00);
17468 evalcond[4]=((((-1.0)*x1381*x1383))+new_r01);
17469 evalcond[5]=((((-1.0)*x1382*x1383))+new_r11);
17470 evalcond[6]=((((-1.0)*new_r11*x1381))+((new_r01*x1382)));
17471 evalcond[7]=((((-1.0)*x1383))+((new_r11*x1382))+((new_r01*x1381)));
17472 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 )
17473 {
17474 continue;
17475 }
17476 }
17477 
17478 {
17479 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17480 vinfos[0].jointtype = 1;
17481 vinfos[0].foffset = j0;
17482 vinfos[0].indices[0] = _ij0[0];
17483 vinfos[0].indices[1] = _ij0[1];
17484 vinfos[0].maxsolutions = _nj0;
17485 vinfos[1].jointtype = 1;
17486 vinfos[1].foffset = j1;
17487 vinfos[1].indices[0] = _ij1[0];
17488 vinfos[1].indices[1] = _ij1[1];
17489 vinfos[1].maxsolutions = _nj1;
17490 vinfos[2].jointtype = 1;
17491 vinfos[2].foffset = j2;
17492 vinfos[2].indices[0] = _ij2[0];
17493 vinfos[2].indices[1] = _ij2[1];
17494 vinfos[2].maxsolutions = _nj2;
17495 vinfos[3].jointtype = 1;
17496 vinfos[3].foffset = j3;
17497 vinfos[3].indices[0] = _ij3[0];
17498 vinfos[3].indices[1] = _ij3[1];
17499 vinfos[3].maxsolutions = _nj3;
17500 vinfos[4].jointtype = 1;
17501 vinfos[4].foffset = j4;
17502 vinfos[4].indices[0] = _ij4[0];
17503 vinfos[4].indices[1] = _ij4[1];
17504 vinfos[4].maxsolutions = _nj4;
17505 vinfos[5].jointtype = 1;
17506 vinfos[5].foffset = j5;
17507 vinfos[5].indices[0] = _ij5[0];
17508 vinfos[5].indices[1] = _ij5[1];
17509 vinfos[5].maxsolutions = _nj5;
17510 std::vector<int> vfree(0);
17511 solutions.AddSolution(vinfos,vfree);
17512 }
17513 }
17514 }
17515 
17516 }
17517 
17518 }
17519 
17520 }
17521 } while(0);
17522 if( bgotonextstatement )
17523 {
17524 bool bgotonextstatement = true;
17525 do
17526 {
17527 if( 1 )
17528 {
17529 bgotonextstatement=false;
17530 continue; // branch miss [j3]
17531 
17532 }
17533 } while(0);
17534 if( bgotonextstatement )
17535 {
17536 }
17537 }
17538 }
17539 }
17540 }
17541 }
17542 }
17543 
17544 } else
17545 {
17546 {
17547 IkReal j3array[1], cj3array[1], sj3array[1];
17548 bool j3valid[1]={false};
17549 _nj3 = 1;
17550 IkReal x1384=((1.0)*gconst17);
17551 CheckValue<IkReal> x1385=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1384)))),-1);
17552 if(!x1385.valid){
17553 continue;
17554 }
17555 CheckValue<IkReal> x1386 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst16*x1384)))),IKFAST_ATAN2_MAGTHRESH);
17556 if(!x1386.valid){
17557 continue;
17558 }
17559 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.value)))+(x1386.value));
17560 sj3array[0]=IKsin(j3array[0]);
17561 cj3array[0]=IKcos(j3array[0]);
17562 if( j3array[0] > IKPI )
17563 {
17564  j3array[0]-=IK2PI;
17565 }
17566 else if( j3array[0] < -IKPI )
17567 { j3array[0]+=IK2PI;
17568 }
17569 j3valid[0] = true;
17570 for(int ij3 = 0; ij3 < 1; ++ij3)
17571 {
17572 if( !j3valid[ij3] )
17573 {
17574  continue;
17575 }
17576 _ij3[0] = ij3; _ij3[1] = -1;
17577 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17578 {
17579 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17580 {
17581  j3valid[iij3]=false; _ij3[1] = iij3; break;
17582 }
17583 }
17584 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17585 {
17586 IkReal evalcond[8];
17587 IkReal x1387=IKsin(j3);
17588 IkReal x1388=IKcos(j3);
17589 IkReal x1389=((1.0)*gconst16);
17590 IkReal x1390=(gconst17*x1387);
17591 IkReal x1391=(gconst17*x1388);
17592 IkReal x1392=((1.0)*x1388);
17593 IkReal x1393=(x1388*x1389);
17594 evalcond[0]=(((new_r10*x1387))+gconst17+((new_r00*x1388)));
17595 evalcond[1]=(x1391+((gconst16*x1387))+new_r00);
17596 evalcond[2]=(gconst16+((new_r00*x1387))+(((-1.0)*new_r10*x1392)));
17597 evalcond[3]=(gconst17+((new_r01*x1387))+(((-1.0)*new_r11*x1392)));
17598 evalcond[4]=(x1390+new_r01+(((-1.0)*x1393)));
17599 evalcond[5]=(x1390+new_r10+(((-1.0)*x1393)));
17600 evalcond[6]=((((-1.0)*x1389))+((new_r11*x1387))+((new_r01*x1388)));
17601 evalcond[7]=((((-1.0)*x1391))+new_r11+(((-1.0)*x1387*x1389)));
17602 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 )
17603 {
17604 continue;
17605 }
17606 }
17607 
17608 {
17609 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17610 vinfos[0].jointtype = 1;
17611 vinfos[0].foffset = j0;
17612 vinfos[0].indices[0] = _ij0[0];
17613 vinfos[0].indices[1] = _ij0[1];
17614 vinfos[0].maxsolutions = _nj0;
17615 vinfos[1].jointtype = 1;
17616 vinfos[1].foffset = j1;
17617 vinfos[1].indices[0] = _ij1[0];
17618 vinfos[1].indices[1] = _ij1[1];
17619 vinfos[1].maxsolutions = _nj1;
17620 vinfos[2].jointtype = 1;
17621 vinfos[2].foffset = j2;
17622 vinfos[2].indices[0] = _ij2[0];
17623 vinfos[2].indices[1] = _ij2[1];
17624 vinfos[2].maxsolutions = _nj2;
17625 vinfos[3].jointtype = 1;
17626 vinfos[3].foffset = j3;
17627 vinfos[3].indices[0] = _ij3[0];
17628 vinfos[3].indices[1] = _ij3[1];
17629 vinfos[3].maxsolutions = _nj3;
17630 vinfos[4].jointtype = 1;
17631 vinfos[4].foffset = j4;
17632 vinfos[4].indices[0] = _ij4[0];
17633 vinfos[4].indices[1] = _ij4[1];
17634 vinfos[4].maxsolutions = _nj4;
17635 vinfos[5].jointtype = 1;
17636 vinfos[5].foffset = j5;
17637 vinfos[5].indices[0] = _ij5[0];
17638 vinfos[5].indices[1] = _ij5[1];
17639 vinfos[5].maxsolutions = _nj5;
17640 std::vector<int> vfree(0);
17641 solutions.AddSolution(vinfos,vfree);
17642 }
17643 }
17644 }
17645 
17646 }
17647 
17648 }
17649 
17650 } else
17651 {
17652 {
17653 IkReal j3array[1], cj3array[1], sj3array[1];
17654 bool j3valid[1]={false};
17655 _nj3 = 1;
17656 IkReal x1394=((1.0)*gconst17);
17657 CheckValue<IkReal> x1395=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r11))+(((-1.0)*new_r01*x1394)))),-1);
17658 if(!x1395.valid){
17659 continue;
17660 }
17661 CheckValue<IkReal> x1396 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1394))+((new_r00*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17662 if(!x1396.valid){
17663 continue;
17664 }
17665 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1395.value)))+(x1396.value));
17666 sj3array[0]=IKsin(j3array[0]);
17667 cj3array[0]=IKcos(j3array[0]);
17668 if( j3array[0] > IKPI )
17669 {
17670  j3array[0]-=IK2PI;
17671 }
17672 else if( j3array[0] < -IKPI )
17673 { j3array[0]+=IK2PI;
17674 }
17675 j3valid[0] = true;
17676 for(int ij3 = 0; ij3 < 1; ++ij3)
17677 {
17678 if( !j3valid[ij3] )
17679 {
17680  continue;
17681 }
17682 _ij3[0] = ij3; _ij3[1] = -1;
17683 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17684 {
17685 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17686 {
17687  j3valid[iij3]=false; _ij3[1] = iij3; break;
17688 }
17689 }
17690 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17691 {
17692 IkReal evalcond[8];
17693 IkReal x1397=IKsin(j3);
17694 IkReal x1398=IKcos(j3);
17695 IkReal x1399=((1.0)*gconst16);
17696 IkReal x1400=(gconst17*x1397);
17697 IkReal x1401=(gconst17*x1398);
17698 IkReal x1402=((1.0)*x1398);
17699 IkReal x1403=(x1398*x1399);
17700 evalcond[0]=(((new_r10*x1397))+gconst17+((new_r00*x1398)));
17701 evalcond[1]=(x1401+new_r00+((gconst16*x1397)));
17702 evalcond[2]=(gconst16+((new_r00*x1397))+(((-1.0)*new_r10*x1402)));
17703 evalcond[3]=(gconst17+((new_r01*x1397))+(((-1.0)*new_r11*x1402)));
17704 evalcond[4]=((((-1.0)*x1403))+x1400+new_r01);
17705 evalcond[5]=((((-1.0)*x1403))+x1400+new_r10);
17706 evalcond[6]=(((new_r11*x1397))+((new_r01*x1398))+(((-1.0)*x1399)));
17707 evalcond[7]=((((-1.0)*x1401))+(((-1.0)*x1397*x1399))+new_r11);
17708 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 )
17709 {
17710 continue;
17711 }
17712 }
17713 
17714 {
17715 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17716 vinfos[0].jointtype = 1;
17717 vinfos[0].foffset = j0;
17718 vinfos[0].indices[0] = _ij0[0];
17719 vinfos[0].indices[1] = _ij0[1];
17720 vinfos[0].maxsolutions = _nj0;
17721 vinfos[1].jointtype = 1;
17722 vinfos[1].foffset = j1;
17723 vinfos[1].indices[0] = _ij1[0];
17724 vinfos[1].indices[1] = _ij1[1];
17725 vinfos[1].maxsolutions = _nj1;
17726 vinfos[2].jointtype = 1;
17727 vinfos[2].foffset = j2;
17728 vinfos[2].indices[0] = _ij2[0];
17729 vinfos[2].indices[1] = _ij2[1];
17730 vinfos[2].maxsolutions = _nj2;
17731 vinfos[3].jointtype = 1;
17732 vinfos[3].foffset = j3;
17733 vinfos[3].indices[0] = _ij3[0];
17734 vinfos[3].indices[1] = _ij3[1];
17735 vinfos[3].maxsolutions = _nj3;
17736 vinfos[4].jointtype = 1;
17737 vinfos[4].foffset = j4;
17738 vinfos[4].indices[0] = _ij4[0];
17739 vinfos[4].indices[1] = _ij4[1];
17740 vinfos[4].maxsolutions = _nj4;
17741 vinfos[5].jointtype = 1;
17742 vinfos[5].foffset = j5;
17743 vinfos[5].indices[0] = _ij5[0];
17744 vinfos[5].indices[1] = _ij5[1];
17745 vinfos[5].maxsolutions = _nj5;
17746 std::vector<int> vfree(0);
17747 solutions.AddSolution(vinfos,vfree);
17748 }
17749 }
17750 }
17751 
17752 }
17753 
17754 }
17755 
17756 } else
17757 {
17758 {
17759 IkReal j3array[1], cj3array[1], sj3array[1];
17760 bool j3valid[1]={false};
17761 _nj3 = 1;
17762 IkReal x1404=((1.0)*new_r10);
17763 CheckValue<IkReal> x1405 = IKatan2WithCheck(IkReal((((gconst17*new_r11))+((gconst17*new_r00)))),IkReal(((((-1.0)*gconst17*x1404))+((gconst17*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17764 if(!x1405.valid){
17765 continue;
17766 }
17767 CheckValue<IkReal> x1406=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1404)))),-1);
17768 if(!x1406.valid){
17769 continue;
17770 }
17771 j3array[0]=((-1.5707963267949)+(x1405.value)+(((1.5707963267949)*(x1406.value))));
17772 sj3array[0]=IKsin(j3array[0]);
17773 cj3array[0]=IKcos(j3array[0]);
17774 if( j3array[0] > IKPI )
17775 {
17776  j3array[0]-=IK2PI;
17777 }
17778 else if( j3array[0] < -IKPI )
17779 { j3array[0]+=IK2PI;
17780 }
17781 j3valid[0] = true;
17782 for(int ij3 = 0; ij3 < 1; ++ij3)
17783 {
17784 if( !j3valid[ij3] )
17785 {
17786  continue;
17787 }
17788 _ij3[0] = ij3; _ij3[1] = -1;
17789 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17790 {
17791 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17792 {
17793  j3valid[iij3]=false; _ij3[1] = iij3; break;
17794 }
17795 }
17796 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17797 {
17798 IkReal evalcond[8];
17799 IkReal x1407=IKsin(j3);
17800 IkReal x1408=IKcos(j3);
17801 IkReal x1409=((1.0)*gconst16);
17802 IkReal x1410=(gconst17*x1407);
17803 IkReal x1411=(gconst17*x1408);
17804 IkReal x1412=((1.0)*x1408);
17805 IkReal x1413=(x1408*x1409);
17806 evalcond[0]=(gconst17+((new_r10*x1407))+((new_r00*x1408)));
17807 evalcond[1]=(((gconst16*x1407))+x1411+new_r00);
17808 evalcond[2]=((((-1.0)*new_r10*x1412))+gconst16+((new_r00*x1407)));
17809 evalcond[3]=((((-1.0)*new_r11*x1412))+((new_r01*x1407))+gconst17);
17810 evalcond[4]=((((-1.0)*x1413))+x1410+new_r01);
17811 evalcond[5]=((((-1.0)*x1413))+x1410+new_r10);
17812 evalcond[6]=(((new_r01*x1408))+((new_r11*x1407))+(((-1.0)*x1409)));
17813 evalcond[7]=((((-1.0)*x1407*x1409))+new_r11+(((-1.0)*x1411)));
17814 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 )
17815 {
17816 continue;
17817 }
17818 }
17819 
17820 {
17821 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17822 vinfos[0].jointtype = 1;
17823 vinfos[0].foffset = j0;
17824 vinfos[0].indices[0] = _ij0[0];
17825 vinfos[0].indices[1] = _ij0[1];
17826 vinfos[0].maxsolutions = _nj0;
17827 vinfos[1].jointtype = 1;
17828 vinfos[1].foffset = j1;
17829 vinfos[1].indices[0] = _ij1[0];
17830 vinfos[1].indices[1] = _ij1[1];
17831 vinfos[1].maxsolutions = _nj1;
17832 vinfos[2].jointtype = 1;
17833 vinfos[2].foffset = j2;
17834 vinfos[2].indices[0] = _ij2[0];
17835 vinfos[2].indices[1] = _ij2[1];
17836 vinfos[2].maxsolutions = _nj2;
17837 vinfos[3].jointtype = 1;
17838 vinfos[3].foffset = j3;
17839 vinfos[3].indices[0] = _ij3[0];
17840 vinfos[3].indices[1] = _ij3[1];
17841 vinfos[3].maxsolutions = _nj3;
17842 vinfos[4].jointtype = 1;
17843 vinfos[4].foffset = j4;
17844 vinfos[4].indices[0] = _ij4[0];
17845 vinfos[4].indices[1] = _ij4[1];
17846 vinfos[4].maxsolutions = _nj4;
17847 vinfos[5].jointtype = 1;
17848 vinfos[5].foffset = j5;
17849 vinfos[5].indices[0] = _ij5[0];
17850 vinfos[5].indices[1] = _ij5[1];
17851 vinfos[5].maxsolutions = _nj5;
17852 std::vector<int> vfree(0);
17853 solutions.AddSolution(vinfos,vfree);
17854 }
17855 }
17856 }
17857 
17858 }
17859 
17860 }
17861 
17862 }
17863 } while(0);
17864 if( bgotonextstatement )
17865 {
17866 bool bgotonextstatement = true;
17867 do
17868 {
17869 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17870 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17871 {
17872 bgotonextstatement=false;
17873 {
17874 IkReal j3array[1], cj3array[1], sj3array[1];
17875 bool j3valid[1]={false};
17876 _nj3 = 1;
17877 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
17878  continue;
17879 j3array[0]=IKatan2(((-1.0)*new_r00), new_r01);
17880 sj3array[0]=IKsin(j3array[0]);
17881 cj3array[0]=IKcos(j3array[0]);
17882 if( j3array[0] > IKPI )
17883 {
17884  j3array[0]-=IK2PI;
17885 }
17886 else if( j3array[0] < -IKPI )
17887 { j3array[0]+=IK2PI;
17888 }
17889 j3valid[0] = true;
17890 for(int ij3 = 0; ij3 < 1; ++ij3)
17891 {
17892 if( !j3valid[ij3] )
17893 {
17894  continue;
17895 }
17896 _ij3[0] = ij3; _ij3[1] = -1;
17897 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17898 {
17899 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17900 {
17901  j3valid[iij3]=false; _ij3[1] = iij3; break;
17902 }
17903 }
17904 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17905 {
17906 IkReal evalcond[8];
17907 IkReal x1414=IKsin(j3);
17908 IkReal x1415=IKcos(j3);
17909 IkReal x1416=((1.0)*x1415);
17910 evalcond[0]=(x1414+new_r00);
17911 evalcond[1]=((((-1.0)*x1416))+new_r01);
17912 evalcond[2]=(new_r11+(((-1.0)*x1414)));
17913 evalcond[3]=((((-1.0)*x1416))+new_r10);
17914 evalcond[4]=(((new_r00*x1415))+((new_r10*x1414)));
17915 evalcond[5]=((((-1.0)*new_r11*x1416))+((new_r01*x1414)));
17916 evalcond[6]=((-1.0)+((new_r01*x1415))+((new_r11*x1414)));
17917 evalcond[7]=((1.0)+(((-1.0)*new_r10*x1416))+((new_r00*x1414)));
17918 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 )
17919 {
17920 continue;
17921 }
17922 }
17923 
17924 {
17925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17926 vinfos[0].jointtype = 1;
17927 vinfos[0].foffset = j0;
17928 vinfos[0].indices[0] = _ij0[0];
17929 vinfos[0].indices[1] = _ij0[1];
17930 vinfos[0].maxsolutions = _nj0;
17931 vinfos[1].jointtype = 1;
17932 vinfos[1].foffset = j1;
17933 vinfos[1].indices[0] = _ij1[0];
17934 vinfos[1].indices[1] = _ij1[1];
17935 vinfos[1].maxsolutions = _nj1;
17936 vinfos[2].jointtype = 1;
17937 vinfos[2].foffset = j2;
17938 vinfos[2].indices[0] = _ij2[0];
17939 vinfos[2].indices[1] = _ij2[1];
17940 vinfos[2].maxsolutions = _nj2;
17941 vinfos[3].jointtype = 1;
17942 vinfos[3].foffset = j3;
17943 vinfos[3].indices[0] = _ij3[0];
17944 vinfos[3].indices[1] = _ij3[1];
17945 vinfos[3].maxsolutions = _nj3;
17946 vinfos[4].jointtype = 1;
17947 vinfos[4].foffset = j4;
17948 vinfos[4].indices[0] = _ij4[0];
17949 vinfos[4].indices[1] = _ij4[1];
17950 vinfos[4].maxsolutions = _nj4;
17951 vinfos[5].jointtype = 1;
17952 vinfos[5].foffset = j5;
17953 vinfos[5].indices[0] = _ij5[0];
17954 vinfos[5].indices[1] = _ij5[1];
17955 vinfos[5].maxsolutions = _nj5;
17956 std::vector<int> vfree(0);
17957 solutions.AddSolution(vinfos,vfree);
17958 }
17959 }
17960 }
17961 
17962 }
17963 } while(0);
17964 if( bgotonextstatement )
17965 {
17966 bool bgotonextstatement = true;
17967 do
17968 {
17969 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17970 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17971 {
17972 bgotonextstatement=false;
17973 {
17974 IkReal j3array[1], cj3array[1], sj3array[1];
17975 bool j3valid[1]={false};
17976 _nj3 = 1;
17977 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
17978  continue;
17979 j3array[0]=IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17980 sj3array[0]=IKsin(j3array[0]);
17981 cj3array[0]=IKcos(j3array[0]);
17982 if( j3array[0] > IKPI )
17983 {
17984  j3array[0]-=IK2PI;
17985 }
17986 else if( j3array[0] < -IKPI )
17987 { j3array[0]+=IK2PI;
17988 }
17989 j3valid[0] = true;
17990 for(int ij3 = 0; ij3 < 1; ++ij3)
17991 {
17992 if( !j3valid[ij3] )
17993 {
17994  continue;
17995 }
17996 _ij3[0] = ij3; _ij3[1] = -1;
17997 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17998 {
17999 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18000 {
18001  j3valid[iij3]=false; _ij3[1] = iij3; break;
18002 }
18003 }
18004 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18005 {
18006 IkReal evalcond[8];
18007 IkReal x1417=IKcos(j3);
18008 IkReal x1418=IKsin(j3);
18009 IkReal x1419=((1.0)*x1417);
18010 evalcond[0]=(x1417+new_r01);
18011 evalcond[1]=(x1418+new_r11);
18012 evalcond[2]=(x1417+new_r10);
18013 evalcond[3]=(new_r00+(((-1.0)*x1418)));
18014 evalcond[4]=(((new_r00*x1417))+((new_r10*x1418)));
18015 evalcond[5]=((((-1.0)*new_r11*x1419))+((new_r01*x1418)));
18016 evalcond[6]=((1.0)+((new_r01*x1417))+((new_r11*x1418)));
18017 evalcond[7]=((-1.0)+(((-1.0)*new_r10*x1419))+((new_r00*x1418)));
18018 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 )
18019 {
18020 continue;
18021 }
18022 }
18023 
18024 {
18025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18026 vinfos[0].jointtype = 1;
18027 vinfos[0].foffset = j0;
18028 vinfos[0].indices[0] = _ij0[0];
18029 vinfos[0].indices[1] = _ij0[1];
18030 vinfos[0].maxsolutions = _nj0;
18031 vinfos[1].jointtype = 1;
18032 vinfos[1].foffset = j1;
18033 vinfos[1].indices[0] = _ij1[0];
18034 vinfos[1].indices[1] = _ij1[1];
18035 vinfos[1].maxsolutions = _nj1;
18036 vinfos[2].jointtype = 1;
18037 vinfos[2].foffset = j2;
18038 vinfos[2].indices[0] = _ij2[0];
18039 vinfos[2].indices[1] = _ij2[1];
18040 vinfos[2].maxsolutions = _nj2;
18041 vinfos[3].jointtype = 1;
18042 vinfos[3].foffset = j3;
18043 vinfos[3].indices[0] = _ij3[0];
18044 vinfos[3].indices[1] = _ij3[1];
18045 vinfos[3].maxsolutions = _nj3;
18046 vinfos[4].jointtype = 1;
18047 vinfos[4].foffset = j4;
18048 vinfos[4].indices[0] = _ij4[0];
18049 vinfos[4].indices[1] = _ij4[1];
18050 vinfos[4].maxsolutions = _nj4;
18051 vinfos[5].jointtype = 1;
18052 vinfos[5].foffset = j5;
18053 vinfos[5].indices[0] = _ij5[0];
18054 vinfos[5].indices[1] = _ij5[1];
18055 vinfos[5].maxsolutions = _nj5;
18056 std::vector<int> vfree(0);
18057 solutions.AddSolution(vinfos,vfree);
18058 }
18059 }
18060 }
18061 
18062 }
18063 } while(0);
18064 if( bgotonextstatement )
18065 {
18066 bool bgotonextstatement = true;
18067 do
18068 {
18069 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
18070 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18071 {
18072 bgotonextstatement=false;
18073 {
18074 IkReal j3eval[3];
18075 sj4=0;
18076 cj4=-1.0;
18077 j4=3.14159265358979;
18078 new_r11=0;
18079 new_r00=0;
18080 j3eval[0]=new_r10;
18081 j3eval[1]=IKsign(new_r10);
18082 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18083 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18084 {
18085 {
18086 IkReal j3eval[3];
18087 sj4=0;
18088 cj4=-1.0;
18089 j4=3.14159265358979;
18090 new_r11=0;
18091 new_r00=0;
18092 j3eval[0]=new_r01;
18093 j3eval[1]=IKsign(new_r01);
18094 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18095 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18096 {
18097 {
18098 IkReal j3eval[2];
18099 sj4=0;
18100 cj4=-1.0;
18101 j4=3.14159265358979;
18102 new_r11=0;
18103 new_r00=0;
18104 j3eval[0]=new_r01;
18105 j3eval[1]=new_r10;
18106 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18107 {
18108 continue; // no branches [j3]
18109 
18110 } else
18111 {
18112 {
18113 IkReal j3array[1], cj3array[1], sj3array[1];
18114 bool j3valid[1]={false};
18115 _nj3 = 1;
18116 CheckValue<IkReal> x1420=IKPowWithIntegerCheck(new_r01,-1);
18117 if(!x1420.valid){
18118 continue;
18119 }
18120 CheckValue<IkReal> x1421=IKPowWithIntegerCheck(new_r10,-1);
18121 if(!x1421.valid){
18122 continue;
18123 }
18124 if( IKabs(((-1.0)*cj5*(x1420.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x1421.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x1420.value)))+IKsqr((sj5*(x1421.value)))-1) <= IKFAST_SINCOS_THRESH )
18125  continue;
18126 j3array[0]=IKatan2(((-1.0)*cj5*(x1420.value)), (sj5*(x1421.value)));
18127 sj3array[0]=IKsin(j3array[0]);
18128 cj3array[0]=IKcos(j3array[0]);
18129 if( j3array[0] > IKPI )
18130 {
18131  j3array[0]-=IK2PI;
18132 }
18133 else if( j3array[0] < -IKPI )
18134 { j3array[0]+=IK2PI;
18135 }
18136 j3valid[0] = true;
18137 for(int ij3 = 0; ij3 < 1; ++ij3)
18138 {
18139 if( !j3valid[ij3] )
18140 {
18141  continue;
18142 }
18143 _ij3[0] = ij3; _ij3[1] = -1;
18144 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18145 {
18146 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18147 {
18148  j3valid[iij3]=false; _ij3[1] = iij3; break;
18149 }
18150 }
18151 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18152 {
18153 IkReal evalcond[7];
18154 IkReal x1422=IKsin(j3);
18155 IkReal x1423=IKcos(j3);
18156 IkReal x1424=((1.0)*sj5);
18157 IkReal x1425=(cj5*x1422);
18158 IkReal x1426=(x1423*x1424);
18159 evalcond[0]=(cj5+((new_r01*x1422)));
18160 evalcond[1]=(cj5+((new_r10*x1422)));
18161 evalcond[2]=(sj5+(((-1.0)*new_r10*x1423)));
18162 evalcond[3]=(((new_r01*x1423))+(((-1.0)*x1424)));
18163 evalcond[4]=(((sj5*x1422))+((cj5*x1423)));
18164 evalcond[5]=(x1425+new_r01+(((-1.0)*x1426)));
18165 evalcond[6]=(x1425+new_r10+(((-1.0)*x1426)));
18166 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 )
18167 {
18168 continue;
18169 }
18170 }
18171 
18172 {
18173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18174 vinfos[0].jointtype = 1;
18175 vinfos[0].foffset = j0;
18176 vinfos[0].indices[0] = _ij0[0];
18177 vinfos[0].indices[1] = _ij0[1];
18178 vinfos[0].maxsolutions = _nj0;
18179 vinfos[1].jointtype = 1;
18180 vinfos[1].foffset = j1;
18181 vinfos[1].indices[0] = _ij1[0];
18182 vinfos[1].indices[1] = _ij1[1];
18183 vinfos[1].maxsolutions = _nj1;
18184 vinfos[2].jointtype = 1;
18185 vinfos[2].foffset = j2;
18186 vinfos[2].indices[0] = _ij2[0];
18187 vinfos[2].indices[1] = _ij2[1];
18188 vinfos[2].maxsolutions = _nj2;
18189 vinfos[3].jointtype = 1;
18190 vinfos[3].foffset = j3;
18191 vinfos[3].indices[0] = _ij3[0];
18192 vinfos[3].indices[1] = _ij3[1];
18193 vinfos[3].maxsolutions = _nj3;
18194 vinfos[4].jointtype = 1;
18195 vinfos[4].foffset = j4;
18196 vinfos[4].indices[0] = _ij4[0];
18197 vinfos[4].indices[1] = _ij4[1];
18198 vinfos[4].maxsolutions = _nj4;
18199 vinfos[5].jointtype = 1;
18200 vinfos[5].foffset = j5;
18201 vinfos[5].indices[0] = _ij5[0];
18202 vinfos[5].indices[1] = _ij5[1];
18203 vinfos[5].maxsolutions = _nj5;
18204 std::vector<int> vfree(0);
18205 solutions.AddSolution(vinfos,vfree);
18206 }
18207 }
18208 }
18209 
18210 }
18211 
18212 }
18213 
18214 } else
18215 {
18216 {
18217 IkReal j3array[1], cj3array[1], sj3array[1];
18218 bool j3valid[1]={false};
18219 _nj3 = 1;
18220 CheckValue<IkReal> x1427=IKPowWithIntegerCheck(IKsign(new_r01),-1);
18221 if(!x1427.valid){
18222 continue;
18223 }
18224 CheckValue<IkReal> x1428 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18225 if(!x1428.valid){
18226 continue;
18227 }
18228 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1427.value)))+(x1428.value));
18229 sj3array[0]=IKsin(j3array[0]);
18230 cj3array[0]=IKcos(j3array[0]);
18231 if( j3array[0] > IKPI )
18232 {
18233  j3array[0]-=IK2PI;
18234 }
18235 else if( j3array[0] < -IKPI )
18236 { j3array[0]+=IK2PI;
18237 }
18238 j3valid[0] = true;
18239 for(int ij3 = 0; ij3 < 1; ++ij3)
18240 {
18241 if( !j3valid[ij3] )
18242 {
18243  continue;
18244 }
18245 _ij3[0] = ij3; _ij3[1] = -1;
18246 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18247 {
18248 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18249 {
18250  j3valid[iij3]=false; _ij3[1] = iij3; break;
18251 }
18252 }
18253 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18254 {
18255 IkReal evalcond[7];
18256 IkReal x1429=IKsin(j3);
18257 IkReal x1430=IKcos(j3);
18258 IkReal x1431=((1.0)*sj5);
18259 IkReal x1432=(cj5*x1429);
18260 IkReal x1433=(x1430*x1431);
18261 evalcond[0]=(cj5+((new_r01*x1429)));
18262 evalcond[1]=(cj5+((new_r10*x1429)));
18263 evalcond[2]=(sj5+(((-1.0)*new_r10*x1430)));
18264 evalcond[3]=(((new_r01*x1430))+(((-1.0)*x1431)));
18265 evalcond[4]=(((cj5*x1430))+((sj5*x1429)));
18266 evalcond[5]=((((-1.0)*x1433))+x1432+new_r01);
18267 evalcond[6]=((((-1.0)*x1433))+x1432+new_r10);
18268 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 )
18269 {
18270 continue;
18271 }
18272 }
18273 
18274 {
18275 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18276 vinfos[0].jointtype = 1;
18277 vinfos[0].foffset = j0;
18278 vinfos[0].indices[0] = _ij0[0];
18279 vinfos[0].indices[1] = _ij0[1];
18280 vinfos[0].maxsolutions = _nj0;
18281 vinfos[1].jointtype = 1;
18282 vinfos[1].foffset = j1;
18283 vinfos[1].indices[0] = _ij1[0];
18284 vinfos[1].indices[1] = _ij1[1];
18285 vinfos[1].maxsolutions = _nj1;
18286 vinfos[2].jointtype = 1;
18287 vinfos[2].foffset = j2;
18288 vinfos[2].indices[0] = _ij2[0];
18289 vinfos[2].indices[1] = _ij2[1];
18290 vinfos[2].maxsolutions = _nj2;
18291 vinfos[3].jointtype = 1;
18292 vinfos[3].foffset = j3;
18293 vinfos[3].indices[0] = _ij3[0];
18294 vinfos[3].indices[1] = _ij3[1];
18295 vinfos[3].maxsolutions = _nj3;
18296 vinfos[4].jointtype = 1;
18297 vinfos[4].foffset = j4;
18298 vinfos[4].indices[0] = _ij4[0];
18299 vinfos[4].indices[1] = _ij4[1];
18300 vinfos[4].maxsolutions = _nj4;
18301 vinfos[5].jointtype = 1;
18302 vinfos[5].foffset = j5;
18303 vinfos[5].indices[0] = _ij5[0];
18304 vinfos[5].indices[1] = _ij5[1];
18305 vinfos[5].maxsolutions = _nj5;
18306 std::vector<int> vfree(0);
18307 solutions.AddSolution(vinfos,vfree);
18308 }
18309 }
18310 }
18311 
18312 }
18313 
18314 }
18315 
18316 } else
18317 {
18318 {
18319 IkReal j3array[1], cj3array[1], sj3array[1];
18320 bool j3valid[1]={false};
18321 _nj3 = 1;
18322 CheckValue<IkReal> x1434=IKPowWithIntegerCheck(IKsign(new_r10),-1);
18323 if(!x1434.valid){
18324 continue;
18325 }
18326 CheckValue<IkReal> x1435 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18327 if(!x1435.valid){
18328 continue;
18329 }
18330 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1434.value)))+(x1435.value));
18331 sj3array[0]=IKsin(j3array[0]);
18332 cj3array[0]=IKcos(j3array[0]);
18333 if( j3array[0] > IKPI )
18334 {
18335  j3array[0]-=IK2PI;
18336 }
18337 else if( j3array[0] < -IKPI )
18338 { j3array[0]+=IK2PI;
18339 }
18340 j3valid[0] = true;
18341 for(int ij3 = 0; ij3 < 1; ++ij3)
18342 {
18343 if( !j3valid[ij3] )
18344 {
18345  continue;
18346 }
18347 _ij3[0] = ij3; _ij3[1] = -1;
18348 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18349 {
18350 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18351 {
18352  j3valid[iij3]=false; _ij3[1] = iij3; break;
18353 }
18354 }
18355 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18356 {
18357 IkReal evalcond[7];
18358 IkReal x1436=IKsin(j3);
18359 IkReal x1437=IKcos(j3);
18360 IkReal x1438=((1.0)*sj5);
18361 IkReal x1439=(cj5*x1436);
18362 IkReal x1440=(x1437*x1438);
18363 evalcond[0]=(cj5+((new_r01*x1436)));
18364 evalcond[1]=(cj5+((new_r10*x1436)));
18365 evalcond[2]=(sj5+(((-1.0)*new_r10*x1437)));
18366 evalcond[3]=(((new_r01*x1437))+(((-1.0)*x1438)));
18367 evalcond[4]=(((sj5*x1436))+((cj5*x1437)));
18368 evalcond[5]=(x1439+(((-1.0)*x1440))+new_r01);
18369 evalcond[6]=(x1439+(((-1.0)*x1440))+new_r10);
18370 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 )
18371 {
18372 continue;
18373 }
18374 }
18375 
18376 {
18377 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18378 vinfos[0].jointtype = 1;
18379 vinfos[0].foffset = j0;
18380 vinfos[0].indices[0] = _ij0[0];
18381 vinfos[0].indices[1] = _ij0[1];
18382 vinfos[0].maxsolutions = _nj0;
18383 vinfos[1].jointtype = 1;
18384 vinfos[1].foffset = j1;
18385 vinfos[1].indices[0] = _ij1[0];
18386 vinfos[1].indices[1] = _ij1[1];
18387 vinfos[1].maxsolutions = _nj1;
18388 vinfos[2].jointtype = 1;
18389 vinfos[2].foffset = j2;
18390 vinfos[2].indices[0] = _ij2[0];
18391 vinfos[2].indices[1] = _ij2[1];
18392 vinfos[2].maxsolutions = _nj2;
18393 vinfos[3].jointtype = 1;
18394 vinfos[3].foffset = j3;
18395 vinfos[3].indices[0] = _ij3[0];
18396 vinfos[3].indices[1] = _ij3[1];
18397 vinfos[3].maxsolutions = _nj3;
18398 vinfos[4].jointtype = 1;
18399 vinfos[4].foffset = j4;
18400 vinfos[4].indices[0] = _ij4[0];
18401 vinfos[4].indices[1] = _ij4[1];
18402 vinfos[4].maxsolutions = _nj4;
18403 vinfos[5].jointtype = 1;
18404 vinfos[5].foffset = j5;
18405 vinfos[5].indices[0] = _ij5[0];
18406 vinfos[5].indices[1] = _ij5[1];
18407 vinfos[5].maxsolutions = _nj5;
18408 std::vector<int> vfree(0);
18409 solutions.AddSolution(vinfos,vfree);
18410 }
18411 }
18412 }
18413 
18414 }
18415 
18416 }
18417 
18418 }
18419 } while(0);
18420 if( bgotonextstatement )
18421 {
18422 bool bgotonextstatement = true;
18423 do
18424 {
18425 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18426 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18427 {
18428 bgotonextstatement=false;
18429 {
18430 IkReal j3eval[1];
18431 sj4=0;
18432 cj4=-1.0;
18433 j4=3.14159265358979;
18434 new_r11=0;
18435 new_r01=0;
18436 new_r22=0;
18437 new_r20=0;
18438 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18439 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18440 {
18441 continue; // no branches [j3]
18442 
18443 } else
18444 {
18445 {
18446 IkReal j3array[2], cj3array[2], sj3array[2];
18447 bool j3valid[2]={false};
18448 _nj3 = 2;
18449 CheckValue<IkReal> x1442 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
18450 if(!x1442.valid){
18451 continue;
18452 }
18453 IkReal x1441=x1442.value;
18454 j3array[0]=((-1.0)*x1441);
18455 sj3array[0]=IKsin(j3array[0]);
18456 cj3array[0]=IKcos(j3array[0]);
18457 j3array[1]=((3.14159265358979)+(((-1.0)*x1441)));
18458 sj3array[1]=IKsin(j3array[1]);
18459 cj3array[1]=IKcos(j3array[1]);
18460 if( j3array[0] > IKPI )
18461 {
18462  j3array[0]-=IK2PI;
18463 }
18464 else if( j3array[0] < -IKPI )
18465 { j3array[0]+=IK2PI;
18466 }
18467 j3valid[0] = true;
18468 if( j3array[1] > IKPI )
18469 {
18470  j3array[1]-=IK2PI;
18471 }
18472 else if( j3array[1] < -IKPI )
18473 { j3array[1]+=IK2PI;
18474 }
18475 j3valid[1] = true;
18476 for(int ij3 = 0; ij3 < 2; ++ij3)
18477 {
18478 if( !j3valid[ij3] )
18479 {
18480  continue;
18481 }
18482 _ij3[0] = ij3; _ij3[1] = -1;
18483 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18484 {
18485 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18486 {
18487  j3valid[iij3]=false; _ij3[1] = iij3; break;
18488 }
18489 }
18490 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18491 {
18492 IkReal evalcond[1];
18493 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
18494 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18495 {
18496 continue;
18497 }
18498 }
18499 
18500 {
18501 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18502 vinfos[0].jointtype = 1;
18503 vinfos[0].foffset = j0;
18504 vinfos[0].indices[0] = _ij0[0];
18505 vinfos[0].indices[1] = _ij0[1];
18506 vinfos[0].maxsolutions = _nj0;
18507 vinfos[1].jointtype = 1;
18508 vinfos[1].foffset = j1;
18509 vinfos[1].indices[0] = _ij1[0];
18510 vinfos[1].indices[1] = _ij1[1];
18511 vinfos[1].maxsolutions = _nj1;
18512 vinfos[2].jointtype = 1;
18513 vinfos[2].foffset = j2;
18514 vinfos[2].indices[0] = _ij2[0];
18515 vinfos[2].indices[1] = _ij2[1];
18516 vinfos[2].maxsolutions = _nj2;
18517 vinfos[3].jointtype = 1;
18518 vinfos[3].foffset = j3;
18519 vinfos[3].indices[0] = _ij3[0];
18520 vinfos[3].indices[1] = _ij3[1];
18521 vinfos[3].maxsolutions = _nj3;
18522 vinfos[4].jointtype = 1;
18523 vinfos[4].foffset = j4;
18524 vinfos[4].indices[0] = _ij4[0];
18525 vinfos[4].indices[1] = _ij4[1];
18526 vinfos[4].maxsolutions = _nj4;
18527 vinfos[5].jointtype = 1;
18528 vinfos[5].foffset = j5;
18529 vinfos[5].indices[0] = _ij5[0];
18530 vinfos[5].indices[1] = _ij5[1];
18531 vinfos[5].maxsolutions = _nj5;
18532 std::vector<int> vfree(0);
18533 solutions.AddSolution(vinfos,vfree);
18534 }
18535 }
18536 }
18537 
18538 }
18539 
18540 }
18541 
18542 }
18543 } while(0);
18544 if( bgotonextstatement )
18545 {
18546 bool bgotonextstatement = true;
18547 do
18548 {
18549 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18550 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18551 {
18552 bgotonextstatement=false;
18553 {
18554 IkReal j3eval[1];
18555 sj4=0;
18556 cj4=-1.0;
18557 j4=3.14159265358979;
18558 new_r00=0;
18559 new_r10=0;
18560 new_r21=0;
18561 new_r22=0;
18562 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18563 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18564 {
18565 continue; // no branches [j3]
18566 
18567 } else
18568 {
18569 {
18570 IkReal j3array[2], cj3array[2], sj3array[2];
18571 bool j3valid[2]={false};
18572 _nj3 = 2;
18573 CheckValue<IkReal> x1444 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
18574 if(!x1444.valid){
18575 continue;
18576 }
18577 IkReal x1443=x1444.value;
18578 j3array[0]=((-1.0)*x1443);
18579 sj3array[0]=IKsin(j3array[0]);
18580 cj3array[0]=IKcos(j3array[0]);
18581 j3array[1]=((3.14159265358979)+(((-1.0)*x1443)));
18582 sj3array[1]=IKsin(j3array[1]);
18583 cj3array[1]=IKcos(j3array[1]);
18584 if( j3array[0] > IKPI )
18585 {
18586  j3array[0]-=IK2PI;
18587 }
18588 else if( j3array[0] < -IKPI )
18589 { j3array[0]+=IK2PI;
18590 }
18591 j3valid[0] = true;
18592 if( j3array[1] > IKPI )
18593 {
18594  j3array[1]-=IK2PI;
18595 }
18596 else if( j3array[1] < -IKPI )
18597 { j3array[1]+=IK2PI;
18598 }
18599 j3valid[1] = true;
18600 for(int ij3 = 0; ij3 < 2; ++ij3)
18601 {
18602 if( !j3valid[ij3] )
18603 {
18604  continue;
18605 }
18606 _ij3[0] = ij3; _ij3[1] = -1;
18607 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18608 {
18609 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18610 {
18611  j3valid[iij3]=false; _ij3[1] = iij3; break;
18612 }
18613 }
18614 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18615 {
18616 IkReal evalcond[1];
18617 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
18618 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18619 {
18620 continue;
18621 }
18622 }
18623 
18624 {
18625 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18626 vinfos[0].jointtype = 1;
18627 vinfos[0].foffset = j0;
18628 vinfos[0].indices[0] = _ij0[0];
18629 vinfos[0].indices[1] = _ij0[1];
18630 vinfos[0].maxsolutions = _nj0;
18631 vinfos[1].jointtype = 1;
18632 vinfos[1].foffset = j1;
18633 vinfos[1].indices[0] = _ij1[0];
18634 vinfos[1].indices[1] = _ij1[1];
18635 vinfos[1].maxsolutions = _nj1;
18636 vinfos[2].jointtype = 1;
18637 vinfos[2].foffset = j2;
18638 vinfos[2].indices[0] = _ij2[0];
18639 vinfos[2].indices[1] = _ij2[1];
18640 vinfos[2].maxsolutions = _nj2;
18641 vinfos[3].jointtype = 1;
18642 vinfos[3].foffset = j3;
18643 vinfos[3].indices[0] = _ij3[0];
18644 vinfos[3].indices[1] = _ij3[1];
18645 vinfos[3].maxsolutions = _nj3;
18646 vinfos[4].jointtype = 1;
18647 vinfos[4].foffset = j4;
18648 vinfos[4].indices[0] = _ij4[0];
18649 vinfos[4].indices[1] = _ij4[1];
18650 vinfos[4].maxsolutions = _nj4;
18651 vinfos[5].jointtype = 1;
18652 vinfos[5].foffset = j5;
18653 vinfos[5].indices[0] = _ij5[0];
18654 vinfos[5].indices[1] = _ij5[1];
18655 vinfos[5].maxsolutions = _nj5;
18656 std::vector<int> vfree(0);
18657 solutions.AddSolution(vinfos,vfree);
18658 }
18659 }
18660 }
18661 
18662 }
18663 
18664 }
18665 
18666 }
18667 } while(0);
18668 if( bgotonextstatement )
18669 {
18670 bool bgotonextstatement = true;
18671 do
18672 {
18673 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
18674 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18675 {
18676 bgotonextstatement=false;
18677 {
18678 IkReal j3eval[3];
18679 sj4=0;
18680 cj4=-1.0;
18681 j4=3.14159265358979;
18682 new_r01=0;
18683 new_r10=0;
18684 j3eval[0]=new_r00;
18685 j3eval[1]=IKsign(new_r00);
18686 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18687 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18688 {
18689 {
18690 IkReal j3eval[2];
18691 sj4=0;
18692 cj4=-1.0;
18693 j4=3.14159265358979;
18694 new_r01=0;
18695 new_r10=0;
18696 j3eval[0]=new_r00;
18697 j3eval[1]=new_r11;
18698 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18699 {
18700 continue; // no branches [j3]
18701 
18702 } else
18703 {
18704 {
18705 IkReal j3array[1], cj3array[1], sj3array[1];
18706 bool j3valid[1]={false};
18707 _nj3 = 1;
18708 CheckValue<IkReal> x1445=IKPowWithIntegerCheck(new_r00,-1);
18709 if(!x1445.valid){
18710 continue;
18711 }
18712 CheckValue<IkReal> x1446=IKPowWithIntegerCheck(new_r11,-1);
18713 if(!x1446.valid){
18714 continue;
18715 }
18716 if( IKabs(((-1.0)*sj5*(x1445.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x1446.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x1445.value)))+IKsqr((cj5*(x1446.value)))-1) <= IKFAST_SINCOS_THRESH )
18717  continue;
18718 j3array[0]=IKatan2(((-1.0)*sj5*(x1445.value)), (cj5*(x1446.value)));
18719 sj3array[0]=IKsin(j3array[0]);
18720 cj3array[0]=IKcos(j3array[0]);
18721 if( j3array[0] > IKPI )
18722 {
18723  j3array[0]-=IK2PI;
18724 }
18725 else if( j3array[0] < -IKPI )
18726 { j3array[0]+=IK2PI;
18727 }
18728 j3valid[0] = true;
18729 for(int ij3 = 0; ij3 < 1; ++ij3)
18730 {
18731 if( !j3valid[ij3] )
18732 {
18733  continue;
18734 }
18735 _ij3[0] = ij3; _ij3[1] = -1;
18736 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18737 {
18738 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18739 {
18740  j3valid[iij3]=false; _ij3[1] = iij3; break;
18741 }
18742 }
18743 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18744 {
18745 IkReal evalcond[7];
18746 IkReal x1447=IKcos(j3);
18747 IkReal x1448=IKsin(j3);
18748 IkReal x1449=((1.0)*sj5);
18749 IkReal x1450=(cj5*x1447);
18750 IkReal x1451=((1.0)*x1447);
18751 evalcond[0]=(sj5+((new_r00*x1448)));
18752 evalcond[1]=(cj5+((new_r00*x1447)));
18753 evalcond[2]=(cj5+(((-1.0)*new_r11*x1451)));
18754 evalcond[3]=(((new_r11*x1448))+(((-1.0)*x1449)));
18755 evalcond[4]=((((-1.0)*x1447*x1449))+((cj5*x1448)));
18756 evalcond[5]=(((sj5*x1448))+x1450+new_r00);
18757 evalcond[6]=((((-1.0)*x1450))+new_r11+(((-1.0)*x1448*x1449)));
18758 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 )
18759 {
18760 continue;
18761 }
18762 }
18763 
18764 {
18765 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18766 vinfos[0].jointtype = 1;
18767 vinfos[0].foffset = j0;
18768 vinfos[0].indices[0] = _ij0[0];
18769 vinfos[0].indices[1] = _ij0[1];
18770 vinfos[0].maxsolutions = _nj0;
18771 vinfos[1].jointtype = 1;
18772 vinfos[1].foffset = j1;
18773 vinfos[1].indices[0] = _ij1[0];
18774 vinfos[1].indices[1] = _ij1[1];
18775 vinfos[1].maxsolutions = _nj1;
18776 vinfos[2].jointtype = 1;
18777 vinfos[2].foffset = j2;
18778 vinfos[2].indices[0] = _ij2[0];
18779 vinfos[2].indices[1] = _ij2[1];
18780 vinfos[2].maxsolutions = _nj2;
18781 vinfos[3].jointtype = 1;
18782 vinfos[3].foffset = j3;
18783 vinfos[3].indices[0] = _ij3[0];
18784 vinfos[3].indices[1] = _ij3[1];
18785 vinfos[3].maxsolutions = _nj3;
18786 vinfos[4].jointtype = 1;
18787 vinfos[4].foffset = j4;
18788 vinfos[4].indices[0] = _ij4[0];
18789 vinfos[4].indices[1] = _ij4[1];
18790 vinfos[4].maxsolutions = _nj4;
18791 vinfos[5].jointtype = 1;
18792 vinfos[5].foffset = j5;
18793 vinfos[5].indices[0] = _ij5[0];
18794 vinfos[5].indices[1] = _ij5[1];
18795 vinfos[5].maxsolutions = _nj5;
18796 std::vector<int> vfree(0);
18797 solutions.AddSolution(vinfos,vfree);
18798 }
18799 }
18800 }
18801 
18802 }
18803 
18804 }
18805 
18806 } else
18807 {
18808 {
18809 IkReal j3array[1], cj3array[1], sj3array[1];
18810 bool j3valid[1]={false};
18811 _nj3 = 1;
18812 CheckValue<IkReal> x1452 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(((-1.0)*cj5)),IKFAST_ATAN2_MAGTHRESH);
18813 if(!x1452.valid){
18814 continue;
18815 }
18816 CheckValue<IkReal> x1453=IKPowWithIntegerCheck(IKsign(new_r00),-1);
18817 if(!x1453.valid){
18818 continue;
18819 }
18820 j3array[0]=((-1.5707963267949)+(x1452.value)+(((1.5707963267949)*(x1453.value))));
18821 sj3array[0]=IKsin(j3array[0]);
18822 cj3array[0]=IKcos(j3array[0]);
18823 if( j3array[0] > IKPI )
18824 {
18825  j3array[0]-=IK2PI;
18826 }
18827 else if( j3array[0] < -IKPI )
18828 { j3array[0]+=IK2PI;
18829 }
18830 j3valid[0] = true;
18831 for(int ij3 = 0; ij3 < 1; ++ij3)
18832 {
18833 if( !j3valid[ij3] )
18834 {
18835  continue;
18836 }
18837 _ij3[0] = ij3; _ij3[1] = -1;
18838 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18839 {
18840 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18841 {
18842  j3valid[iij3]=false; _ij3[1] = iij3; break;
18843 }
18844 }
18845 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18846 {
18847 IkReal evalcond[7];
18848 IkReal x1454=IKcos(j3);
18849 IkReal x1455=IKsin(j3);
18850 IkReal x1456=((1.0)*sj5);
18851 IkReal x1457=(cj5*x1454);
18852 IkReal x1458=((1.0)*x1454);
18853 evalcond[0]=(sj5+((new_r00*x1455)));
18854 evalcond[1]=(cj5+((new_r00*x1454)));
18855 evalcond[2]=(cj5+(((-1.0)*new_r11*x1458)));
18856 evalcond[3]=((((-1.0)*x1456))+((new_r11*x1455)));
18857 evalcond[4]=(((cj5*x1455))+(((-1.0)*x1454*x1456)));
18858 evalcond[5]=(((sj5*x1455))+x1457+new_r00);
18859 evalcond[6]=((((-1.0)*x1457))+(((-1.0)*x1455*x1456))+new_r11);
18860 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 )
18861 {
18862 continue;
18863 }
18864 }
18865 
18866 {
18867 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18868 vinfos[0].jointtype = 1;
18869 vinfos[0].foffset = j0;
18870 vinfos[0].indices[0] = _ij0[0];
18871 vinfos[0].indices[1] = _ij0[1];
18872 vinfos[0].maxsolutions = _nj0;
18873 vinfos[1].jointtype = 1;
18874 vinfos[1].foffset = j1;
18875 vinfos[1].indices[0] = _ij1[0];
18876 vinfos[1].indices[1] = _ij1[1];
18877 vinfos[1].maxsolutions = _nj1;
18878 vinfos[2].jointtype = 1;
18879 vinfos[2].foffset = j2;
18880 vinfos[2].indices[0] = _ij2[0];
18881 vinfos[2].indices[1] = _ij2[1];
18882 vinfos[2].maxsolutions = _nj2;
18883 vinfos[3].jointtype = 1;
18884 vinfos[3].foffset = j3;
18885 vinfos[3].indices[0] = _ij3[0];
18886 vinfos[3].indices[1] = _ij3[1];
18887 vinfos[3].maxsolutions = _nj3;
18888 vinfos[4].jointtype = 1;
18889 vinfos[4].foffset = j4;
18890 vinfos[4].indices[0] = _ij4[0];
18891 vinfos[4].indices[1] = _ij4[1];
18892 vinfos[4].maxsolutions = _nj4;
18893 vinfos[5].jointtype = 1;
18894 vinfos[5].foffset = j5;
18895 vinfos[5].indices[0] = _ij5[0];
18896 vinfos[5].indices[1] = _ij5[1];
18897 vinfos[5].maxsolutions = _nj5;
18898 std::vector<int> vfree(0);
18899 solutions.AddSolution(vinfos,vfree);
18900 }
18901 }
18902 }
18903 
18904 }
18905 
18906 }
18907 
18908 }
18909 } while(0);
18910 if( bgotonextstatement )
18911 {
18912 bool bgotonextstatement = true;
18913 do
18914 {
18915 if( 1 )
18916 {
18917 bgotonextstatement=false;
18918 continue; // branch miss [j3]
18919 
18920 }
18921 } while(0);
18922 if( bgotonextstatement )
18923 {
18924 }
18925 }
18926 }
18927 }
18928 }
18929 }
18930 }
18931 }
18932 }
18933 }
18934 }
18935 }
18936 
18937 } else
18938 {
18939 {
18940 IkReal j3array[1], cj3array[1], sj3array[1];
18941 bool j3valid[1]={false};
18942 _nj3 = 1;
18943 IkReal x1459=((1.0)*new_r00);
18944 CheckValue<IkReal> x1460 = IKatan2WithCheck(IkReal(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))),IkReal(((((-1.0)*new_r10*x1459))+((cj5*sj5)))),IKFAST_ATAN2_MAGTHRESH);
18945 if(!x1460.valid){
18946 continue;
18947 }
18948 CheckValue<IkReal> x1461=IKPowWithIntegerCheck(IKsign((((cj5*new_r10))+(((-1.0)*sj5*x1459)))),-1);
18949 if(!x1461.valid){
18950 continue;
18951 }
18952 j3array[0]=((-1.5707963267949)+(x1460.value)+(((1.5707963267949)*(x1461.value))));
18953 sj3array[0]=IKsin(j3array[0]);
18954 cj3array[0]=IKcos(j3array[0]);
18955 if( j3array[0] > IKPI )
18956 {
18957  j3array[0]-=IK2PI;
18958 }
18959 else if( j3array[0] < -IKPI )
18960 { j3array[0]+=IK2PI;
18961 }
18962 j3valid[0] = true;
18963 for(int ij3 = 0; ij3 < 1; ++ij3)
18964 {
18965 if( !j3valid[ij3] )
18966 {
18967  continue;
18968 }
18969 _ij3[0] = ij3; _ij3[1] = -1;
18970 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18971 {
18972 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18973 {
18974  j3valid[iij3]=false; _ij3[1] = iij3; break;
18975 }
18976 }
18977 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18978 {
18979 IkReal evalcond[8];
18980 IkReal x1462=IKsin(j3);
18981 IkReal x1463=IKcos(j3);
18982 IkReal x1464=((1.0)*sj5);
18983 IkReal x1465=(cj5*x1462);
18984 IkReal x1466=((1.0)*x1463);
18985 IkReal x1467=(x1463*x1464);
18986 evalcond[0]=(((new_r00*x1463))+cj5+((new_r10*x1462)));
18987 evalcond[1]=(((cj5*x1463))+new_r00+((sj5*x1462)));
18988 evalcond[2]=((((-1.0)*new_r10*x1466))+((new_r00*x1462))+sj5);
18989 evalcond[3]=(((new_r01*x1462))+cj5+(((-1.0)*new_r11*x1466)));
18990 evalcond[4]=((((-1.0)*x1467))+x1465+new_r01);
18991 evalcond[5]=((((-1.0)*x1467))+x1465+new_r10);
18992 evalcond[6]=(((new_r01*x1463))+(((-1.0)*x1464))+((new_r11*x1462)));
18993 evalcond[7]=((((-1.0)*cj5*x1466))+(((-1.0)*x1462*x1464))+new_r11);
18994 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 )
18995 {
18996 continue;
18997 }
18998 }
18999 
19000 {
19001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19002 vinfos[0].jointtype = 1;
19003 vinfos[0].foffset = j0;
19004 vinfos[0].indices[0] = _ij0[0];
19005 vinfos[0].indices[1] = _ij0[1];
19006 vinfos[0].maxsolutions = _nj0;
19007 vinfos[1].jointtype = 1;
19008 vinfos[1].foffset = j1;
19009 vinfos[1].indices[0] = _ij1[0];
19010 vinfos[1].indices[1] = _ij1[1];
19011 vinfos[1].maxsolutions = _nj1;
19012 vinfos[2].jointtype = 1;
19013 vinfos[2].foffset = j2;
19014 vinfos[2].indices[0] = _ij2[0];
19015 vinfos[2].indices[1] = _ij2[1];
19016 vinfos[2].maxsolutions = _nj2;
19017 vinfos[3].jointtype = 1;
19018 vinfos[3].foffset = j3;
19019 vinfos[3].indices[0] = _ij3[0];
19020 vinfos[3].indices[1] = _ij3[1];
19021 vinfos[3].maxsolutions = _nj3;
19022 vinfos[4].jointtype = 1;
19023 vinfos[4].foffset = j4;
19024 vinfos[4].indices[0] = _ij4[0];
19025 vinfos[4].indices[1] = _ij4[1];
19026 vinfos[4].maxsolutions = _nj4;
19027 vinfos[5].jointtype = 1;
19028 vinfos[5].foffset = j5;
19029 vinfos[5].indices[0] = _ij5[0];
19030 vinfos[5].indices[1] = _ij5[1];
19031 vinfos[5].maxsolutions = _nj5;
19032 std::vector<int> vfree(0);
19033 solutions.AddSolution(vinfos,vfree);
19034 }
19035 }
19036 }
19037 
19038 }
19039 
19040 }
19041 
19042 } else
19043 {
19044 {
19045 IkReal j3array[1], cj3array[1], sj3array[1];
19046 bool j3valid[1]={false};
19047 _nj3 = 1;
19048 IkReal x1468=((1.0)*new_r10);
19049 CheckValue<IkReal> x1469 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((((-1.0)*new_r01*x1468))+(cj5*cj5))),IKFAST_ATAN2_MAGTHRESH);
19050 if(!x1469.valid){
19051 continue;
19052 }
19053 CheckValue<IkReal> x1470=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x1468)))),-1);
19054 if(!x1470.valid){
19055 continue;
19056 }
19057 j3array[0]=((-1.5707963267949)+(x1469.value)+(((1.5707963267949)*(x1470.value))));
19058 sj3array[0]=IKsin(j3array[0]);
19059 cj3array[0]=IKcos(j3array[0]);
19060 if( j3array[0] > IKPI )
19061 {
19062  j3array[0]-=IK2PI;
19063 }
19064 else if( j3array[0] < -IKPI )
19065 { j3array[0]+=IK2PI;
19066 }
19067 j3valid[0] = true;
19068 for(int ij3 = 0; ij3 < 1; ++ij3)
19069 {
19070 if( !j3valid[ij3] )
19071 {
19072  continue;
19073 }
19074 _ij3[0] = ij3; _ij3[1] = -1;
19075 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19076 {
19077 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19078 {
19079  j3valid[iij3]=false; _ij3[1] = iij3; break;
19080 }
19081 }
19082 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19083 {
19084 IkReal evalcond[8];
19085 IkReal x1471=IKsin(j3);
19086 IkReal x1472=IKcos(j3);
19087 IkReal x1473=((1.0)*sj5);
19088 IkReal x1474=(cj5*x1471);
19089 IkReal x1475=((1.0)*x1472);
19090 IkReal x1476=(x1472*x1473);
19091 evalcond[0]=(((new_r10*x1471))+cj5+((new_r00*x1472)));
19092 evalcond[1]=(((cj5*x1472))+((sj5*x1471))+new_r00);
19093 evalcond[2]=(sj5+((new_r00*x1471))+(((-1.0)*new_r10*x1475)));
19094 evalcond[3]=(cj5+(((-1.0)*new_r11*x1475))+((new_r01*x1471)));
19095 evalcond[4]=(x1474+(((-1.0)*x1476))+new_r01);
19096 evalcond[5]=(x1474+(((-1.0)*x1476))+new_r10);
19097 evalcond[6]=(((new_r11*x1471))+((new_r01*x1472))+(((-1.0)*x1473)));
19098 evalcond[7]=((((-1.0)*x1471*x1473))+(((-1.0)*cj5*x1475))+new_r11);
19099 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 )
19100 {
19101 continue;
19102 }
19103 }
19104 
19105 {
19106 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19107 vinfos[0].jointtype = 1;
19108 vinfos[0].foffset = j0;
19109 vinfos[0].indices[0] = _ij0[0];
19110 vinfos[0].indices[1] = _ij0[1];
19111 vinfos[0].maxsolutions = _nj0;
19112 vinfos[1].jointtype = 1;
19113 vinfos[1].foffset = j1;
19114 vinfos[1].indices[0] = _ij1[0];
19115 vinfos[1].indices[1] = _ij1[1];
19116 vinfos[1].maxsolutions = _nj1;
19117 vinfos[2].jointtype = 1;
19118 vinfos[2].foffset = j2;
19119 vinfos[2].indices[0] = _ij2[0];
19120 vinfos[2].indices[1] = _ij2[1];
19121 vinfos[2].maxsolutions = _nj2;
19122 vinfos[3].jointtype = 1;
19123 vinfos[3].foffset = j3;
19124 vinfos[3].indices[0] = _ij3[0];
19125 vinfos[3].indices[1] = _ij3[1];
19126 vinfos[3].maxsolutions = _nj3;
19127 vinfos[4].jointtype = 1;
19128 vinfos[4].foffset = j4;
19129 vinfos[4].indices[0] = _ij4[0];
19130 vinfos[4].indices[1] = _ij4[1];
19131 vinfos[4].maxsolutions = _nj4;
19132 vinfos[5].jointtype = 1;
19133 vinfos[5].foffset = j5;
19134 vinfos[5].indices[0] = _ij5[0];
19135 vinfos[5].indices[1] = _ij5[1];
19136 vinfos[5].maxsolutions = _nj5;
19137 std::vector<int> vfree(0);
19138 solutions.AddSolution(vinfos,vfree);
19139 }
19140 }
19141 }
19142 
19143 }
19144 
19145 }
19146 
19147 } else
19148 {
19149 {
19150 IkReal j3array[1], cj3array[1], sj3array[1];
19151 bool j3valid[1]={false};
19152 _nj3 = 1;
19153 IkReal x1477=((1.0)*new_r10);
19154 CheckValue<IkReal> x1478 = IKatan2WithCheck(IkReal((((cj5*new_r11))+((cj5*new_r00)))),IkReal((((cj5*new_r01))+(((-1.0)*cj5*x1477)))),IKFAST_ATAN2_MAGTHRESH);
19155 if(!x1478.valid){
19156 continue;
19157 }
19158 CheckValue<IkReal> x1479=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x1477))+(((-1.0)*new_r00*new_r01)))),-1);
19159 if(!x1479.valid){
19160 continue;
19161 }
19162 j3array[0]=((-1.5707963267949)+(x1478.value)+(((1.5707963267949)*(x1479.value))));
19163 sj3array[0]=IKsin(j3array[0]);
19164 cj3array[0]=IKcos(j3array[0]);
19165 if( j3array[0] > IKPI )
19166 {
19167  j3array[0]-=IK2PI;
19168 }
19169 else if( j3array[0] < -IKPI )
19170 { j3array[0]+=IK2PI;
19171 }
19172 j3valid[0] = true;
19173 for(int ij3 = 0; ij3 < 1; ++ij3)
19174 {
19175 if( !j3valid[ij3] )
19176 {
19177  continue;
19178 }
19179 _ij3[0] = ij3; _ij3[1] = -1;
19180 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19181 {
19182 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19183 {
19184  j3valid[iij3]=false; _ij3[1] = iij3; break;
19185 }
19186 }
19187 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19188 {
19189 IkReal evalcond[8];
19190 IkReal x1480=IKsin(j3);
19191 IkReal x1481=IKcos(j3);
19192 IkReal x1482=((1.0)*sj5);
19193 IkReal x1483=(cj5*x1480);
19194 IkReal x1484=((1.0)*x1481);
19195 IkReal x1485=(x1481*x1482);
19196 evalcond[0]=(cj5+((new_r00*x1481))+((new_r10*x1480)));
19197 evalcond[1]=(((cj5*x1481))+new_r00+((sj5*x1480)));
19198 evalcond[2]=(sj5+(((-1.0)*new_r10*x1484))+((new_r00*x1480)));
19199 evalcond[3]=(cj5+(((-1.0)*new_r11*x1484))+((new_r01*x1480)));
19200 evalcond[4]=((((-1.0)*x1485))+x1483+new_r01);
19201 evalcond[5]=((((-1.0)*x1485))+x1483+new_r10);
19202 evalcond[6]=((((-1.0)*x1482))+((new_r11*x1480))+((new_r01*x1481)));
19203 evalcond[7]=((((-1.0)*x1480*x1482))+new_r11+(((-1.0)*cj5*x1484)));
19204 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 )
19205 {
19206 continue;
19207 }
19208 }
19209 
19210 {
19211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19212 vinfos[0].jointtype = 1;
19213 vinfos[0].foffset = j0;
19214 vinfos[0].indices[0] = _ij0[0];
19215 vinfos[0].indices[1] = _ij0[1];
19216 vinfos[0].maxsolutions = _nj0;
19217 vinfos[1].jointtype = 1;
19218 vinfos[1].foffset = j1;
19219 vinfos[1].indices[0] = _ij1[0];
19220 vinfos[1].indices[1] = _ij1[1];
19221 vinfos[1].maxsolutions = _nj1;
19222 vinfos[2].jointtype = 1;
19223 vinfos[2].foffset = j2;
19224 vinfos[2].indices[0] = _ij2[0];
19225 vinfos[2].indices[1] = _ij2[1];
19226 vinfos[2].maxsolutions = _nj2;
19227 vinfos[3].jointtype = 1;
19228 vinfos[3].foffset = j3;
19229 vinfos[3].indices[0] = _ij3[0];
19230 vinfos[3].indices[1] = _ij3[1];
19231 vinfos[3].maxsolutions = _nj3;
19232 vinfos[4].jointtype = 1;
19233 vinfos[4].foffset = j4;
19234 vinfos[4].indices[0] = _ij4[0];
19235 vinfos[4].indices[1] = _ij4[1];
19236 vinfos[4].maxsolutions = _nj4;
19237 vinfos[5].jointtype = 1;
19238 vinfos[5].foffset = j5;
19239 vinfos[5].indices[0] = _ij5[0];
19240 vinfos[5].indices[1] = _ij5[1];
19241 vinfos[5].maxsolutions = _nj5;
19242 std::vector<int> vfree(0);
19243 solutions.AddSolution(vinfos,vfree);
19244 }
19245 }
19246 }
19247 
19248 }
19249 
19250 }
19251 
19252 }
19253 } while(0);
19254 if( bgotonextstatement )
19255 {
19256 bool bgotonextstatement = true;
19257 do
19258 {
19259 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
19260 if( IKabs(evalcond[0]) < 0.0000050000000000 )
19261 {
19262 bgotonextstatement=false;
19263 {
19264 IkReal j3eval[1];
19265 new_r02=0;
19266 new_r12=0;
19267 new_r20=0;
19268 new_r21=0;
19269 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
19270 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19271 {
19272 {
19273 IkReal j3eval[1];
19274 new_r02=0;
19275 new_r12=0;
19276 new_r20=0;
19277 new_r21=0;
19278 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
19279 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19280 {
19281 {
19282 IkReal j3eval[1];
19283 new_r02=0;
19284 new_r12=0;
19285 new_r20=0;
19286 new_r21=0;
19287 j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
19288 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19289 {
19290 continue; // no branches [j3]
19291 
19292 } else
19293 {
19294 {
19295 IkReal j3array[2], cj3array[2], sj3array[2];
19296 bool j3valid[2]={false};
19297 _nj3 = 2;
19298 IkReal x1486=((-1.0)*new_r22);
19299 CheckValue<IkReal> x1488 = IKatan2WithCheck(IkReal((new_r00*x1486)),IkReal((new_r10*x1486)),IKFAST_ATAN2_MAGTHRESH);
19300 if(!x1488.valid){
19301 continue;
19302 }
19303 IkReal x1487=x1488.value;
19304 j3array[0]=((-1.0)*x1487);
19305 sj3array[0]=IKsin(j3array[0]);
19306 cj3array[0]=IKcos(j3array[0]);
19307 j3array[1]=((3.14159265358979)+(((-1.0)*x1487)));
19308 sj3array[1]=IKsin(j3array[1]);
19309 cj3array[1]=IKcos(j3array[1]);
19310 if( j3array[0] > IKPI )
19311 {
19312  j3array[0]-=IK2PI;
19313 }
19314 else if( j3array[0] < -IKPI )
19315 { j3array[0]+=IK2PI;
19316 }
19317 j3valid[0] = true;
19318 if( j3array[1] > IKPI )
19319 {
19320  j3array[1]-=IK2PI;
19321 }
19322 else if( j3array[1] < -IKPI )
19323 { j3array[1]+=IK2PI;
19324 }
19325 j3valid[1] = true;
19326 for(int ij3 = 0; ij3 < 2; ++ij3)
19327 {
19328 if( !j3valid[ij3] )
19329 {
19330  continue;
19331 }
19332 _ij3[0] = ij3; _ij3[1] = -1;
19333 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19334 {
19335 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19336 {
19337  j3valid[iij3]=false; _ij3[1] = iij3; break;
19338 }
19339 }
19340 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19341 {
19342 IkReal evalcond[5];
19343 IkReal x1489=IKsin(j3);
19344 IkReal x1490=IKcos(j3);
19345 IkReal x1491=((1.0)*new_r11);
19346 IkReal x1492=(new_r01*x1490);
19347 evalcond[0]=(((new_r11*x1489))+x1492);
19348 evalcond[1]=(((new_r00*x1490))+((new_r10*x1489)));
19349 evalcond[2]=(((new_r00*x1489))+(((-1.0)*new_r10*x1490)));
19350 evalcond[3]=(((new_r01*x1489))+(((-1.0)*x1490*x1491)));
19351 evalcond[4]=((((-1.0)*new_r22*x1492))+(((-1.0)*new_r22*x1489*x1491)));
19352 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 )
19353 {
19354 continue;
19355 }
19356 }
19357 
19358 {
19359 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19360 vinfos[0].jointtype = 1;
19361 vinfos[0].foffset = j0;
19362 vinfos[0].indices[0] = _ij0[0];
19363 vinfos[0].indices[1] = _ij0[1];
19364 vinfos[0].maxsolutions = _nj0;
19365 vinfos[1].jointtype = 1;
19366 vinfos[1].foffset = j1;
19367 vinfos[1].indices[0] = _ij1[0];
19368 vinfos[1].indices[1] = _ij1[1];
19369 vinfos[1].maxsolutions = _nj1;
19370 vinfos[2].jointtype = 1;
19371 vinfos[2].foffset = j2;
19372 vinfos[2].indices[0] = _ij2[0];
19373 vinfos[2].indices[1] = _ij2[1];
19374 vinfos[2].maxsolutions = _nj2;
19375 vinfos[3].jointtype = 1;
19376 vinfos[3].foffset = j3;
19377 vinfos[3].indices[0] = _ij3[0];
19378 vinfos[3].indices[1] = _ij3[1];
19379 vinfos[3].maxsolutions = _nj3;
19380 vinfos[4].jointtype = 1;
19381 vinfos[4].foffset = j4;
19382 vinfos[4].indices[0] = _ij4[0];
19383 vinfos[4].indices[1] = _ij4[1];
19384 vinfos[4].maxsolutions = _nj4;
19385 vinfos[5].jointtype = 1;
19386 vinfos[5].foffset = j5;
19387 vinfos[5].indices[0] = _ij5[0];
19388 vinfos[5].indices[1] = _ij5[1];
19389 vinfos[5].maxsolutions = _nj5;
19390 std::vector<int> vfree(0);
19391 solutions.AddSolution(vinfos,vfree);
19392 }
19393 }
19394 }
19395 
19396 }
19397 
19398 }
19399 
19400 } else
19401 {
19402 {
19403 IkReal j3array[2], cj3array[2], sj3array[2];
19404 bool j3valid[2]={false};
19405 _nj3 = 2;
19406 CheckValue<IkReal> x1494 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
19407 if(!x1494.valid){
19408 continue;
19409 }
19410 IkReal x1493=x1494.value;
19411 j3array[0]=((-1.0)*x1493);
19412 sj3array[0]=IKsin(j3array[0]);
19413 cj3array[0]=IKcos(j3array[0]);
19414 j3array[1]=((3.14159265358979)+(((-1.0)*x1493)));
19415 sj3array[1]=IKsin(j3array[1]);
19416 cj3array[1]=IKcos(j3array[1]);
19417 if( j3array[0] > IKPI )
19418 {
19419  j3array[0]-=IK2PI;
19420 }
19421 else if( j3array[0] < -IKPI )
19422 { j3array[0]+=IK2PI;
19423 }
19424 j3valid[0] = true;
19425 if( j3array[1] > IKPI )
19426 {
19427  j3array[1]-=IK2PI;
19428 }
19429 else if( j3array[1] < -IKPI )
19430 { j3array[1]+=IK2PI;
19431 }
19432 j3valid[1] = true;
19433 for(int ij3 = 0; ij3 < 2; ++ij3)
19434 {
19435 if( !j3valid[ij3] )
19436 {
19437  continue;
19438 }
19439 _ij3[0] = ij3; _ij3[1] = -1;
19440 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19441 {
19442 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19443 {
19444  j3valid[iij3]=false; _ij3[1] = iij3; break;
19445 }
19446 }
19447 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19448 {
19449 IkReal evalcond[5];
19450 IkReal x1495=IKsin(j3);
19451 IkReal x1496=IKcos(j3);
19452 IkReal x1497=((1.0)*x1496);
19453 IkReal x1498=((1.0)*new_r22*x1495);
19454 evalcond[0]=(((new_r01*x1496))+((new_r11*x1495)));
19455 evalcond[1]=((((-1.0)*new_r10*x1497))+((new_r00*x1495)));
19456 evalcond[2]=(((new_r01*x1495))+(((-1.0)*new_r11*x1497)));
19457 evalcond[3]=((((-1.0)*new_r10*x1498))+(((-1.0)*new_r00*new_r22*x1497)));
19458 evalcond[4]=((((-1.0)*new_r11*x1498))+(((-1.0)*new_r01*new_r22*x1497)));
19459 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 )
19460 {
19461 continue;
19462 }
19463 }
19464 
19465 {
19466 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19467 vinfos[0].jointtype = 1;
19468 vinfos[0].foffset = j0;
19469 vinfos[0].indices[0] = _ij0[0];
19470 vinfos[0].indices[1] = _ij0[1];
19471 vinfos[0].maxsolutions = _nj0;
19472 vinfos[1].jointtype = 1;
19473 vinfos[1].foffset = j1;
19474 vinfos[1].indices[0] = _ij1[0];
19475 vinfos[1].indices[1] = _ij1[1];
19476 vinfos[1].maxsolutions = _nj1;
19477 vinfos[2].jointtype = 1;
19478 vinfos[2].foffset = j2;
19479 vinfos[2].indices[0] = _ij2[0];
19480 vinfos[2].indices[1] = _ij2[1];
19481 vinfos[2].maxsolutions = _nj2;
19482 vinfos[3].jointtype = 1;
19483 vinfos[3].foffset = j3;
19484 vinfos[3].indices[0] = _ij3[0];
19485 vinfos[3].indices[1] = _ij3[1];
19486 vinfos[3].maxsolutions = _nj3;
19487 vinfos[4].jointtype = 1;
19488 vinfos[4].foffset = j4;
19489 vinfos[4].indices[0] = _ij4[0];
19490 vinfos[4].indices[1] = _ij4[1];
19491 vinfos[4].maxsolutions = _nj4;
19492 vinfos[5].jointtype = 1;
19493 vinfos[5].foffset = j5;
19494 vinfos[5].indices[0] = _ij5[0];
19495 vinfos[5].indices[1] = _ij5[1];
19496 vinfos[5].maxsolutions = _nj5;
19497 std::vector<int> vfree(0);
19498 solutions.AddSolution(vinfos,vfree);
19499 }
19500 }
19501 }
19502 
19503 }
19504 
19505 }
19506 
19507 } else
19508 {
19509 {
19510 IkReal j3array[2], cj3array[2], sj3array[2];
19511 bool j3valid[2]={false};
19512 _nj3 = 2;
19513 CheckValue<IkReal> x1500 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
19514 if(!x1500.valid){
19515 continue;
19516 }
19517 IkReal x1499=x1500.value;
19518 j3array[0]=((-1.0)*x1499);
19519 sj3array[0]=IKsin(j3array[0]);
19520 cj3array[0]=IKcos(j3array[0]);
19521 j3array[1]=((3.14159265358979)+(((-1.0)*x1499)));
19522 sj3array[1]=IKsin(j3array[1]);
19523 cj3array[1]=IKcos(j3array[1]);
19524 if( j3array[0] > IKPI )
19525 {
19526  j3array[0]-=IK2PI;
19527 }
19528 else if( j3array[0] < -IKPI )
19529 { j3array[0]+=IK2PI;
19530 }
19531 j3valid[0] = true;
19532 if( j3array[1] > IKPI )
19533 {
19534  j3array[1]-=IK2PI;
19535 }
19536 else if( j3array[1] < -IKPI )
19537 { j3array[1]+=IK2PI;
19538 }
19539 j3valid[1] = true;
19540 for(int ij3 = 0; ij3 < 2; ++ij3)
19541 {
19542 if( !j3valid[ij3] )
19543 {
19544  continue;
19545 }
19546 _ij3[0] = ij3; _ij3[1] = -1;
19547 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19548 {
19549 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19550 {
19551  j3valid[iij3]=false; _ij3[1] = iij3; break;
19552 }
19553 }
19554 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19555 {
19556 IkReal evalcond[5];
19557 IkReal x1501=IKsin(j3);
19558 IkReal x1502=IKcos(j3);
19559 IkReal x1503=((1.0)*new_r22);
19560 IkReal x1504=(new_r10*x1501);
19561 IkReal x1505=((1.0)*x1502);
19562 IkReal x1506=(new_r00*x1502);
19563 evalcond[0]=(x1506+x1504);
19564 evalcond[1]=((((-1.0)*new_r10*x1505))+((new_r00*x1501)));
19565 evalcond[2]=((((-1.0)*new_r11*x1505))+((new_r01*x1501)));
19566 evalcond[3]=((((-1.0)*x1503*x1506))+(((-1.0)*x1503*x1504)));
19567 evalcond[4]=((((-1.0)*new_r11*x1501*x1503))+(((-1.0)*new_r01*x1502*x1503)));
19568 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 )
19569 {
19570 continue;
19571 }
19572 }
19573 
19574 {
19575 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19576 vinfos[0].jointtype = 1;
19577 vinfos[0].foffset = j0;
19578 vinfos[0].indices[0] = _ij0[0];
19579 vinfos[0].indices[1] = _ij0[1];
19580 vinfos[0].maxsolutions = _nj0;
19581 vinfos[1].jointtype = 1;
19582 vinfos[1].foffset = j1;
19583 vinfos[1].indices[0] = _ij1[0];
19584 vinfos[1].indices[1] = _ij1[1];
19585 vinfos[1].maxsolutions = _nj1;
19586 vinfos[2].jointtype = 1;
19587 vinfos[2].foffset = j2;
19588 vinfos[2].indices[0] = _ij2[0];
19589 vinfos[2].indices[1] = _ij2[1];
19590 vinfos[2].maxsolutions = _nj2;
19591 vinfos[3].jointtype = 1;
19592 vinfos[3].foffset = j3;
19593 vinfos[3].indices[0] = _ij3[0];
19594 vinfos[3].indices[1] = _ij3[1];
19595 vinfos[3].maxsolutions = _nj3;
19596 vinfos[4].jointtype = 1;
19597 vinfos[4].foffset = j4;
19598 vinfos[4].indices[0] = _ij4[0];
19599 vinfos[4].indices[1] = _ij4[1];
19600 vinfos[4].maxsolutions = _nj4;
19601 vinfos[5].jointtype = 1;
19602 vinfos[5].foffset = j5;
19603 vinfos[5].indices[0] = _ij5[0];
19604 vinfos[5].indices[1] = _ij5[1];
19605 vinfos[5].maxsolutions = _nj5;
19606 std::vector<int> vfree(0);
19607 solutions.AddSolution(vinfos,vfree);
19608 }
19609 }
19610 }
19611 
19612 }
19613 
19614 }
19615 
19616 }
19617 } while(0);
19618 if( bgotonextstatement )
19619 {
19620 bool bgotonextstatement = true;
19621 do
19622 {
19623 if( 1 )
19624 {
19625 bgotonextstatement=false;
19626 continue; // branch miss [j3]
19627 
19628 }
19629 } while(0);
19630 if( bgotonextstatement )
19631 {
19632 }
19633 }
19634 }
19635 }
19636 }
19637 
19638 } else
19639 {
19640 {
19641 IkReal j3array[1], cj3array[1], sj3array[1];
19642 bool j3valid[1]={false};
19643 _nj3 = 1;
19644 CheckValue<IkReal> x1508=IKPowWithIntegerCheck(sj4,-1);
19645 if(!x1508.valid){
19646 continue;
19647 }
19648 IkReal x1507=x1508.value;
19649 CheckValue<IkReal> x1509=IKPowWithIntegerCheck(new_r12,-1);
19650 if(!x1509.valid){
19651 continue;
19652 }
19653 if( IKabs((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x1507)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x1507))-1) <= IKFAST_SINCOS_THRESH )
19654  continue;
19655 j3array[0]=IKatan2((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1507));
19656 sj3array[0]=IKsin(j3array[0]);
19657 cj3array[0]=IKcos(j3array[0]);
19658 if( j3array[0] > IKPI )
19659 {
19660  j3array[0]-=IK2PI;
19661 }
19662 else if( j3array[0] < -IKPI )
19663 { j3array[0]+=IK2PI;
19664 }
19665 j3valid[0] = true;
19666 for(int ij3 = 0; ij3 < 1; ++ij3)
19667 {
19668 if( !j3valid[ij3] )
19669 {
19670  continue;
19671 }
19672 _ij3[0] = ij3; _ij3[1] = -1;
19673 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19674 {
19675 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19676 {
19677  j3valid[iij3]=false; _ij3[1] = iij3; break;
19678 }
19679 }
19680 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19681 {
19682 IkReal evalcond[18];
19683 IkReal x1510=IKcos(j3);
19684 IkReal x1511=IKsin(j3);
19685 IkReal x1512=(cj4*cj5);
19686 IkReal x1513=(cj4*sj5);
19687 IkReal x1514=((1.0)*new_r20);
19688 IkReal x1515=((1.0)*cj4);
19689 IkReal x1516=((1.0)*sj4);
19690 IkReal x1517=(sj4*x1511);
19691 IkReal x1518=((1.0)*x1510);
19692 IkReal x1519=(sj4*x1510);
19693 IkReal x1520=(x1511*x1515);
19694 evalcond[0]=(x1519+new_r02);
19695 evalcond[1]=(x1517+new_r12);
19696 evalcond[2]=((((-1.0)*new_r02*x1511))+((new_r12*x1510)));
19697 evalcond[3]=(sj4+((new_r02*x1510))+((new_r12*x1511)));
19698 evalcond[4]=(sj5+(((-1.0)*new_r10*x1518))+((new_r00*x1511)));
19699 evalcond[5]=((((-1.0)*new_r11*x1518))+cj5+((new_r01*x1511)));
19700 evalcond[6]=(((x1510*x1513))+((cj5*x1511))+new_r01);
19701 evalcond[7]=(x1513+((new_r01*x1510))+((new_r11*x1511)));
19702 evalcond[8]=(((sj5*x1511))+(((-1.0)*x1512*x1518))+new_r00);
19703 evalcond[9]=((((-1.0)*cj5*x1518))+new_r11+((x1511*x1513)));
19704 evalcond[10]=(((new_r10*x1511))+(((-1.0)*x1512))+((new_r00*x1510)));
19705 evalcond[11]=((((-1.0)*x1511*x1512))+(((-1.0)*sj5*x1518))+new_r10);
19706 evalcond[12]=(((new_r10*x1517))+(((-1.0)*cj4*x1514))+((new_r00*x1519)));
19707 evalcond[13]=((((-1.0)*new_r21*x1515))+((new_r01*x1519))+((new_r11*x1517)));
19708 evalcond[14]=((1.0)+(((-1.0)*new_r22*x1515))+((new_r02*x1519))+((new_r12*x1517)));
19709 evalcond[15]=((((-1.0)*new_r22*x1516))+(((-1.0)*new_r12*x1520))+(((-1.0)*new_r02*x1510*x1515)));
19710 evalcond[16]=(cj5+(((-1.0)*new_r00*x1510*x1515))+(((-1.0)*sj4*x1514))+(((-1.0)*new_r10*x1520)));
19711 evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r11*x1520))+(((-1.0)*new_r21*x1516))+(((-1.0)*new_r01*x1510*x1515)));
19712 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 )
19713 {
19714 continue;
19715 }
19716 }
19717 
19718 {
19719 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19720 vinfos[0].jointtype = 1;
19721 vinfos[0].foffset = j0;
19722 vinfos[0].indices[0] = _ij0[0];
19723 vinfos[0].indices[1] = _ij0[1];
19724 vinfos[0].maxsolutions = _nj0;
19725 vinfos[1].jointtype = 1;
19726 vinfos[1].foffset = j1;
19727 vinfos[1].indices[0] = _ij1[0];
19728 vinfos[1].indices[1] = _ij1[1];
19729 vinfos[1].maxsolutions = _nj1;
19730 vinfos[2].jointtype = 1;
19731 vinfos[2].foffset = j2;
19732 vinfos[2].indices[0] = _ij2[0];
19733 vinfos[2].indices[1] = _ij2[1];
19734 vinfos[2].maxsolutions = _nj2;
19735 vinfos[3].jointtype = 1;
19736 vinfos[3].foffset = j3;
19737 vinfos[3].indices[0] = _ij3[0];
19738 vinfos[3].indices[1] = _ij3[1];
19739 vinfos[3].maxsolutions = _nj3;
19740 vinfos[4].jointtype = 1;
19741 vinfos[4].foffset = j4;
19742 vinfos[4].indices[0] = _ij4[0];
19743 vinfos[4].indices[1] = _ij4[1];
19744 vinfos[4].maxsolutions = _nj4;
19745 vinfos[5].jointtype = 1;
19746 vinfos[5].foffset = j5;
19747 vinfos[5].indices[0] = _ij5[0];
19748 vinfos[5].indices[1] = _ij5[1];
19749 vinfos[5].maxsolutions = _nj5;
19750 std::vector<int> vfree(0);
19751 solutions.AddSolution(vinfos,vfree);
19752 }
19753 }
19754 }
19755 
19756 }
19757 
19758 }
19759 
19760 } else
19761 {
19762 {
19763 IkReal j3array[1], cj3array[1], sj3array[1];
19764 bool j3valid[1]={false};
19765 _nj3 = 1;
19766 CheckValue<IkReal> x1521=IKPowWithIntegerCheck(IKsign(sj4),-1);
19767 if(!x1521.valid){
19768 continue;
19769 }
19770 CheckValue<IkReal> x1522 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
19771 if(!x1522.valid){
19772 continue;
19773 }
19774 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1521.value)))+(x1522.value));
19775 sj3array[0]=IKsin(j3array[0]);
19776 cj3array[0]=IKcos(j3array[0]);
19777 if( j3array[0] > IKPI )
19778 {
19779  j3array[0]-=IK2PI;
19780 }
19781 else if( j3array[0] < -IKPI )
19782 { j3array[0]+=IK2PI;
19783 }
19784 j3valid[0] = true;
19785 for(int ij3 = 0; ij3 < 1; ++ij3)
19786 {
19787 if( !j3valid[ij3] )
19788 {
19789  continue;
19790 }
19791 _ij3[0] = ij3; _ij3[1] = -1;
19792 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19793 {
19794 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19795 {
19796  j3valid[iij3]=false; _ij3[1] = iij3; break;
19797 }
19798 }
19799 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19800 {
19801 IkReal evalcond[18];
19802 IkReal x1523=IKcos(j3);
19803 IkReal x1524=IKsin(j3);
19804 IkReal x1525=(cj4*cj5);
19805 IkReal x1526=(cj4*sj5);
19806 IkReal x1527=((1.0)*new_r20);
19807 IkReal x1528=((1.0)*cj4);
19808 IkReal x1529=((1.0)*sj4);
19809 IkReal x1530=(sj4*x1524);
19810 IkReal x1531=((1.0)*x1523);
19811 IkReal x1532=(sj4*x1523);
19812 IkReal x1533=(x1524*x1528);
19813 evalcond[0]=(x1532+new_r02);
19814 evalcond[1]=(x1530+new_r12);
19815 evalcond[2]=(((new_r12*x1523))+(((-1.0)*new_r02*x1524)));
19816 evalcond[3]=(((new_r12*x1524))+sj4+((new_r02*x1523)));
19817 evalcond[4]=((((-1.0)*new_r10*x1531))+sj5+((new_r00*x1524)));
19818 evalcond[5]=(cj5+((new_r01*x1524))+(((-1.0)*new_r11*x1531)));
19819 evalcond[6]=(((x1523*x1526))+((cj5*x1524))+new_r01);
19820 evalcond[7]=(((new_r01*x1523))+x1526+((new_r11*x1524)));
19821 evalcond[8]=(((sj5*x1524))+(((-1.0)*x1525*x1531))+new_r00);
19822 evalcond[9]=(((x1524*x1526))+(((-1.0)*cj5*x1531))+new_r11);
19823 evalcond[10]=(((new_r00*x1523))+((new_r10*x1524))+(((-1.0)*x1525)));
19824 evalcond[11]=((((-1.0)*x1524*x1525))+new_r10+(((-1.0)*sj5*x1531)));
19825 evalcond[12]=((((-1.0)*cj4*x1527))+((new_r00*x1532))+((new_r10*x1530)));
19826 evalcond[13]=((((-1.0)*new_r21*x1528))+((new_r01*x1532))+((new_r11*x1530)));
19827 evalcond[14]=((1.0)+((new_r02*x1532))+(((-1.0)*new_r22*x1528))+((new_r12*x1530)));
19828 evalcond[15]=((((-1.0)*new_r02*x1523*x1528))+(((-1.0)*new_r22*x1529))+(((-1.0)*new_r12*x1533)));
19829 evalcond[16]=((((-1.0)*new_r10*x1533))+(((-1.0)*sj4*x1527))+cj5+(((-1.0)*new_r00*x1523*x1528)));
19830 evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r21*x1529))+(((-1.0)*new_r01*x1523*x1528))+(((-1.0)*new_r11*x1533)));
19831 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 )
19832 {
19833 continue;
19834 }
19835 }
19836 
19837 {
19838 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19839 vinfos[0].jointtype = 1;
19840 vinfos[0].foffset = j0;
19841 vinfos[0].indices[0] = _ij0[0];
19842 vinfos[0].indices[1] = _ij0[1];
19843 vinfos[0].maxsolutions = _nj0;
19844 vinfos[1].jointtype = 1;
19845 vinfos[1].foffset = j1;
19846 vinfos[1].indices[0] = _ij1[0];
19847 vinfos[1].indices[1] = _ij1[1];
19848 vinfos[1].maxsolutions = _nj1;
19849 vinfos[2].jointtype = 1;
19850 vinfos[2].foffset = j2;
19851 vinfos[2].indices[0] = _ij2[0];
19852 vinfos[2].indices[1] = _ij2[1];
19853 vinfos[2].maxsolutions = _nj2;
19854 vinfos[3].jointtype = 1;
19855 vinfos[3].foffset = j3;
19856 vinfos[3].indices[0] = _ij3[0];
19857 vinfos[3].indices[1] = _ij3[1];
19858 vinfos[3].maxsolutions = _nj3;
19859 vinfos[4].jointtype = 1;
19860 vinfos[4].foffset = j4;
19861 vinfos[4].indices[0] = _ij4[0];
19862 vinfos[4].indices[1] = _ij4[1];
19863 vinfos[4].maxsolutions = _nj4;
19864 vinfos[5].jointtype = 1;
19865 vinfos[5].foffset = j5;
19866 vinfos[5].indices[0] = _ij5[0];
19867 vinfos[5].indices[1] = _ij5[1];
19868 vinfos[5].maxsolutions = _nj5;
19869 std::vector<int> vfree(0);
19870 solutions.AddSolution(vinfos,vfree);
19871 }
19872 }
19873 }
19874 
19875 }
19876 
19877 }
19878 }
19879 }
19880 
19881 }
19882 
19883 }
19884 }
19885 }
19886 }
19887 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
19888 {
19889  using std::complex;
19890  if( rawcoeffs[0] == 0 ) {
19891  // solve with one reduced degree
19892  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
19893  return;
19894  }
19895  IKFAST_ASSERT(rawcoeffs[0] != 0);
19896  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19897  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19898  complex<IkReal> coeffs[3];
19899  const int maxsteps = 110;
19900  for(int i = 0; i < 3; ++i) {
19901  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19902  }
19903  complex<IkReal> roots[3];
19904  IkReal err[3];
19905  roots[0] = complex<IkReal>(1,0);
19906  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19907  err[0] = 1.0;
19908  err[1] = 1.0;
19909  for(int i = 2; i < 3; ++i) {
19910  roots[i] = roots[i-1]*roots[1];
19911  err[i] = 1.0;
19912  }
19913  for(int step = 0; step < maxsteps; ++step) {
19914  bool changed = false;
19915  for(int i = 0; i < 3; ++i) {
19916  if ( err[i] >= tol ) {
19917  changed = true;
19918  // evaluate
19919  complex<IkReal> x = roots[i] + coeffs[0];
19920  for(int j = 1; j < 3; ++j) {
19921  x = roots[i] * x + coeffs[j];
19922  }
19923  for(int j = 0; j < 3; ++j) {
19924  if( i != j ) {
19925  if( roots[i] != roots[j] ) {
19926  x /= (roots[i] - roots[j]);
19927  }
19928  }
19929  }
19930  roots[i] -= x;
19931  err[i] = abs(x);
19932  }
19933  }
19934  if( !changed ) {
19935  break;
19936  }
19937  }
19938 
19939  numroots = 0;
19940  bool visited[3] = {false};
19941  for(int i = 0; i < 3; ++i) {
19942  if( !visited[i] ) {
19943  // might be a multiple root, in which case it will have more error than the other roots
19944  // find any neighboring roots, and take the average
19945  complex<IkReal> newroot=roots[i];
19946  int n = 1;
19947  for(int j = i+1; j < 3; ++j) {
19948  // care about error in real much more than imaginary
19949  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19950  newroot += roots[j];
19951  n += 1;
19952  visited[j] = true;
19953  }
19954  }
19955  if( n > 1 ) {
19956  newroot /= n;
19957  }
19958  // 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
19959  if( IKabs(imag(newroot)) < tolsqrt ) {
19960  rawroots[numroots++] = real(newroot);
19961  }
19962  }
19963  }
19964 }
19965 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
19966  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19967  if( det < 0 ) {
19968  numroots=0;
19969  }
19970  else if( det == 0 ) {
19971  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19972  numroots = 1;
19973  }
19974  else {
19975  det = IKsqrt(det);
19976  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19977  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
19978  numroots = 2;
19979  }
19980 }
19981 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
19982 {
19983  using std::complex;
19984  if( rawcoeffs[0] == 0 ) {
19985  // solve with one reduced degree
19986  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19987  return;
19988  }
19989  IKFAST_ASSERT(rawcoeffs[0] != 0);
19990  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19991  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19992  complex<IkReal> coeffs[4];
19993  const int maxsteps = 110;
19994  for(int i = 0; i < 4; ++i) {
19995  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19996  }
19997  complex<IkReal> roots[4];
19998  IkReal err[4];
19999  roots[0] = complex<IkReal>(1,0);
20000  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
20001  err[0] = 1.0;
20002  err[1] = 1.0;
20003  for(int i = 2; i < 4; ++i) {
20004  roots[i] = roots[i-1]*roots[1];
20005  err[i] = 1.0;
20006  }
20007  for(int step = 0; step < maxsteps; ++step) {
20008  bool changed = false;
20009  for(int i = 0; i < 4; ++i) {
20010  if ( err[i] >= tol ) {
20011  changed = true;
20012  // evaluate
20013  complex<IkReal> x = roots[i] + coeffs[0];
20014  for(int j = 1; j < 4; ++j) {
20015  x = roots[i] * x + coeffs[j];
20016  }
20017  for(int j = 0; j < 4; ++j) {
20018  if( i != j ) {
20019  if( roots[i] != roots[j] ) {
20020  x /= (roots[i] - roots[j]);
20021  }
20022  }
20023  }
20024  roots[i] -= x;
20025  err[i] = abs(x);
20026  }
20027  }
20028  if( !changed ) {
20029  break;
20030  }
20031  }
20032 
20033  numroots = 0;
20034  bool visited[4] = {false};
20035  for(int i = 0; i < 4; ++i) {
20036  if( !visited[i] ) {
20037  // might be a multiple root, in which case it will have more error than the other roots
20038  // find any neighboring roots, and take the average
20039  complex<IkReal> newroot=roots[i];
20040  int n = 1;
20041  for(int j = i+1; j < 4; ++j) {
20042  // care about error in real much more than imaginary
20043  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
20044  newroot += roots[j];
20045  n += 1;
20046  visited[j] = true;
20047  }
20048  }
20049  if( n > 1 ) {
20050  newroot /= n;
20051  }
20052  // 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
20053  if( IKabs(imag(newroot)) < tolsqrt ) {
20054  rawroots[numroots++] = real(newroot);
20055  }
20056  }
20057  }
20058 }
20059 };
20060 
20061 
20064 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
20065 IKSolver solver;
20066 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20067 }
20068 
20069 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
20070 IKSolver solver;
20071 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20072 }
20073 
20074 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - prbt (11ad975bd60aa3f28e8d228465d0f213)>"; }
20075 
20076 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
20077 
20078 #ifdef IKFAST_NAMESPACE
20079 } // end namespace
20080 #endif
20081 
20082 #ifndef IKFAST_NO_MAIN
20083 #include <stdio.h>
20084 #include <stdlib.h>
20085 #ifdef IKFAST_NAMESPACE
20086 using namespace IKFAST_NAMESPACE;
20087 #endif
20088 int main(int argc, char** argv)
20089 {
20090  if( argc != 12+GetNumFreeParameters()+1 ) {
20091  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
20092  "Returns the ik solutions given the transformation of the end effector specified by\n"
20093  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
20094  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
20095  return 1;
20096  }
20097 
20098  IkSolutionList<IkReal> solutions;
20099  std::vector<IkReal> vfree(GetNumFreeParameters());
20100  IkReal eerot[9],eetrans[3];
20101  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
20102  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
20103  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20104  for(std::size_t i = 0; i < vfree.size(); ++i)
20105  vfree[i] = atof(argv[13+i]);
20106  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
20107 
20108  if( !bSuccess ) {
20109  fprintf(stderr,"Failed to get ik solution\n");
20110  return -1;
20111  }
20112 
20113  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
20114  std::vector<IkReal> solvalues(GetNumJoints());
20115  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
20116  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
20117  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
20118  std::vector<IkReal> vsolfree(sol.GetFree().size());
20119  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
20120  for( std::size_t j = 0; j < solvalues.size(); ++j)
20121  printf("%.15f, ", solvalues[j]);
20122  printf("\n");
20123  }
20124  return 0;
20125 }
20126 
20127 #endif
IKfmod
float IKfmod(float x, float y)
Definition: prbt_manipulator_ikfast_solver.cpp:141
ikfast
Definition: ikfast.h:45
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKsign
float IKsign(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:229
IKatan2
float IKatan2(float fy, float fx)
Definition: prbt_manipulator_ikfast_solver.cpp:183
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: prbt_manipulator_ikfast_solver.cpp:20076
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: prbt_manipulator_ikfast_solver.cpp:20074
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)
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
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: prbt_manipulator_ikfast_solver.cpp:370
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: prbt_manipulator_ikfast_solver.cpp:20064
IKsqrt
float IKsqrt(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:178
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: prbt_manipulator_ikfast_solver.cpp:303
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:121
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: prbt_manipulator_ikfast_solver.cpp:877
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: prbt_manipulator_ikfast_solver.cpp:180
GetIkType
IKFAST_API int GetIkType()
Definition: prbt_manipulator_ikfast_solver.cpp:374
IKasin
float IKasin(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:125
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: prbt_manipulator_ikfast_solver.cpp:53
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
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: prbt_manipulator_ikfast_solver.cpp:19887
IKabs
float IKabs(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:95
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: prbt_manipulator_ikfast_solver.cpp:368
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: prbt_manipulator_ikfast_solver.cpp:369
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: prbt_manipulator_ikfast_solver.cpp:111
IKsin
float IKsin(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:172
IKPI
#define IKPI
Definition: prbt_manipulator_ikfast_solver.cpp:64
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: prbt_manipulator_ikfast_solver.cpp:215
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:116
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: prbt_manipulator_ikfast_solver.cpp:383
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: prbt_manipulator_ikfast_solver.cpp:19981
IK2PI
#define IK2PI
Definition: prbt_manipulator_ikfast_solver.cpp:63
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: prbt_manipulator_ikfast_solver.cpp:29
step
unsigned int step
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKlog
float IKlog(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:101
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)
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: prbt_manipulator_ikfast_solver.cpp:250
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:106
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: prbt_manipulator_ikfast_solver.cpp:20069
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: prbt_manipulator_ikfast_solver.cpp:372
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: prbt_manipulator_ikfast_solver.cpp:19965
IKacos
float IKacos(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:158
IKcos
float IKcos(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:174
x
double x
IKsqr
float IKsqr(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:98
main
int main(int argc, char **argv)
Definition: prbt_manipulator_ikfast_solver.cpp:20088
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
IKtan
float IKtan(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:176
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
IKPI_2
#define IKPI_2
Definition: prbt_manipulator_ikfast_solver.cpp:65
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)

prbt_ikfast_manipulator_plugin
Author(s):
autogenerated on Tue Feb 2 2021 03:50:30