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 }
64