kr6_r900_workspace_manipulator_ikfast_solver.hpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
22 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
23 using namespace ikfast;
24 
25 // check if the included ikfast version matches what this file was compiled with
26 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
27 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a);
28 
29 #include <cmath>
30 #include <vector>
31 #include <limits>
32 #include <algorithm>
33 #include <complex>
34 
35 #ifndef IKFAST_ASSERT
36 #include <stdexcept>
37 #include <sstream>
38 #include <iostream>
39 
40 #ifdef _MSC_VER
41 #ifndef __PRETTY_FUNCTION__
42 #define __PRETTY_FUNCTION__ __FUNCDNAME__
43 #endif
44 #endif
45 
46 #ifndef __PRETTY_FUNCTION__
47 #define __PRETTY_FUNCTION__ __func__
48 #endif
49 
50 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
51 
52 #endif
53 
54 #if defined(_MSC_VER)
55 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
56 #else
57 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
58 #endif
59 
60 #define IK2PI ((IkReal)6.28318530717959)
61 #define IKPI ((IkReal)3.14159265358979)
62 #define IKPI_2 ((IkReal)1.57079632679490)
63 
64 #ifdef _MSC_VER
65 #ifndef isnan
66 #define isnan _isnan
67 #endif
68 #ifndef isinf
69 #define isinf _isinf
70 #endif
71 //#ifndef isfinite
72 //#define isfinite _isfinite
73 //#endif
74 #endif // _MSC_VER
75 
76 // lapack routines
77 extern "C" {
78  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
79  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
80  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
81  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
82  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
83  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
84 }
85 
86 using namespace std; // necessary to get std math routines
87 
88 #ifdef IKFAST_NAMESPACE
89 namespace IKFAST_NAMESPACE {
90 #endif
91 
92 inline float IKabs(float f) { return fabsf(f); }
93 inline double IKabs(double f) { return fabs(f); }
94 
95 inline float IKsqr(float f) { return f*f; }
96 inline double IKsqr(double f) { return f*f; }
97 
98 inline float IKlog(float f) { return logf(f); }
99 inline double IKlog(double f) { return log(f); }
100 
101 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
102 #ifndef IKFAST_SINCOS_THRESH
103 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
104 #endif
105 
106 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
107 #ifndef IKFAST_ATAN2_MAGTHRESH
108 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
109 #endif
110 
111 // minimum distance of separate solutions
112 #ifndef IKFAST_SOLUTION_THRESH
113 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
114 #endif
115 
116 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
117 #ifndef IKFAST_EVALCOND_THRESH
118 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
119 #endif
120 
121 
122 inline float IKasin(float f)
123 {
124 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
125 if( f <= -1 ) return float(-IKPI_2);
126 else if( f >= 1 ) return float(IKPI_2);
127 return asinf(f);
128 }
129 inline double IKasin(double f)
130 {
131 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
132 if( f <= -1 ) return -IKPI_2;
133 else if( f >= 1 ) return IKPI_2;
134 return asin(f);
135 }
136 
137 // return positive value in [0,y)
138 inline float IKfmod(float x, float y)
139 {
140  while(x < 0) {
141  x += y;
142  }
143  return fmodf(x,y);
144 }
145 
146 // return positive value in [0,y)
147 inline double IKfmod(double x, double y)
148 {
149  while(x < 0) {
150  x += y;
151  }
152  return fmod(x,y);
153 }
154 
155 inline float IKacos(float f)
156 {
157 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
158 if( f <= -1 ) return float(IKPI);
159 else if( f >= 1 ) return float(0);
160 return acosf(f);
161 }
162 inline double IKacos(double f)
163 {
164 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
165 if( f <= -1 ) return IKPI;
166 else if( f >= 1 ) return 0;
167 return acos(f);
168 }
169 inline float IKsin(float f) { return sinf(f); }
170 inline double IKsin(double f) { return sin(f); }
171 inline float IKcos(float f) { return cosf(f); }
172 inline double IKcos(double f) { return cos(f); }
173 inline float IKtan(float f) { return tanf(f); }
174 inline double IKtan(double f) { return tan(f); }
175 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
176 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
177 inline float IKatan2Simple(float fy, float fx) {
178  return atan2f(fy,fx);
179 }
180 inline float IKatan2(float fy, float fx) {
181  if( isnan(fy) ) {
182  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
183  return float(IKPI_2);
184  }
185  else if( isnan(fx) ) {
186  return 0;
187  }
188  return atan2f(fy,fx);
189 }
190 inline double IKatan2Simple(double fy, double fx) {
191  return atan2(fy,fx);
192 }
193 inline double IKatan2(double fy, double fx) {
194  if( isnan(fy) ) {
195  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
196  return IKPI_2;
197  }
198  else if( isnan(fx) ) {
199  return 0;
200  }
201  return atan2(fy,fx);
202 }
203 
204 template <typename T>
205 struct CheckValue
206 {
207  T value;
208  bool valid;
209 };
210 
211 template <typename T>
212 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
213 {
214  CheckValue<T> ret;
215  ret.valid = false;
216  ret.value = 0;
217  if( !isnan(fy) && !isnan(fx) ) {
218  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
219  ret.value = IKatan2Simple(fy,fx);
220  ret.valid = true;
221  }
222  }
223  return ret;
224 }
225 
226 inline float IKsign(float f) {
227  if( f > 0 ) {
228  return float(1);
229  }
230  else if( f < 0 ) {
231  return float(-1);
232  }
233  return 0;
234 }
235 
236 inline double IKsign(double f) {
237  if( f > 0 ) {
238  return 1.0;
239  }
240  else if( f < 0 ) {
241  return -1.0;
242  }
243  return 0;
244 }
245 
246 template <typename T>
247 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
248 {
249  CheckValue<T> ret;
250  ret.valid = true;
251  if( n == 0 ) {
252  ret.value = 1.0;
253  return ret;
254  }
255  else if( n == 1 )
256  {
257  ret.value = f;
258  return ret;
259  }
260  else if( n < 0 )
261  {
262  if( f == 0 )
263  {
264  ret.valid = false;
265  ret.value = (T)1.0e30;
266  return ret;
267  }
268  if( n == -1 ) {
269  ret.value = T(1.0)/f;
270  return ret;
271  }
272  }
273 
274  int num = n > 0 ? n : -n;
275  if( num == 2 ) {
276  ret.value = f*f;
277  }
278  else if( num == 3 ) {
279  ret.value = f*f*f;
280  }
281  else {
282  ret.value = 1.0;
283  while(num>0) {
284  if( num & 1 ) {
285  ret.value *= f;
286  }
287  num >>= 1;
288  f *= f;
289  }
290  }
291 
292  if( n < 0 ) {
293  ret.value = T(1.0)/ret.value;
294  }
295  return ret;
296 }
297 
300 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
301 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45;
302 x0=IKcos(j[0]);
303 x1=IKcos(j[1]);
304 x2=IKcos(j[2]);
305 x3=IKsin(j[1]);
306 x4=IKsin(j[2]);
307 x5=IKsin(j[0]);
308 x6=IKsin(j[3]);
309 x7=IKcos(j[3]);
310 x8=IKcos(j[5]);
311 x9=IKsin(j[5]);
312 x10=IKcos(j[4]);
313 x11=IKsin(j[4]);
314 x12=((0.42)*x5);
315 x13=((1.0)*x7);
316 x14=((0.035)*x5);
317 x15=((1.0)*x5);
318 x16=((0.035)*x0);
319 x17=((1.0)*x0);
320 x18=((0.42)*x0);
321 x19=((0.455)*x1);
322 x20=((0.08)*x5);
323 x21=((0.08)*x0);
324 x22=((0.08)*x7);
325 x23=(x2*x3);
326 x24=(x0*x6);
327 x25=(x3*x4);
328 x26=(x1*x2);
329 x27=(x1*x4);
330 x28=((-1.0)*x6);
331 x29=(x5*x7);
332 x30=(x11*x7);
333 x31=(x15*x6);
334 x32=(x0*x13);
335 x33=((1.0)*x26);
336 x34=(x15*x25);
337 x35=((((-1.0)*x33))+x25);
338 x36=(x35*x7);
339 x37=(((x0*x26))+(((-1.0)*x17*x25)));
340 x38=(x17*(((((-1.0)*x23))+(((-1.0)*x27)))));
341 x39=(x15*(((((-1.0)*x23))+(((-1.0)*x27)))));
342 x40=(x11*(((((-1.0)*x34))+((x26*x5)))));
343 x41=(x38*x6);
344 x42=(x39*x7);
345 x43=((((-1.0)*x17*x6))+(((-1.0)*x13*x39)));
346 x44=(((x10*x36))+((x11*((x27+x23)))));
347 x45=(((x10*(((((-1.0)*x31))+(((1.0)*x13*x38))))))+(((-1.0)*x11*x37)));
348 eerot[0]=(((x9*(((((-1.0)*x29))+((x28*x38))))))+((x45*x8)));
349 eerot[1]=(((x8*((x41+x29))))+((x45*x9)));
350 eerot[2]=(((x10*x37))+((x11*(((((-1.0)*x31))+((x38*x7)))))));
351 IkReal x46=((1.0)*x25);
352 eetrans[0]=(((x10*((((x21*x26))+(((-1.0)*x21*x46))))))+((x18*x26))+(((0.025)*x0))+((x11*((((x22*x38))+(((-1.0)*x20*x6))))))+((x0*x19))+(((-1.0)*x18*x46))+((x16*x27))+((x16*x23)));
353 eerot[3]=(((x8*((x40+((x10*(((((-1.0)*x42))+(((-1.0)*x24))))))))))+((x9*(((((-1.0)*x32))+((x39*x6)))))));
354 eerot[4]=(((x8*((((x28*x39))+x32))))+((x9*((((x10*x43))+x40)))));
355 eerot[5]=(((x11*x43))+((x10*(((((-1.0)*x15*x26))+x34)))));
356 IkReal x47=((1.0)*x26);
357 IkReal x48=((1.0)*x14);
358 eetrans[1]=((((-1.0)*x23*x48))+(((-1.0)*x19*x5))+(((-0.025)*x5))+(((-1.0)*x27*x48))+((x11*(((((-1.0)*x21*x6))+(((-1.0)*x22*x39))))))+((x12*x25))+(((-1.0)*x12*x47))+((x10*(((((-1.0)*x20*x47))+((x20*x25)))))));
359 eerot[6]=(((x6*x9*(((((-1.0)*x25))+x33))))+((x44*x8)));
360 eerot[7]=(((x44*x9))+((x35*x6*x8)));
361 eerot[8]=(((x10*(((((-1.0)*x23))+(((-1.0)*x27))))))+((x30*x35)));
362 eetrans[2]=((0.4)+((x10*(((((-0.08)*x23))+(((-0.08)*x27))))))+((x30*(((((-0.08)*x26))+(((0.08)*x25))))))+(((-0.035)*x25))+(((-0.455)*x3))+(((-0.42)*x23))+(((-0.42)*x27))+(((0.035)*x26)));
363 }
364 
365 IKFAST_API int GetNumFreeParameters() { return 0; }
366 IKFAST_API int* GetFreeParameters() { return NULL; }
367 IKFAST_API int GetNumJoints() { return 6; }
368 
369 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
370 
371 IKFAST_API int GetIkType() { return 0x67000001; }
372 
373 class IKSolver {
374 public:
375 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;
376 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
377 
378 IkReal j100, cj100, sj100;
379 unsigned char _ij100[2], _nj100;
380 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
381 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;
382 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
383  solutions.Clear();
384 r00 = eerot[0*3+0];
385 r01 = eerot[0*3+1];
386 r02 = eerot[0*3+2];
387 r10 = eerot[1*3+0];
388 r11 = eerot[1*3+1];
389 r12 = eerot[1*3+2];
390 r20 = eerot[2*3+0];
391 r21 = eerot[2*3+1];
392 r22 = eerot[2*3+2];
393 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
394 
395 new_r00=((-1.0)*r00);
396 new_r01=r01;
397 new_r02=((-1.0)*r02);
398 new_px=((((-0.08)*r02))+px);
399 new_r10=r10;
400 new_r11=((-1.0)*r11);
401 new_r12=r12;
402 new_py=((((-1.0)*py))+(((0.08)*r12)));
403 new_r20=r20;
404 new_r21=((-1.0)*r21);
405 new_r22=r22;
406 new_pz=((0.4)+(((-1.0)*pz))+(((0.08)*r22)));
407 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;
408 IkReal x49=((1.0)*px);
409 IkReal x50=((1.0)*pz);
410 IkReal x51=((1.0)*py);
411 pp=((px*px)+(py*py)+(pz*pz));
412 npx=(((px*r00))+((py*r10))+((pz*r20)));
413 npy=(((px*r01))+((py*r11))+((pz*r21)));
414 npz=(((px*r02))+((py*r12))+((pz*r22)));
415 rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
416 rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
417 rxp0_2=((((-1.0)*r10*x49))+((py*r00)));
418 rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
419 rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
420 rxp1_2=((((-1.0)*r11*x49))+((py*r01)));
421 rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
422 rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
423 rxp2_2=((((-1.0)*r12*x49))+((py*r02)));
424 {
425 IkReal j0eval[1];
426 j0eval[0]=((IKabs(px))+(IKabs(py)));
427 if( IKabs(j0eval[0]) < 0.0000010000000000 )
428 {
429 continue; // no branches [j0, j1, j2]
430 
431 } else
432 {
433 {
434 IkReal j0array[2], cj0array[2], sj0array[2];
435 bool j0valid[2]={false};
436 _nj0 = 2;
437 CheckValue<IkReal> x53 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
438 if(!x53.valid){
439 continue;
440 }
441 IkReal x52=x53.value;
442 j0array[0]=((-1.0)*x52);
443 sj0array[0]=IKsin(j0array[0]);
444 cj0array[0]=IKcos(j0array[0]);
445 j0array[1]=((3.14159265358979)+(((-1.0)*x52)));
446 sj0array[1]=IKsin(j0array[1]);
447 cj0array[1]=IKcos(j0array[1]);
448 if( j0array[0] > IKPI )
449 {
450  j0array[0]-=IK2PI;
451 }
452 else if( j0array[0] < -IKPI )
453 { j0array[0]+=IK2PI;
454 }
455 j0valid[0] = true;
456 if( j0array[1] > IKPI )
457 {
458  j0array[1]-=IK2PI;
459 }
460 else if( j0array[1] < -IKPI )
461 { j0array[1]+=IK2PI;
462 }
463 j0valid[1] = true;
464 for(int ij0 = 0; ij0 < 2; ++ij0)
465 {
466 if( !j0valid[ij0] )
467 {
468  continue;
469 }
470 _ij0[0] = ij0; _ij0[1] = -1;
471 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
472 {
473 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
474 {
475  j0valid[iij0]=false; _ij0[1] = iij0; break;
476 }
477 }
478 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
479 
480 {
481 IkReal j2array[2], cj2array[2], sj2array[2];
482 bool j2valid[2]={false};
483 _nj2 = 2;
484 if( (((1.00130425120353)+(((0.130369670100063)*py*sj0))+(((0.130369670100063)*cj0*px))+(((-2.60739340200126)*pp)))) < -1-IKFAST_SINCOS_THRESH || (((1.00130425120353)+(((0.130369670100063)*py*sj0))+(((0.130369670100063)*cj0*px))+(((-2.60739340200126)*pp)))) > 1+IKFAST_SINCOS_THRESH )
485  continue;
486 IkReal x54=IKasin(((1.00130425120353)+(((0.130369670100063)*py*sj0))+(((0.130369670100063)*cj0*px))+(((-2.60739340200126)*pp))));
487 j2array[0]=((-1.48765509490646)+(((-1.0)*x54)));
488 sj2array[0]=IKsin(j2array[0]);
489 cj2array[0]=IKcos(j2array[0]);
490 j2array[1]=((1.65393755868334)+x54);
491 sj2array[1]=IKsin(j2array[1]);
492 cj2array[1]=IKcos(j2array[1]);
493 if( j2array[0] > IKPI )
494 {
495  j2array[0]-=IK2PI;
496 }
497 else if( j2array[0] < -IKPI )
498 { j2array[0]+=IK2PI;
499 }
500 j2valid[0] = true;
501 if( j2array[1] > IKPI )
502 {
503  j2array[1]-=IK2PI;
504 }
505 else if( j2array[1] < -IKPI )
506 { j2array[1]+=IK2PI;
507 }
508 j2valid[1] = true;
509 for(int ij2 = 0; ij2 < 2; ++ij2)
510 {
511 if( !j2valid[ij2] )
512 {
513  continue;
514 }
515 _ij2[0] = ij2; _ij2[1] = -1;
516 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
517 {
518 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
519 {
520  j2valid[iij2]=false; _ij2[1] = iij2; break;
521 }
522 }
523 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
524 
525 {
526 IkReal j1eval[3];
527 IkReal x55=cj2*cj2;
528 IkReal x56=(py*sj0);
529 IkReal x57=((40.0)*cj2);
530 IkReal x58=(cj0*px);
531 IkReal x59=((480.0)*sj2);
532 IkReal x60=((0.42)*sj2);
533 IkReal x61=(cj2*sj2);
534 IkReal x62=(cj2*pz);
535 IkReal x63=((0.035)*cj2);
536 IkReal x64=(pz*sj2);
537 j1eval[0]=((((-12.0)*sj2))+(((-1.0)*x56*x57))+cj2+((x58*x59))+(((480.0)*x62))+(((520.0)*pz))+((x56*x59))+(((-1.0)*x57*x58))+(((40.0)*x64)));
538 j1eval[1]=IKsign(((((-1.0)*x56*x63))+(((0.42)*x62))+(((0.035)*x64))+(((0.455)*pz))+((x58*x60))+(((-1.0)*x58*x63))+((x56*x60))+(((-0.0105)*sj2))+(((0.000875)*cj2))));
539 j1eval[2]=((IKabs(((-0.1764)+(pz*pz)+(((0.0294)*x61))+(((0.175175)*x55)))))+(IKabs(((0.0147)+(((0.1911)*sj2))+(((0.175175)*x61))+(((-0.0294)*x55))+((pz*x56))+((pz*x58))+(((-0.015925)*cj2))+(((-0.025)*pz))))));
540 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
541 {
542 {
543 IkReal j1eval[2];
544 IkReal x65=(py*sj0);
545 IkReal x66=((0.035)*sj2);
546 IkReal x67=(cj0*px);
547 IkReal x68=((0.42)*cj2);
548 IkReal x69=((40.0)*sj2);
549 IkReal x70=(cj2*pz);
550 IkReal x71=(pz*sj2);
551 IkReal x72=((480.0)*cj2);
552 j1eval[0]=((13.0)+(((12.0)*cj2))+sj2+(((-1.0)*x67*x69))+(((-40.0)*x70))+(((-1.0)*x67*x72))+(((-1.0)*x65*x69))+(((480.0)*x71))+(((-520.0)*x65))+(((-520.0)*x67))+(((-1.0)*x65*x72)));
553 j1eval[1]=IKsign(((0.011375)+(((-1.0)*x66*x67))+(((-1.0)*x67*x68))+(((-0.035)*x70))+(((0.42)*x71))+(((0.0105)*cj2))+(((0.000875)*sj2))+(((-1.0)*x65*x66))+(((-1.0)*x65*x68))+(((-0.455)*x67))+(((-0.455)*x65))));
554 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
555 {
556 {
557 IkReal j1eval[2];
558 IkReal x73=cj0*cj0;
559 IkReal x74=py*py;
560 IkReal x75=px*px;
561 IkReal x76=pz*pz;
562 IkReal x77=(cj0*px);
563 IkReal x78=(py*sj0);
564 IkReal x79=((1600.0)*x74);
565 IkReal x80=(x73*x75);
566 j1eval[0]=((-1.0)+(((-1.0)*x79))+(((-1600.0)*x80))+(((-3200.0)*x77*x78))+(((80.0)*x77))+(((80.0)*x78))+((x73*x79))+(((-1600.0)*x76)));
567 j1eval[1]=IKsign(((-0.000625)+(((-2.0)*x77*x78))+(((-1.0)*x80))+(((-1.0)*x74))+(((-1.0)*x76))+(((0.05)*x77))+(((0.05)*x78))+((x73*x74))));
568 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
569 {
570 continue; // no branches [j1]
571 
572 } else
573 {
574 {
575 IkReal j1array[1], cj1array[1], sj1array[1];
576 bool j1valid[1]={false};
577 _nj1 = 1;
578 IkReal x81=py*py;
579 IkReal x82=cj0*cj0;
580 IkReal x83=(py*sj0);
581 IkReal x84=(cj0*px);
582 IkReal x85=((0.42)*cj2);
583 IkReal x86=((0.035)*sj2);
584 IkReal x87=((0.42)*sj2);
585 IkReal x88=((0.035)*cj2);
586 CheckValue<IkReal> x89 = IKatan2WithCheck(IkReal((((x84*x87))+(((-1.0)*x83*x88))+(((-0.455)*pz))+(((-1.0)*x84*x88))+((x83*x87))+(((-0.0105)*sj2))+(((-1.0)*pz*x86))+(((-1.0)*pz*x85))+(((0.000875)*cj2)))),IkReal(((0.011375)+(((-1.0)*x83*x85))+(((-1.0)*x83*x86))+(((-0.455)*x84))+(((-0.455)*x83))+((pz*x88))+(((-1.0)*x84*x86))+(((-1.0)*x84*x85))+(((0.0105)*cj2))+(((0.000875)*sj2))+(((-1.0)*pz*x87)))),IKFAST_ATAN2_MAGTHRESH);
587 if(!x89.valid){
588 continue;
589 }
590 CheckValue<IkReal> x90=IKPowWithIntegerCheck(IKsign(((-0.000625)+(((0.05)*x83))+(((0.05)*x84))+((x81*x82))+(((-2.0)*x83*x84))+(((-1.0)*x81))+(((-1.0)*(pz*pz)))+(((-1.0)*x82*(px*px))))),-1);
591 if(!x90.valid){
592 continue;
593 }
594 j1array[0]=((-1.5707963267949)+(x89.value)+(((1.5707963267949)*(x90.value))));
595 sj1array[0]=IKsin(j1array[0]);
596 cj1array[0]=IKcos(j1array[0]);
597 if( j1array[0] > IKPI )
598 {
599  j1array[0]-=IK2PI;
600 }
601 else if( j1array[0] < -IKPI )
602 { j1array[0]+=IK2PI;
603 }
604 j1valid[0] = true;
605 for(int ij1 = 0; ij1 < 1; ++ij1)
606 {
607 if( !j1valid[ij1] )
608 {
609  continue;
610 }
611 _ij1[0] = ij1; _ij1[1] = -1;
612 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
613 {
614 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
615 {
616  j1valid[iij1]=false; _ij1[1] = iij1; break;
617 }
618 }
619 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
620 {
621 IkReal evalcond[5];
622 IkReal x91=IKsin(j1);
623 IkReal x92=IKcos(j1);
624 IkReal x93=(cj0*px);
625 IkReal x94=((0.035)*sj2);
626 IkReal x95=(py*sj0);
627 IkReal x96=((0.42)*sj2);
628 IkReal x97=((1.0)*pz);
629 IkReal x98=((0.035)*cj2);
630 IkReal x99=((0.42)*x91);
631 IkReal x100=((1.0)*x92);
632 IkReal x101=((0.91)*x92);
633 IkReal x102=(cj2*x92);
634 evalcond[0]=((((-0.025)*x91))+x96+(((-1.0)*x92*x97))+(((-1.0)*x98))+((x91*x93))+((x91*x95)));
635 evalcond[1]=((0.455)+(((0.025)*x92))+(((-1.0)*x91*x97))+x94+(((0.42)*cj2))+(((-1.0)*x100*x95))+(((-1.0)*x100*x93)));
636 evalcond[2]=((((0.455)*x91))+((x92*x96))+((cj2*x99))+(((-1.0)*x92*x98))+(((-1.0)*x97))+((x91*x94)));
637 evalcond[3]=((-0.030025)+(((0.05)*x95))+(((0.05)*x93))+((x101*x95))+((x101*x93))+(((-0.02275)*x92))+(((-1.0)*pp))+(((0.91)*pz*x91)));
638 evalcond[4]=((0.025)+(((0.455)*x92))+((x92*x94))+(((-1.0)*x91*x96))+(((-1.0)*x95))+(((-1.0)*x93))+(((0.42)*x102))+((x91*x98)));
639 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
640 {
641 continue;
642 }
643 }
644 
645 rotationfunction0(solutions);
646 }
647 }
648 
649 }
650 
651 }
652 
653 } else
654 {
655 {
656 IkReal j1array[1], cj1array[1], sj1array[1];
657 bool j1valid[1]={false};
658 _nj1 = 1;
659 IkReal x605=cj2*cj2;
660 IkReal x606=(py*sj0);
661 IkReal x607=(cj0*px);
662 IkReal x608=((0.42)*cj2);
663 IkReal x609=(cj2*sj2);
664 IkReal x610=((0.035)*sj2);
665 IkReal x611=((1.0)*pz);
666 CheckValue<IkReal> x612=IKPowWithIntegerCheck(IKsign(((0.011375)+(((-0.035)*cj2*pz))+(((0.42)*pz*sj2))+(((-0.455)*x606))+(((-0.455)*x607))+(((-1.0)*x606*x610))+(((-1.0)*x607*x610))+(((0.0105)*cj2))+(((0.000875)*sj2))+(((-1.0)*x606*x608))+(((-1.0)*x607*x608)))),-1);
667 if(!x612.valid){
668 continue;
669 }
670 CheckValue<IkReal> x613 = IKatan2WithCheck(IkReal(((0.0147)+(((0.1911)*sj2))+(((0.025)*pz))+(((-1.0)*x606*x611))+(((-1.0)*x607*x611))+(((0.175175)*x609))+(((-0.015925)*cj2))+(((-0.0294)*x605)))),IkReal(((-0.20825)+(((-0.3822)*cj2))+(((-0.03185)*sj2))+(pz*pz)+(((-0.175175)*x605))+(((-0.0294)*x609)))),IKFAST_ATAN2_MAGTHRESH);
671 if(!x613.valid){
672 continue;
673 }
674 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x612.value)))+(x613.value));
675 sj1array[0]=IKsin(j1array[0]);
676 cj1array[0]=IKcos(j1array[0]);
677 if( j1array[0] > IKPI )
678 {
679  j1array[0]-=IK2PI;
680 }
681 else if( j1array[0] < -IKPI )
682 { j1array[0]+=IK2PI;
683 }
684 j1valid[0] = true;
685 for(int ij1 = 0; ij1 < 1; ++ij1)
686 {
687 if( !j1valid[ij1] )
688 {
689  continue;
690 }
691 _ij1[0] = ij1; _ij1[1] = -1;
692 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
693 {
694 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
695 {
696  j1valid[iij1]=false; _ij1[1] = iij1; break;
697 }
698 }
699 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
700 {
701 IkReal evalcond[5];
702 IkReal x614=IKsin(j1);
703 IkReal x615=IKcos(j1);
704 IkReal x616=(cj0*px);
705 IkReal x617=((0.035)*sj2);
706 IkReal x618=(py*sj0);
707 IkReal x619=((0.42)*sj2);
708 IkReal x620=((1.0)*pz);
709 IkReal x621=((0.035)*cj2);
710 IkReal x622=((0.42)*x614);
711 IkReal x623=((1.0)*x615);
712 IkReal x624=((0.91)*x615);
713 IkReal x625=(cj2*x615);
714 evalcond[0]=((((-1.0)*x621))+(((-0.025)*x614))+((x614*x618))+((x614*x616))+x619+(((-1.0)*x615*x620)));
715 evalcond[1]=((0.455)+(((0.025)*x615))+(((-1.0)*x614*x620))+x617+(((0.42)*cj2))+(((-1.0)*x618*x623))+(((-1.0)*x616*x623)));
716 evalcond[2]=((((0.455)*x614))+(((-1.0)*x620))+((x614*x617))+((x615*x619))+(((-1.0)*x615*x621))+((cj2*x622)));
717 evalcond[3]=((-0.030025)+(((-0.02275)*x615))+((x616*x624))+((x618*x624))+(((-1.0)*pp))+(((0.05)*x618))+(((0.05)*x616))+(((0.91)*pz*x614)));
718 evalcond[4]=((0.025)+(((0.42)*x625))+(((-1.0)*x614*x619))+((x614*x621))+(((0.455)*x615))+(((-1.0)*x616))+(((-1.0)*x618))+((x615*x617)));
719 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
720 {
721 continue;
722 }
723 }
724 
725 rotationfunction0(solutions);
726 }
727 }
728 
729 }
730 
731 }
732 
733 } else
734 {
735 {
736 IkReal j1array[1], cj1array[1], sj1array[1];
737 bool j1valid[1]={false};
738 _nj1 = 1;
739 IkReal x626=cj2*cj2;
740 IkReal x627=(py*sj0);
741 IkReal x628=((0.42)*sj2);
742 IkReal x629=(cj0*px);
743 IkReal x630=((0.035)*cj2);
744 IkReal x631=(cj2*sj2);
745 CheckValue<IkReal> x632 = IKatan2WithCheck(IkReal(((-0.1764)+(((0.175175)*x626))+(pz*pz)+(((0.0294)*x631)))),IkReal(((0.0147)+(((0.1911)*sj2))+((pz*x627))+((pz*x629))+(((0.175175)*x631))+(((-0.0294)*x626))+(((-0.015925)*cj2))+(((-0.025)*pz)))),IKFAST_ATAN2_MAGTHRESH);
746 if(!x632.valid){
747 continue;
748 }
749 CheckValue<IkReal> x633=IKPowWithIntegerCheck(IKsign(((((-1.0)*x629*x630))+(((0.455)*pz))+((x628*x629))+(((-1.0)*x627*x630))+(((0.42)*cj2*pz))+(((-0.0105)*sj2))+(((0.035)*pz*sj2))+((x627*x628))+(((0.000875)*cj2)))),-1);
750 if(!x633.valid){
751 continue;
752 }
753 j1array[0]=((-1.5707963267949)+(x632.value)+(((1.5707963267949)*(x633.value))));
754 sj1array[0]=IKsin(j1array[0]);
755 cj1array[0]=IKcos(j1array[0]);
756 if( j1array[0] > IKPI )
757 {
758  j1array[0]-=IK2PI;
759 }
760 else if( j1array[0] < -IKPI )
761 { j1array[0]+=IK2PI;
762 }
763 j1valid[0] = true;
764 for(int ij1 = 0; ij1 < 1; ++ij1)
765 {
766 if( !j1valid[ij1] )
767 {
768  continue;
769 }
770 _ij1[0] = ij1; _ij1[1] = -1;
771 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
772 {
773 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
774 {
775  j1valid[iij1]=false; _ij1[1] = iij1; break;
776 }
777 }
778 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
779 {
780 IkReal evalcond[5];
781 IkReal x634=IKsin(j1);
782 IkReal x635=IKcos(j1);
783 IkReal x636=(cj0*px);
784 IkReal x637=((0.035)*sj2);
785 IkReal x638=(py*sj0);
786 IkReal x639=((0.42)*sj2);
787 IkReal x640=((1.0)*pz);
788 IkReal x641=((0.035)*cj2);
789 IkReal x642=((0.42)*x634);
790 IkReal x643=((1.0)*x635);
791 IkReal x644=((0.91)*x635);
792 IkReal x645=(cj2*x635);
793 evalcond[0]=(((x634*x636))+((x634*x638))+(((-0.025)*x634))+(((-1.0)*x641))+(((-1.0)*x635*x640))+x639);
794 evalcond[1]=((0.455)+(((0.025)*x635))+x637+(((-1.0)*x636*x643))+(((0.42)*cj2))+(((-1.0)*x634*x640))+(((-1.0)*x638*x643)));
795 evalcond[2]=(((x634*x637))+((x635*x639))+(((-1.0)*x640))+(((-1.0)*x635*x641))+((cj2*x642))+(((0.455)*x634)));
796 evalcond[3]=((-0.030025)+((x638*x644))+(((0.05)*x636))+(((0.05)*x638))+((x636*x644))+(((0.91)*pz*x634))+(((-1.0)*pp))+(((-0.02275)*x635)));
797 evalcond[4]=((0.025)+((x635*x637))+(((-1.0)*x634*x639))+((x634*x641))+(((0.42)*x645))+(((0.455)*x635))+(((-1.0)*x638))+(((-1.0)*x636)));
798 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
799 {
800 continue;
801 }
802 }
803 
804 rotationfunction0(solutions);
805 }
806 }
807 
808 }
809 
810 }
811 }
812 }
813 }
814 }
815 
816 }
817 
818 }
819 }
820 return solutions.GetNumSolutions()>0;
821 }
822 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
823 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
824 IkReal x103=((1.0)*cj0);
825 IkReal x104=(r11*sj0);
826 IkReal x105=(r10*sj0);
827 IkReal x106=((1.0)*cj2);
828 IkReal x107=(cj1*sj2);
829 IkReal x108=(r12*sj0);
830 IkReal x109=(((cj2*sj1))+x107);
831 IkReal x110=((((-1.0)*cj1*x106))+((sj1*sj2)));
832 IkReal x111=(sj0*x110);
833 IkReal x112=(cj0*x109);
834 IkReal x113=(cj0*x110);
835 IkReal x114=((((-1.0)*x107))+(((-1.0)*sj1*x106)));
836 new_r00=(((r20*x110))+((r00*x112))+((x105*x109)));
837 new_r01=(((r21*x110))+((r01*x112))+((x104*x109)));
838 new_r02=(((r22*x110))+((r02*x112))+((x108*x109)));
839 new_r10=(((r00*sj0))+(((-1.0)*r10*x103)));
840 new_r11=((((-1.0)*r11*x103))+((r01*sj0)));
841 new_r12=((((-1.0)*r12*x103))+((r02*sj0)));
842 new_r20=(((r20*x114))+((r00*x113))+((x105*x110)));
843 new_r21=(((r21*x114))+((r01*x113))+((x104*x110)));
844 new_r22=(((r22*x114))+((r02*x113))+((x108*x110)));
845 {
846 IkReal j4array[2], cj4array[2], sj4array[2];
847 bool j4valid[2]={false};
848 _nj4 = 2;
849 cj4array[0]=new_r22;
850 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
851 {
852  j4valid[0] = j4valid[1] = true;
853  j4array[0] = IKacos(cj4array[0]);
854  sj4array[0] = IKsin(j4array[0]);
855  cj4array[1] = cj4array[0];
856  j4array[1] = -j4array[0];
857  sj4array[1] = -sj4array[0];
858 }
859 else if( isnan(cj4array[0]) )
860 {
861  // probably any value will work
862  j4valid[0] = true;
863  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
864 }
865 for(int ij4 = 0; ij4 < 2; ++ij4)
866 {
867 if( !j4valid[ij4] )
868 {
869  continue;
870 }
871 _ij4[0] = ij4; _ij4[1] = -1;
872 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
873 {
874 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
875 {
876  j4valid[iij4]=false; _ij4[1] = iij4; break;
877 }
878 }
879 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
880 
881 {
882 IkReal j3eval[3];
883 j3eval[0]=sj4;
884 j3eval[1]=IKsign(sj4);
885 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
886 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
887 {
888 {
889 IkReal j5eval[3];
890 j5eval[0]=sj4;
891 j5eval[1]=IKsign(sj4);
892 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
893 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
894 {
895 {
896 IkReal j3eval[2];
897 j3eval[0]=new_r12;
898 j3eval[1]=sj4;
899 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
900 {
901 {
902 IkReal evalcond[5];
903 bool bgotonextstatement = true;
904 do
905 {
906 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
907 evalcond[1]=new_r20;
908 evalcond[2]=new_r02;
909 evalcond[3]=new_r12;
910 evalcond[4]=new_r21;
911 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 )
912 {
913 bgotonextstatement=false;
914 IkReal j5mul = 1;
915 j5=0;
916 j3mul=-1.0;
917 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 )
918  continue;
919 j3=IKatan2(((-1.0)*new_r01), new_r00);
920 {
921 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
922 vinfos[0].jointtype = 1;
923 vinfos[0].foffset = j0;
924 vinfos[0].indices[0] = _ij0[0];
925 vinfos[0].indices[1] = _ij0[1];
926 vinfos[0].maxsolutions = _nj0;
927 vinfos[1].jointtype = 1;
928 vinfos[1].foffset = j1;
929 vinfos[1].indices[0] = _ij1[0];
930 vinfos[1].indices[1] = _ij1[1];
931 vinfos[1].maxsolutions = _nj1;
932 vinfos[2].jointtype = 1;
933 vinfos[2].foffset = j2;
934 vinfos[2].indices[0] = _ij2[0];
935 vinfos[2].indices[1] = _ij2[1];
936 vinfos[2].maxsolutions = _nj2;
937 vinfos[3].jointtype = 1;
938 vinfos[3].foffset = j3;
939 vinfos[3].fmul = j3mul;
940 vinfos[3].freeind = 0;
941 vinfos[3].maxsolutions = 0;
942 vinfos[4].jointtype = 1;
943 vinfos[4].foffset = j4;
944 vinfos[4].indices[0] = _ij4[0];
945 vinfos[4].indices[1] = _ij4[1];
946 vinfos[4].maxsolutions = _nj4;
947 vinfos[5].jointtype = 1;
948 vinfos[5].foffset = j5;
949 vinfos[5].fmul = j5mul;
950 vinfos[5].freeind = 0;
951 vinfos[5].maxsolutions = 0;
952 std::vector<int> vfree(1);
953 vfree[0] = 5;
954 solutions.AddSolution(vinfos,vfree);
955 }
956 
957 }
958 } while(0);
959 if( bgotonextstatement )
960 {
961 bool bgotonextstatement = true;
962 do
963 {
964 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
965 evalcond[1]=new_r20;
966 evalcond[2]=new_r02;
967 evalcond[3]=new_r12;
968 evalcond[4]=new_r21;
969 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 )
970 {
971 bgotonextstatement=false;
972 IkReal j5mul = 1;
973 j5=0;
974 j3mul=1.0;
975 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 )
976  continue;
977 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
978 {
979 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
980 vinfos[0].jointtype = 1;
981 vinfos[0].foffset = j0;
982 vinfos[0].indices[0] = _ij0[0];
983 vinfos[0].indices[1] = _ij0[1];
984 vinfos[0].maxsolutions = _nj0;
985 vinfos[1].jointtype = 1;
986 vinfos[1].foffset = j1;
987 vinfos[1].indices[0] = _ij1[0];
988 vinfos[1].indices[1] = _ij1[1];
989 vinfos[1].maxsolutions = _nj1;
990 vinfos[2].jointtype = 1;
991 vinfos[2].foffset = j2;
992 vinfos[2].indices[0] = _ij2[0];
993 vinfos[2].indices[1] = _ij2[1];
994 vinfos[2].maxsolutions = _nj2;
995 vinfos[3].jointtype = 1;
996 vinfos[3].foffset = j3;
997 vinfos[3].fmul = j3mul;
998 vinfos[3].freeind = 0;
999 vinfos[3].maxsolutions = 0;
1000 vinfos[4].jointtype = 1;
1001 vinfos[4].foffset = j4;
1002 vinfos[4].indices[0] = _ij4[0];
1003 vinfos[4].indices[1] = _ij4[1];
1004 vinfos[4].maxsolutions = _nj4;
1005 vinfos[5].jointtype = 1;
1006 vinfos[5].foffset = j5;
1007 vinfos[5].fmul = j5mul;
1008 vinfos[5].freeind = 0;
1009 vinfos[5].maxsolutions = 0;
1010 std::vector<int> vfree(1);
1011 vfree[0] = 5;
1012 solutions.AddSolution(vinfos,vfree);
1013 }
1014 
1015 }
1016 } while(0);
1017 if( bgotonextstatement )
1018 {
1019 bool bgotonextstatement = true;
1020 do
1021 {
1022 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
1023 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1024 {
1025 bgotonextstatement=false;
1026 {
1027 IkReal j3eval[1];
1028 new_r02=0;
1029 new_r12=0;
1030 new_r20=0;
1031 new_r21=0;
1032 IkReal x115=new_r22*new_r22;
1033 IkReal x116=((16.0)*new_r10);
1034 IkReal x117=((16.0)*new_r01);
1035 IkReal x118=((16.0)*new_r22);
1036 IkReal x119=((8.0)*new_r11);
1037 IkReal x120=((8.0)*new_r00);
1038 IkReal x121=(x115*x116);
1039 IkReal x122=(x115*x117);
1040 j3eval[0]=((IKabs((((new_r11*x118))+(((16.0)*new_r00))+(((-32.0)*new_r00*x115)))))+(IKabs(((((-1.0)*x121))+x116)))+(IKabs(((((-1.0)*new_r22*x120))+((x115*x119)))))+(IKabs(((((-1.0)*x122))+x117)))+(IKabs(((((-1.0)*x116))+x121)))+(IKabs(((((-1.0)*x117))+x122)))+(IKabs((((new_r22*x119))+(((-1.0)*x120)))))+(IKabs(((((32.0)*new_r11))+(((-1.0)*new_r00*x118))+(((-16.0)*new_r11*x115))))));
1041 if( IKabs(j3eval[0]) < 0.0000000100000000 )
1042 {
1043 continue; // no branches [j3, j5]
1044 
1045 } else
1046 {
1047 IkReal op[4+1], zeror[4];
1048 int numroots;
1049 IkReal j3evalpoly[1];
1050 IkReal x123=new_r22*new_r22;
1051 IkReal x124=((16.0)*new_r10);
1052 IkReal x125=(new_r11*new_r22);
1053 IkReal x126=(x123*x124);
1054 IkReal x127=((((-8.0)*new_r00))+(((8.0)*x125)));
1055 op[0]=x127;
1056 op[1]=((((-1.0)*x126))+x124);
1057 op[2]=((((16.0)*x125))+(((16.0)*new_r00))+(((-32.0)*new_r00*x123)));
1058 op[3]=((((-1.0)*x124))+x126);
1059 op[4]=x127;
1060 polyroots4(op,zeror,numroots);
1061 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1062 int numsolutions = 0;
1063 for(int ij3 = 0; ij3 < numroots; ++ij3)
1064 {
1065 IkReal htj3 = zeror[ij3];
1066 tempj3array[0]=((2.0)*(atan(htj3)));
1067 for(int kj3 = 0; kj3 < 1; ++kj3)
1068 {
1069 j3array[numsolutions] = tempj3array[kj3];
1070 if( j3array[numsolutions] > IKPI )
1071 {
1072  j3array[numsolutions]-=IK2PI;
1073 }
1074 else if( j3array[numsolutions] < -IKPI )
1075 {
1076  j3array[numsolutions]+=IK2PI;
1077 }
1078 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1079 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1080 numsolutions++;
1081 }
1082 }
1083 bool j3valid[4]={true,true,true,true};
1084 _nj3 = 4;
1085 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1086  {
1087 if( !j3valid[ij3] )
1088 {
1089  continue;
1090 }
1091  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1092 htj3 = IKtan(j3/2);
1093 
1094 IkReal x128=new_r22*new_r22;
1095 IkReal x129=((16.0)*new_r01);
1096 IkReal x130=(new_r00*new_r22);
1097 IkReal x131=((8.0)*x130);
1098 IkReal x132=(new_r11*x128);
1099 IkReal x133=((8.0)*x132);
1100 IkReal x134=(x128*x129);
1101 j3evalpoly[0]=((((htj3*htj3*htj3*htj3)*((x133+(((-1.0)*x131))))))+(((htj3*htj3)*(((((32.0)*new_r11))+(((-16.0)*x130))+(((-16.0)*x132))))))+x133+(((-1.0)*x131))+(((htj3*htj3*htj3)*(((((-1.0)*x129))+x134))))+((htj3*((x129+(((-1.0)*x134)))))));
1102 if( IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1103 {
1104  continue;
1105 }
1106 _ij3[0] = ij3; _ij3[1] = -1;
1107 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1108 {
1109 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1110 {
1111  j3valid[iij3]=false; _ij3[1] = iij3; break;
1112 }
1113 }
1114 {
1115 IkReal j5eval[3];
1116 new_r02=0;
1117 new_r12=0;
1118 new_r20=0;
1119 new_r21=0;
1120 IkReal x135=cj3*cj3;
1121 IkReal x136=(cj3*new_r22);
1122 IkReal x137=((-1.0)+(((-1.0)*x135*(new_r22*new_r22)))+x135);
1123 j5eval[0]=x137;
1124 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x136)))))+(IKabs((((new_r01*x136))+((new_r00*sj3))))));
1125 j5eval[2]=IKsign(x137);
1126 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1127 {
1128 {
1129 IkReal j5eval[1];
1130 new_r02=0;
1131 new_r12=0;
1132 new_r20=0;
1133 new_r21=0;
1134 j5eval[0]=new_r22;
1135 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1136 {
1137 {
1138 IkReal j5eval[2];
1139 new_r02=0;
1140 new_r12=0;
1141 new_r20=0;
1142 new_r21=0;
1143 IkReal x138=new_r22*new_r22;
1144 j5eval[0]=(((cj3*x138))+(((-1.0)*cj3)));
1145 j5eval[1]=((((-1.0)*sj3))+((sj3*x138)));
1146 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
1147 {
1148 {
1149 IkReal evalcond[1];
1150 bool bgotonextstatement = true;
1151 do
1152 {
1153 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
1154 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1155 {
1156 bgotonextstatement=false;
1157 {
1158 IkReal j5array[1], cj5array[1], sj5array[1];
1159 bool j5valid[1]={false};
1160 _nj5 = 1;
1161 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 )
1162  continue;
1163 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
1164 sj5array[0]=IKsin(j5array[0]);
1165 cj5array[0]=IKcos(j5array[0]);
1166 if( j5array[0] > IKPI )
1167 {
1168  j5array[0]-=IK2PI;
1169 }
1170 else if( j5array[0] < -IKPI )
1171 { j5array[0]+=IK2PI;
1172 }
1173 j5valid[0] = true;
1174 for(int ij5 = 0; ij5 < 1; ++ij5)
1175 {
1176 if( !j5valid[ij5] )
1177 {
1178  continue;
1179 }
1180 _ij5[0] = ij5; _ij5[1] = -1;
1181 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1182 {
1183 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1184 {
1185  j5valid[iij5]=false; _ij5[1] = iij5; break;
1186 }
1187 }
1188 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1189 {
1190 IkReal evalcond[4];
1191 IkReal x139=IKsin(j5);
1192 IkReal x140=IKcos(j5);
1193 evalcond[0]=x140;
1194 evalcond[1]=((-1.0)*x139);
1195 evalcond[2]=((((-1.0)*x139))+(((-1.0)*new_r00)));
1196 evalcond[3]=((((-1.0)*x140))+(((-1.0)*new_r01)));
1197 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1198 {
1199 continue;
1200 }
1201 }
1202 
1203 {
1204 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1205 vinfos[0].jointtype = 1;
1206 vinfos[0].foffset = j0;
1207 vinfos[0].indices[0] = _ij0[0];
1208 vinfos[0].indices[1] = _ij0[1];
1209 vinfos[0].maxsolutions = _nj0;
1210 vinfos[1].jointtype = 1;
1211 vinfos[1].foffset = j1;
1212 vinfos[1].indices[0] = _ij1[0];
1213 vinfos[1].indices[1] = _ij1[1];
1214 vinfos[1].maxsolutions = _nj1;
1215 vinfos[2].jointtype = 1;
1216 vinfos[2].foffset = j2;
1217 vinfos[2].indices[0] = _ij2[0];
1218 vinfos[2].indices[1] = _ij2[1];
1219 vinfos[2].maxsolutions = _nj2;
1220 vinfos[3].jointtype = 1;
1221 vinfos[3].foffset = j3;
1222 vinfos[3].indices[0] = _ij3[0];
1223 vinfos[3].indices[1] = _ij3[1];
1224 vinfos[3].maxsolutions = _nj3;
1225 vinfos[4].jointtype = 1;
1226 vinfos[4].foffset = j4;
1227 vinfos[4].indices[0] = _ij4[0];
1228 vinfos[4].indices[1] = _ij4[1];
1229 vinfos[4].maxsolutions = _nj4;
1230 vinfos[5].jointtype = 1;
1231 vinfos[5].foffset = j5;
1232 vinfos[5].indices[0] = _ij5[0];
1233 vinfos[5].indices[1] = _ij5[1];
1234 vinfos[5].maxsolutions = _nj5;
1235 std::vector<int> vfree(0);
1236 solutions.AddSolution(vinfos,vfree);
1237 }
1238 }
1239 }
1240 
1241 }
1242 } while(0);
1243 if( bgotonextstatement )
1244 {
1245 bool bgotonextstatement = true;
1246 do
1247 {
1248 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
1249 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1250 {
1251 bgotonextstatement=false;
1252 {
1253 IkReal j5array[1], cj5array[1], sj5array[1];
1254 bool j5valid[1]={false};
1255 _nj5 = 1;
1256 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
1257  continue;
1258 j5array[0]=IKatan2(new_r00, new_r01);
1259 sj5array[0]=IKsin(j5array[0]);
1260 cj5array[0]=IKcos(j5array[0]);
1261 if( j5array[0] > IKPI )
1262 {
1263  j5array[0]-=IK2PI;
1264 }
1265 else if( j5array[0] < -IKPI )
1266 { j5array[0]+=IK2PI;
1267 }
1268 j5valid[0] = true;
1269 for(int ij5 = 0; ij5 < 1; ++ij5)
1270 {
1271 if( !j5valid[ij5] )
1272 {
1273  continue;
1274 }
1275 _ij5[0] = ij5; _ij5[1] = -1;
1276 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1277 {
1278 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1279 {
1280  j5valid[iij5]=false; _ij5[1] = iij5; break;
1281 }
1282 }
1283 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1284 {
1285 IkReal evalcond[4];
1286 IkReal x141=IKsin(j5);
1287 IkReal x142=IKcos(j5);
1288 evalcond[0]=x142;
1289 evalcond[1]=((-1.0)*x141);
1290 evalcond[2]=((((-1.0)*x141))+new_r00);
1291 evalcond[3]=((((-1.0)*x142))+new_r01);
1292 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1293 {
1294 continue;
1295 }
1296 }
1297 
1298 {
1299 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1300 vinfos[0].jointtype = 1;
1301 vinfos[0].foffset = j0;
1302 vinfos[0].indices[0] = _ij0[0];
1303 vinfos[0].indices[1] = _ij0[1];
1304 vinfos[0].maxsolutions = _nj0;
1305 vinfos[1].jointtype = 1;
1306 vinfos[1].foffset = j1;
1307 vinfos[1].indices[0] = _ij1[0];
1308 vinfos[1].indices[1] = _ij1[1];
1309 vinfos[1].maxsolutions = _nj1;
1310 vinfos[2].jointtype = 1;
1311 vinfos[2].foffset = j2;
1312 vinfos[2].indices[0] = _ij2[0];
1313 vinfos[2].indices[1] = _ij2[1];
1314 vinfos[2].maxsolutions = _nj2;
1315 vinfos[3].jointtype = 1;
1316 vinfos[3].foffset = j3;
1317 vinfos[3].indices[0] = _ij3[0];
1318 vinfos[3].indices[1] = _ij3[1];
1319 vinfos[3].maxsolutions = _nj3;
1320 vinfos[4].jointtype = 1;
1321 vinfos[4].foffset = j4;
1322 vinfos[4].indices[0] = _ij4[0];
1323 vinfos[4].indices[1] = _ij4[1];
1324 vinfos[4].maxsolutions = _nj4;
1325 vinfos[5].jointtype = 1;
1326 vinfos[5].foffset = j5;
1327 vinfos[5].indices[0] = _ij5[0];
1328 vinfos[5].indices[1] = _ij5[1];
1329 vinfos[5].maxsolutions = _nj5;
1330 std::vector<int> vfree(0);
1331 solutions.AddSolution(vinfos,vfree);
1332 }
1333 }
1334 }
1335 
1336 }
1337 } while(0);
1338 if( bgotonextstatement )
1339 {
1340 bool bgotonextstatement = true;
1341 do
1342 {
1343 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1344 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1345 {
1346 bgotonextstatement=false;
1347 {
1348 IkReal j5array[1], cj5array[1], sj5array[1];
1349 bool j5valid[1]={false};
1350 _nj5 = 1;
1351 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1352  continue;
1353 j5array[0]=IKatan2(new_r10, new_r11);
1354 sj5array[0]=IKsin(j5array[0]);
1355 cj5array[0]=IKcos(j5array[0]);
1356 if( j5array[0] > IKPI )
1357 {
1358  j5array[0]-=IK2PI;
1359 }
1360 else if( j5array[0] < -IKPI )
1361 { j5array[0]+=IK2PI;
1362 }
1363 j5valid[0] = true;
1364 for(int ij5 = 0; ij5 < 1; ++ij5)
1365 {
1366 if( !j5valid[ij5] )
1367 {
1368  continue;
1369 }
1370 _ij5[0] = ij5; _ij5[1] = -1;
1371 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1372 {
1373 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1374 {
1375  j5valid[iij5]=false; _ij5[1] = iij5; break;
1376 }
1377 }
1378 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1379 {
1380 IkReal evalcond[4];
1381 IkReal x143=IKsin(j5);
1382 IkReal x144=IKcos(j5);
1383 evalcond[0]=x144;
1384 evalcond[1]=((-1.0)*x143);
1385 evalcond[2]=((((-1.0)*x143))+new_r10);
1386 evalcond[3]=((((-1.0)*x144))+new_r11);
1387 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1388 {
1389 continue;
1390 }
1391 }
1392 
1393 {
1394 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1395 vinfos[0].jointtype = 1;
1396 vinfos[0].foffset = j0;
1397 vinfos[0].indices[0] = _ij0[0];
1398 vinfos[0].indices[1] = _ij0[1];
1399 vinfos[0].maxsolutions = _nj0;
1400 vinfos[1].jointtype = 1;
1401 vinfos[1].foffset = j1;
1402 vinfos[1].indices[0] = _ij1[0];
1403 vinfos[1].indices[1] = _ij1[1];
1404 vinfos[1].maxsolutions = _nj1;
1405 vinfos[2].jointtype = 1;
1406 vinfos[2].foffset = j2;
1407 vinfos[2].indices[0] = _ij2[0];
1408 vinfos[2].indices[1] = _ij2[1];
1409 vinfos[2].maxsolutions = _nj2;
1410 vinfos[3].jointtype = 1;
1411 vinfos[3].foffset = j3;
1412 vinfos[3].indices[0] = _ij3[0];
1413 vinfos[3].indices[1] = _ij3[1];
1414 vinfos[3].maxsolutions = _nj3;
1415 vinfos[4].jointtype = 1;
1416 vinfos[4].foffset = j4;
1417 vinfos[4].indices[0] = _ij4[0];
1418 vinfos[4].indices[1] = _ij4[1];
1419 vinfos[4].maxsolutions = _nj4;
1420 vinfos[5].jointtype = 1;
1421 vinfos[5].foffset = j5;
1422 vinfos[5].indices[0] = _ij5[0];
1423 vinfos[5].indices[1] = _ij5[1];
1424 vinfos[5].maxsolutions = _nj5;
1425 std::vector<int> vfree(0);
1426 solutions.AddSolution(vinfos,vfree);
1427 }
1428 }
1429 }
1430 
1431 }
1432 } while(0);
1433 if( bgotonextstatement )
1434 {
1435 bool bgotonextstatement = true;
1436 do
1437 {
1438 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1439 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1440 {
1441 bgotonextstatement=false;
1442 {
1443 IkReal j5array[1], cj5array[1], sj5array[1];
1444 bool j5valid[1]={false};
1445 _nj5 = 1;
1446 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 )
1447  continue;
1448 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1449 sj5array[0]=IKsin(j5array[0]);
1450 cj5array[0]=IKcos(j5array[0]);
1451 if( j5array[0] > IKPI )
1452 {
1453  j5array[0]-=IK2PI;
1454 }
1455 else if( j5array[0] < -IKPI )
1456 { j5array[0]+=IK2PI;
1457 }
1458 j5valid[0] = true;
1459 for(int ij5 = 0; ij5 < 1; ++ij5)
1460 {
1461 if( !j5valid[ij5] )
1462 {
1463  continue;
1464 }
1465 _ij5[0] = ij5; _ij5[1] = -1;
1466 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1467 {
1468 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1469 {
1470  j5valid[iij5]=false; _ij5[1] = iij5; break;
1471 }
1472 }
1473 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1474 {
1475 IkReal evalcond[4];
1476 IkReal x145=IKsin(j5);
1477 IkReal x146=IKcos(j5);
1478 evalcond[0]=x146;
1479 evalcond[1]=((-1.0)*x145);
1480 evalcond[2]=((((-1.0)*x145))+(((-1.0)*new_r10)));
1481 evalcond[3]=((((-1.0)*x146))+(((-1.0)*new_r11)));
1482 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1483 {
1484 continue;
1485 }
1486 }
1487 
1488 {
1489 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1490 vinfos[0].jointtype = 1;
1491 vinfos[0].foffset = j0;
1492 vinfos[0].indices[0] = _ij0[0];
1493 vinfos[0].indices[1] = _ij0[1];
1494 vinfos[0].maxsolutions = _nj0;
1495 vinfos[1].jointtype = 1;
1496 vinfos[1].foffset = j1;
1497 vinfos[1].indices[0] = _ij1[0];
1498 vinfos[1].indices[1] = _ij1[1];
1499 vinfos[1].maxsolutions = _nj1;
1500 vinfos[2].jointtype = 1;
1501 vinfos[2].foffset = j2;
1502 vinfos[2].indices[0] = _ij2[0];
1503 vinfos[2].indices[1] = _ij2[1];
1504 vinfos[2].maxsolutions = _nj2;
1505 vinfos[3].jointtype = 1;
1506 vinfos[3].foffset = j3;
1507 vinfos[3].indices[0] = _ij3[0];
1508 vinfos[3].indices[1] = _ij3[1];
1509 vinfos[3].maxsolutions = _nj3;
1510 vinfos[4].jointtype = 1;
1511 vinfos[4].foffset = j4;
1512 vinfos[4].indices[0] = _ij4[0];
1513 vinfos[4].indices[1] = _ij4[1];
1514 vinfos[4].maxsolutions = _nj4;
1515 vinfos[5].jointtype = 1;
1516 vinfos[5].foffset = j5;
1517 vinfos[5].indices[0] = _ij5[0];
1518 vinfos[5].indices[1] = _ij5[1];
1519 vinfos[5].maxsolutions = _nj5;
1520 std::vector<int> vfree(0);
1521 solutions.AddSolution(vinfos,vfree);
1522 }
1523 }
1524 }
1525 
1526 }
1527 } while(0);
1528 if( bgotonextstatement )
1529 {
1530 bool bgotonextstatement = true;
1531 do
1532 {
1533 CheckValue<IkReal> x147=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1534 if(!x147.valid){
1535 continue;
1536 }
1537 if((x147.value) < -0.00001)
1538 continue;
1539 IkReal gconst6=((-1.0)*(IKsqrt(x147.value)));
1540 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst6)))))), 6.28318530717959)));
1541 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1542 {
1543 bgotonextstatement=false;
1544 {
1545 IkReal j5eval[1];
1546 new_r02=0;
1547 new_r12=0;
1548 new_r20=0;
1549 new_r21=0;
1550 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1551 continue;
1552 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))));
1553 cj3=gconst6;
1554 if( (gconst6) < -1-IKFAST_SINCOS_THRESH || (gconst6) > 1+IKFAST_SINCOS_THRESH )
1555  continue;
1556 j3=IKacos(gconst6);
1557 CheckValue<IkReal> x148=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1558 if(!x148.valid){
1559 continue;
1560 }
1561 if((x148.value) < -0.00001)
1562 continue;
1563 IkReal gconst6=((-1.0)*(IKsqrt(x148.value)));
1564 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1565 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1566 {
1567 {
1568 IkReal j5array[1], cj5array[1], sj5array[1];
1569 bool j5valid[1]={false};
1570 _nj5 = 1;
1571 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1572 continue;
1573 CheckValue<IkReal> x149=IKPowWithIntegerCheck(gconst6,-1);
1574 if(!x149.valid){
1575 continue;
1576 }
1577 if( IKabs((((gconst6*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x149.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst6*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6))))))))))+IKsqr((new_r11*(x149.value)))-1) <= IKFAST_SINCOS_THRESH )
1578  continue;
1579 j5array[0]=IKatan2((((gconst6*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6))))))))), (new_r11*(x149.value)));
1580 sj5array[0]=IKsin(j5array[0]);
1581 cj5array[0]=IKcos(j5array[0]);
1582 if( j5array[0] > IKPI )
1583 {
1584  j5array[0]-=IK2PI;
1585 }
1586 else if( j5array[0] < -IKPI )
1587 { j5array[0]+=IK2PI;
1588 }
1589 j5valid[0] = true;
1590 for(int ij5 = 0; ij5 < 1; ++ij5)
1591 {
1592 if( !j5valid[ij5] )
1593 {
1594  continue;
1595 }
1596 _ij5[0] = ij5; _ij5[1] = -1;
1597 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1598 {
1599 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1600 {
1601  j5valid[iij5]=false; _ij5[1] = iij5; break;
1602 }
1603 }
1604 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1605 {
1606 IkReal evalcond[8];
1607 IkReal x150=IKcos(j5);
1608 IkReal x151=IKsin(j5);
1609 IkReal x152=((1.0)*x151);
1610 IkReal x153=((1.0)*x150);
1611 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1612 continue;
1613 IkReal x154=IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))));
1614 IkReal x155=((1.0)*x154);
1615 evalcond[0]=x150;
1616 evalcond[1]=((-1.0)*x151);
1617 evalcond[2]=((((-1.0)*gconst6*x153))+new_r11);
1618 evalcond[3]=((((-1.0)*gconst6*x152))+new_r10);
1619 evalcond[4]=(((x150*x154))+new_r01);
1620 evalcond[5]=(((x151*x154))+new_r00);
1621 evalcond[6]=((((-1.0)*x152))+((gconst6*new_r10))+(((-1.0)*new_r00*x155)));
1622 evalcond[7]=((((-1.0)*x153))+((gconst6*new_r11))+(((-1.0)*new_r01*x155)));
1623 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1624 {
1625 continue;
1626 }
1627 }
1628 
1629 {
1630 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1631 vinfos[0].jointtype = 1;
1632 vinfos[0].foffset = j0;
1633 vinfos[0].indices[0] = _ij0[0];
1634 vinfos[0].indices[1] = _ij0[1];
1635 vinfos[0].maxsolutions = _nj0;
1636 vinfos[1].jointtype = 1;
1637 vinfos[1].foffset = j1;
1638 vinfos[1].indices[0] = _ij1[0];
1639 vinfos[1].indices[1] = _ij1[1];
1640 vinfos[1].maxsolutions = _nj1;
1641 vinfos[2].jointtype = 1;
1642 vinfos[2].foffset = j2;
1643 vinfos[2].indices[0] = _ij2[0];
1644 vinfos[2].indices[1] = _ij2[1];
1645 vinfos[2].maxsolutions = _nj2;
1646 vinfos[3].jointtype = 1;
1647 vinfos[3].foffset = j3;
1648 vinfos[3].indices[0] = _ij3[0];
1649 vinfos[3].indices[1] = _ij3[1];
1650 vinfos[3].maxsolutions = _nj3;
1651 vinfos[4].jointtype = 1;
1652 vinfos[4].foffset = j4;
1653 vinfos[4].indices[0] = _ij4[0];
1654 vinfos[4].indices[1] = _ij4[1];
1655 vinfos[4].maxsolutions = _nj4;
1656 vinfos[5].jointtype = 1;
1657 vinfos[5].foffset = j5;
1658 vinfos[5].indices[0] = _ij5[0];
1659 vinfos[5].indices[1] = _ij5[1];
1660 vinfos[5].maxsolutions = _nj5;
1661 std::vector<int> vfree(0);
1662 solutions.AddSolution(vinfos,vfree);
1663 }
1664 }
1665 }
1666 
1667 } else
1668 {
1669 {
1670 IkReal j5array[1], cj5array[1], sj5array[1];
1671 bool j5valid[1]={false};
1672 _nj5 = 1;
1673 CheckValue<IkReal> x156 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1674 if(!x156.valid){
1675 continue;
1676 }
1677 CheckValue<IkReal> x157=IKPowWithIntegerCheck(IKsign(gconst6),-1);
1678 if(!x157.valid){
1679 continue;
1680 }
1681 j5array[0]=((-1.5707963267949)+(x156.value)+(((1.5707963267949)*(x157.value))));
1682 sj5array[0]=IKsin(j5array[0]);
1683 cj5array[0]=IKcos(j5array[0]);
1684 if( j5array[0] > IKPI )
1685 {
1686  j5array[0]-=IK2PI;
1687 }
1688 else if( j5array[0] < -IKPI )
1689 { j5array[0]+=IK2PI;
1690 }
1691 j5valid[0] = true;
1692 for(int ij5 = 0; ij5 < 1; ++ij5)
1693 {
1694 if( !j5valid[ij5] )
1695 {
1696  continue;
1697 }
1698 _ij5[0] = ij5; _ij5[1] = -1;
1699 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1700 {
1701 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1702 {
1703  j5valid[iij5]=false; _ij5[1] = iij5; break;
1704 }
1705 }
1706 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1707 {
1708 IkReal evalcond[8];
1709 IkReal x158=IKcos(j5);
1710 IkReal x159=IKsin(j5);
1711 IkReal x160=((1.0)*x159);
1712 IkReal x161=((1.0)*x158);
1713 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1714 continue;
1715 IkReal x162=IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))));
1716 IkReal x163=((1.0)*x162);
1717 evalcond[0]=x158;
1718 evalcond[1]=((-1.0)*x159);
1719 evalcond[2]=((((-1.0)*gconst6*x161))+new_r11);
1720 evalcond[3]=((((-1.0)*gconst6*x160))+new_r10);
1721 evalcond[4]=(new_r01+((x158*x162)));
1722 evalcond[5]=(new_r00+((x159*x162)));
1723 evalcond[6]=((((-1.0)*new_r00*x163))+(((-1.0)*x160))+((gconst6*new_r10)));
1724 evalcond[7]=((((-1.0)*x161))+((gconst6*new_r11))+(((-1.0)*new_r01*x163)));
1725 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1726 {
1727 continue;
1728 }
1729 }
1730 
1731 {
1732 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1733 vinfos[0].jointtype = 1;
1734 vinfos[0].foffset = j0;
1735 vinfos[0].indices[0] = _ij0[0];
1736 vinfos[0].indices[1] = _ij0[1];
1737 vinfos[0].maxsolutions = _nj0;
1738 vinfos[1].jointtype = 1;
1739 vinfos[1].foffset = j1;
1740 vinfos[1].indices[0] = _ij1[0];
1741 vinfos[1].indices[1] = _ij1[1];
1742 vinfos[1].maxsolutions = _nj1;
1743 vinfos[2].jointtype = 1;
1744 vinfos[2].foffset = j2;
1745 vinfos[2].indices[0] = _ij2[0];
1746 vinfos[2].indices[1] = _ij2[1];
1747 vinfos[2].maxsolutions = _nj2;
1748 vinfos[3].jointtype = 1;
1749 vinfos[3].foffset = j3;
1750 vinfos[3].indices[0] = _ij3[0];
1751 vinfos[3].indices[1] = _ij3[1];
1752 vinfos[3].maxsolutions = _nj3;
1753 vinfos[4].jointtype = 1;
1754 vinfos[4].foffset = j4;
1755 vinfos[4].indices[0] = _ij4[0];
1756 vinfos[4].indices[1] = _ij4[1];
1757 vinfos[4].maxsolutions = _nj4;
1758 vinfos[5].jointtype = 1;
1759 vinfos[5].foffset = j5;
1760 vinfos[5].indices[0] = _ij5[0];
1761 vinfos[5].indices[1] = _ij5[1];
1762 vinfos[5].maxsolutions = _nj5;
1763 std::vector<int> vfree(0);
1764 solutions.AddSolution(vinfos,vfree);
1765 }
1766 }
1767 }
1768 
1769 }
1770 
1771 }
1772 
1773 }
1774 } while(0);
1775 if( bgotonextstatement )
1776 {
1777 bool bgotonextstatement = true;
1778 do
1779 {
1780 CheckValue<IkReal> x164=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1781 if(!x164.valid){
1782 continue;
1783 }
1784 if((x164.value) < -0.00001)
1785 continue;
1786 IkReal gconst6=((-1.0)*(IKsqrt(x164.value)));
1787 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst6)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1788 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1789 {
1790 bgotonextstatement=false;
1791 {
1792 IkReal j5eval[1];
1793 new_r02=0;
1794 new_r12=0;
1795 new_r20=0;
1796 new_r21=0;
1797 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1798 continue;
1799 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))))));
1800 cj3=gconst6;
1801 if( (gconst6) < -1-IKFAST_SINCOS_THRESH || (gconst6) > 1+IKFAST_SINCOS_THRESH )
1802  continue;
1803 j3=((-1.0)*(IKacos(gconst6)));
1804 CheckValue<IkReal> x165=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1805 if(!x165.valid){
1806 continue;
1807 }
1808 if((x165.value) < -0.00001)
1809 continue;
1810 IkReal gconst6=((-1.0)*(IKsqrt(x165.value)));
1811 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1812 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1813 {
1814 {
1815 IkReal j5array[1], cj5array[1], sj5array[1];
1816 bool j5valid[1]={false};
1817 _nj5 = 1;
1818 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1819 continue;
1820 CheckValue<IkReal> x166=IKPowWithIntegerCheck(gconst6,-1);
1821 if(!x166.valid){
1822 continue;
1823 }
1824 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6))))))))+((gconst6*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x166.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6))))))))+((gconst6*new_r10))))+IKsqr((new_r11*(x166.value)))-1) <= IKFAST_SINCOS_THRESH )
1825  continue;
1826 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6))))))))+((gconst6*new_r10))), (new_r11*(x166.value)));
1827 sj5array[0]=IKsin(j5array[0]);
1828 cj5array[0]=IKcos(j5array[0]);
1829 if( j5array[0] > IKPI )
1830 {
1831  j5array[0]-=IK2PI;
1832 }
1833 else if( j5array[0] < -IKPI )
1834 { j5array[0]+=IK2PI;
1835 }
1836 j5valid[0] = true;
1837 for(int ij5 = 0; ij5 < 1; ++ij5)
1838 {
1839 if( !j5valid[ij5] )
1840 {
1841  continue;
1842 }
1843 _ij5[0] = ij5; _ij5[1] = -1;
1844 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1845 {
1846 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1847 {
1848  j5valid[iij5]=false; _ij5[1] = iij5; break;
1849 }
1850 }
1851 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1852 {
1853 IkReal evalcond[8];
1854 IkReal x167=IKcos(j5);
1855 IkReal x168=IKsin(j5);
1856 IkReal x169=((1.0)*x167);
1857 IkReal x170=((1.0)*x168);
1858 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1859 continue;
1860 IkReal x171=IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))));
1861 IkReal x172=((1.0)*x171);
1862 evalcond[0]=x167;
1863 evalcond[1]=((-1.0)*x168);
1864 evalcond[2]=((((-1.0)*gconst6*x169))+new_r11);
1865 evalcond[3]=((((-1.0)*gconst6*x170))+new_r10);
1866 evalcond[4]=(new_r01+(((-1.0)*x169*x171)));
1867 evalcond[5]=((((-1.0)*x170*x171))+new_r00);
1868 evalcond[6]=(((new_r00*x171))+(((-1.0)*x170))+((gconst6*new_r10)));
1869 evalcond[7]=(((new_r01*x171))+(((-1.0)*x169))+((gconst6*new_r11)));
1870 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1871 {
1872 continue;
1873 }
1874 }
1875 
1876 {
1877 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1878 vinfos[0].jointtype = 1;
1879 vinfos[0].foffset = j0;
1880 vinfos[0].indices[0] = _ij0[0];
1881 vinfos[0].indices[1] = _ij0[1];
1882 vinfos[0].maxsolutions = _nj0;
1883 vinfos[1].jointtype = 1;
1884 vinfos[1].foffset = j1;
1885 vinfos[1].indices[0] = _ij1[0];
1886 vinfos[1].indices[1] = _ij1[1];
1887 vinfos[1].maxsolutions = _nj1;
1888 vinfos[2].jointtype = 1;
1889 vinfos[2].foffset = j2;
1890 vinfos[2].indices[0] = _ij2[0];
1891 vinfos[2].indices[1] = _ij2[1];
1892 vinfos[2].maxsolutions = _nj2;
1893 vinfos[3].jointtype = 1;
1894 vinfos[3].foffset = j3;
1895 vinfos[3].indices[0] = _ij3[0];
1896 vinfos[3].indices[1] = _ij3[1];
1897 vinfos[3].maxsolutions = _nj3;
1898 vinfos[4].jointtype = 1;
1899 vinfos[4].foffset = j4;
1900 vinfos[4].indices[0] = _ij4[0];
1901 vinfos[4].indices[1] = _ij4[1];
1902 vinfos[4].maxsolutions = _nj4;
1903 vinfos[5].jointtype = 1;
1904 vinfos[5].foffset = j5;
1905 vinfos[5].indices[0] = _ij5[0];
1906 vinfos[5].indices[1] = _ij5[1];
1907 vinfos[5].maxsolutions = _nj5;
1908 std::vector<int> vfree(0);
1909 solutions.AddSolution(vinfos,vfree);
1910 }
1911 }
1912 }
1913 
1914 } else
1915 {
1916 {
1917 IkReal j5array[1], cj5array[1], sj5array[1];
1918 bool j5valid[1]={false};
1919 _nj5 = 1;
1920 CheckValue<IkReal> x173 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1921 if(!x173.valid){
1922 continue;
1923 }
1924 CheckValue<IkReal> x174=IKPowWithIntegerCheck(IKsign(gconst6),-1);
1925 if(!x174.valid){
1926 continue;
1927 }
1928 j5array[0]=((-1.5707963267949)+(x173.value)+(((1.5707963267949)*(x174.value))));
1929 sj5array[0]=IKsin(j5array[0]);
1930 cj5array[0]=IKcos(j5array[0]);
1931 if( j5array[0] > IKPI )
1932 {
1933  j5array[0]-=IK2PI;
1934 }
1935 else if( j5array[0] < -IKPI )
1936 { j5array[0]+=IK2PI;
1937 }
1938 j5valid[0] = true;
1939 for(int ij5 = 0; ij5 < 1; ++ij5)
1940 {
1941 if( !j5valid[ij5] )
1942 {
1943  continue;
1944 }
1945 _ij5[0] = ij5; _ij5[1] = -1;
1946 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1947 {
1948 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1949 {
1950  j5valid[iij5]=false; _ij5[1] = iij5; break;
1951 }
1952 }
1953 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1954 {
1955 IkReal evalcond[8];
1956 IkReal x175=IKcos(j5);
1957 IkReal x176=IKsin(j5);
1958 IkReal x177=((1.0)*x175);
1959 IkReal x178=((1.0)*x176);
1960 if((((1.0)+(((-1.0)*(gconst6*gconst6))))) < -0.00001)
1961 continue;
1962 IkReal x179=IKsqrt(((1.0)+(((-1.0)*(gconst6*gconst6)))));
1963 IkReal x180=((1.0)*x179);
1964 evalcond[0]=x175;
1965 evalcond[1]=((-1.0)*x176);
1966 evalcond[2]=((((-1.0)*gconst6*x177))+new_r11);
1967 evalcond[3]=((((-1.0)*gconst6*x178))+new_r10);
1968 evalcond[4]=((((-1.0)*x177*x179))+new_r01);
1969 evalcond[5]=((((-1.0)*x178*x179))+new_r00);
1970 evalcond[6]=(((new_r00*x179))+(((-1.0)*x178))+((gconst6*new_r10)));
1971 evalcond[7]=(((new_r01*x179))+(((-1.0)*x177))+((gconst6*new_r11)));
1972 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1973 {
1974 continue;
1975 }
1976 }
1977 
1978 {
1979 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1980 vinfos[0].jointtype = 1;
1981 vinfos[0].foffset = j0;
1982 vinfos[0].indices[0] = _ij0[0];
1983 vinfos[0].indices[1] = _ij0[1];
1984 vinfos[0].maxsolutions = _nj0;
1985 vinfos[1].jointtype = 1;
1986 vinfos[1].foffset = j1;
1987 vinfos[1].indices[0] = _ij1[0];
1988 vinfos[1].indices[1] = _ij1[1];
1989 vinfos[1].maxsolutions = _nj1;
1990 vinfos[2].jointtype = 1;
1991 vinfos[2].foffset = j2;
1992 vinfos[2].indices[0] = _ij2[0];
1993 vinfos[2].indices[1] = _ij2[1];
1994 vinfos[2].maxsolutions = _nj2;
1995 vinfos[3].jointtype = 1;
1996 vinfos[3].foffset = j3;
1997 vinfos[3].indices[0] = _ij3[0];
1998 vinfos[3].indices[1] = _ij3[1];
1999 vinfos[3].maxsolutions = _nj3;
2000 vinfos[4].jointtype = 1;
2001 vinfos[4].foffset = j4;
2002 vinfos[4].indices[0] = _ij4[0];
2003 vinfos[4].indices[1] = _ij4[1];
2004 vinfos[4].maxsolutions = _nj4;
2005 vinfos[5].jointtype = 1;
2006 vinfos[5].foffset = j5;
2007 vinfos[5].indices[0] = _ij5[0];
2008 vinfos[5].indices[1] = _ij5[1];
2009 vinfos[5].maxsolutions = _nj5;
2010 std::vector<int> vfree(0);
2011 solutions.AddSolution(vinfos,vfree);
2012 }
2013 }
2014 }
2015 
2016 }
2017 
2018 }
2019 
2020 }
2021 } while(0);
2022 if( bgotonextstatement )
2023 {
2024 bool bgotonextstatement = true;
2025 do
2026 {
2027 CheckValue<IkReal> x181=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2028 if(!x181.valid){
2029 continue;
2030 }
2031 if((x181.value) < -0.00001)
2032 continue;
2033 IkReal gconst7=IKsqrt(x181.value);
2034 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst7)))))), 6.28318530717959)));
2035 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2036 {
2037 bgotonextstatement=false;
2038 {
2039 IkReal j5eval[1];
2040 new_r02=0;
2041 new_r12=0;
2042 new_r20=0;
2043 new_r21=0;
2044 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2045 continue;
2046 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))));
2047 cj3=gconst7;
2048 if( (gconst7) < -1-IKFAST_SINCOS_THRESH || (gconst7) > 1+IKFAST_SINCOS_THRESH )
2049  continue;
2050 j3=IKacos(gconst7);
2051 CheckValue<IkReal> x182=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2052 if(!x182.valid){
2053 continue;
2054 }
2055 if((x182.value) < -0.00001)
2056 continue;
2057 IkReal gconst7=IKsqrt(x182.value);
2058 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2059 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2060 {
2061 {
2062 IkReal j5array[1], cj5array[1], sj5array[1];
2063 bool j5valid[1]={false};
2064 _nj5 = 1;
2065 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2066 continue;
2067 CheckValue<IkReal> x183=IKPowWithIntegerCheck(gconst7,-1);
2068 if(!x183.valid){
2069 continue;
2070 }
2071 if( IKabs(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7))))))))+((gconst7*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x183.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7))))))))+((gconst7*new_r10))))+IKsqr((new_r11*(x183.value)))-1) <= IKFAST_SINCOS_THRESH )
2072  continue;
2073 j5array[0]=IKatan2(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7))))))))+((gconst7*new_r10))), (new_r11*(x183.value)));
2074 sj5array[0]=IKsin(j5array[0]);
2075 cj5array[0]=IKcos(j5array[0]);
2076 if( j5array[0] > IKPI )
2077 {
2078  j5array[0]-=IK2PI;
2079 }
2080 else if( j5array[0] < -IKPI )
2081 { j5array[0]+=IK2PI;
2082 }
2083 j5valid[0] = true;
2084 for(int ij5 = 0; ij5 < 1; ++ij5)
2085 {
2086 if( !j5valid[ij5] )
2087 {
2088  continue;
2089 }
2090 _ij5[0] = ij5; _ij5[1] = -1;
2091 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2092 {
2093 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2094 {
2095  j5valid[iij5]=false; _ij5[1] = iij5; break;
2096 }
2097 }
2098 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2099 {
2100 IkReal evalcond[8];
2101 IkReal x184=IKcos(j5);
2102 IkReal x185=IKsin(j5);
2103 IkReal x186=((1.0)*x185);
2104 IkReal x187=((1.0)*x184);
2105 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2106 continue;
2107 IkReal x188=IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))));
2108 IkReal x189=((1.0)*x188);
2109 evalcond[0]=x184;
2110 evalcond[1]=((-1.0)*x185);
2111 evalcond[2]=(new_r11+(((-1.0)*gconst7*x187)));
2112 evalcond[3]=(new_r10+(((-1.0)*gconst7*x186)));
2113 evalcond[4]=(((x184*x188))+new_r01);
2114 evalcond[5]=(((x185*x188))+new_r00);
2115 evalcond[6]=((((-1.0)*x186))+(((-1.0)*new_r00*x189))+((gconst7*new_r10)));
2116 evalcond[7]=((((-1.0)*x187))+(((-1.0)*new_r01*x189))+((gconst7*new_r11)));
2117 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2118 {
2119 continue;
2120 }
2121 }
2122 
2123 {
2124 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2125 vinfos[0].jointtype = 1;
2126 vinfos[0].foffset = j0;
2127 vinfos[0].indices[0] = _ij0[0];
2128 vinfos[0].indices[1] = _ij0[1];
2129 vinfos[0].maxsolutions = _nj0;
2130 vinfos[1].jointtype = 1;
2131 vinfos[1].foffset = j1;
2132 vinfos[1].indices[0] = _ij1[0];
2133 vinfos[1].indices[1] = _ij1[1];
2134 vinfos[1].maxsolutions = _nj1;
2135 vinfos[2].jointtype = 1;
2136 vinfos[2].foffset = j2;
2137 vinfos[2].indices[0] = _ij2[0];
2138 vinfos[2].indices[1] = _ij2[1];
2139 vinfos[2].maxsolutions = _nj2;
2140 vinfos[3].jointtype = 1;
2141 vinfos[3].foffset = j3;
2142 vinfos[3].indices[0] = _ij3[0];
2143 vinfos[3].indices[1] = _ij3[1];
2144 vinfos[3].maxsolutions = _nj3;
2145 vinfos[4].jointtype = 1;
2146 vinfos[4].foffset = j4;
2147 vinfos[4].indices[0] = _ij4[0];
2148 vinfos[4].indices[1] = _ij4[1];
2149 vinfos[4].maxsolutions = _nj4;
2150 vinfos[5].jointtype = 1;
2151 vinfos[5].foffset = j5;
2152 vinfos[5].indices[0] = _ij5[0];
2153 vinfos[5].indices[1] = _ij5[1];
2154 vinfos[5].maxsolutions = _nj5;
2155 std::vector<int> vfree(0);
2156 solutions.AddSolution(vinfos,vfree);
2157 }
2158 }
2159 }
2160 
2161 } else
2162 {
2163 {
2164 IkReal j5array[1], cj5array[1], sj5array[1];
2165 bool j5valid[1]={false};
2166 _nj5 = 1;
2167 CheckValue<IkReal> x190=IKPowWithIntegerCheck(IKsign(gconst7),-1);
2168 if(!x190.valid){
2169 continue;
2170 }
2171 CheckValue<IkReal> x191 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2172 if(!x191.valid){
2173 continue;
2174 }
2175 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x190.value)))+(x191.value));
2176 sj5array[0]=IKsin(j5array[0]);
2177 cj5array[0]=IKcos(j5array[0]);
2178 if( j5array[0] > IKPI )
2179 {
2180  j5array[0]-=IK2PI;
2181 }
2182 else if( j5array[0] < -IKPI )
2183 { j5array[0]+=IK2PI;
2184 }
2185 j5valid[0] = true;
2186 for(int ij5 = 0; ij5 < 1; ++ij5)
2187 {
2188 if( !j5valid[ij5] )
2189 {
2190  continue;
2191 }
2192 _ij5[0] = ij5; _ij5[1] = -1;
2193 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2194 {
2195 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2196 {
2197  j5valid[iij5]=false; _ij5[1] = iij5; break;
2198 }
2199 }
2200 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2201 {
2202 IkReal evalcond[8];
2203 IkReal x192=IKcos(j5);
2204 IkReal x193=IKsin(j5);
2205 IkReal x194=((1.0)*x193);
2206 IkReal x195=((1.0)*x192);
2207 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2208 continue;
2209 IkReal x196=IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))));
2210 IkReal x197=((1.0)*x196);
2211 evalcond[0]=x192;
2212 evalcond[1]=((-1.0)*x193);
2213 evalcond[2]=(new_r11+(((-1.0)*gconst7*x195)));
2214 evalcond[3]=(new_r10+(((-1.0)*gconst7*x194)));
2215 evalcond[4]=(((x192*x196))+new_r01);
2216 evalcond[5]=(((x193*x196))+new_r00);
2217 evalcond[6]=((((-1.0)*x194))+(((-1.0)*new_r00*x197))+((gconst7*new_r10)));
2218 evalcond[7]=((((-1.0)*x195))+(((-1.0)*new_r01*x197))+((gconst7*new_r11)));
2219 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2220 {
2221 continue;
2222 }
2223 }
2224 
2225 {
2226 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2227 vinfos[0].jointtype = 1;
2228 vinfos[0].foffset = j0;
2229 vinfos[0].indices[0] = _ij0[0];
2230 vinfos[0].indices[1] = _ij0[1];
2231 vinfos[0].maxsolutions = _nj0;
2232 vinfos[1].jointtype = 1;
2233 vinfos[1].foffset = j1;
2234 vinfos[1].indices[0] = _ij1[0];
2235 vinfos[1].indices[1] = _ij1[1];
2236 vinfos[1].maxsolutions = _nj1;
2237 vinfos[2].jointtype = 1;
2238 vinfos[2].foffset = j2;
2239 vinfos[2].indices[0] = _ij2[0];
2240 vinfos[2].indices[1] = _ij2[1];
2241 vinfos[2].maxsolutions = _nj2;
2242 vinfos[3].jointtype = 1;
2243 vinfos[3].foffset = j3;
2244 vinfos[3].indices[0] = _ij3[0];
2245 vinfos[3].indices[1] = _ij3[1];
2246 vinfos[3].maxsolutions = _nj3;
2247 vinfos[4].jointtype = 1;
2248 vinfos[4].foffset = j4;
2249 vinfos[4].indices[0] = _ij4[0];
2250 vinfos[4].indices[1] = _ij4[1];
2251 vinfos[4].maxsolutions = _nj4;
2252 vinfos[5].jointtype = 1;
2253 vinfos[5].foffset = j5;
2254 vinfos[5].indices[0] = _ij5[0];
2255 vinfos[5].indices[1] = _ij5[1];
2256 vinfos[5].maxsolutions = _nj5;
2257 std::vector<int> vfree(0);
2258 solutions.AddSolution(vinfos,vfree);
2259 }
2260 }
2261 }
2262 
2263 }
2264 
2265 }
2266 
2267 }
2268 } while(0);
2269 if( bgotonextstatement )
2270 {
2271 bool bgotonextstatement = true;
2272 do
2273 {
2274 CheckValue<IkReal> x198=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2275 if(!x198.valid){
2276 continue;
2277 }
2278 if((x198.value) < -0.00001)
2279 continue;
2280 IkReal gconst7=IKsqrt(x198.value);
2281 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst7)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
2282 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2283 {
2284 bgotonextstatement=false;
2285 {
2286 IkReal j5eval[1];
2287 new_r02=0;
2288 new_r12=0;
2289 new_r20=0;
2290 new_r21=0;
2291 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2292 continue;
2293 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))))));
2294 cj3=gconst7;
2295 if( (gconst7) < -1-IKFAST_SINCOS_THRESH || (gconst7) > 1+IKFAST_SINCOS_THRESH )
2296  continue;
2297 j3=((-1.0)*(IKacos(gconst7)));
2298 CheckValue<IkReal> x199=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2299 if(!x199.valid){
2300 continue;
2301 }
2302 if((x199.value) < -0.00001)
2303 continue;
2304 IkReal gconst7=IKsqrt(x199.value);
2305 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2306 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2307 {
2308 {
2309 IkReal j5array[1], cj5array[1], sj5array[1];
2310 bool j5valid[1]={false};
2311 _nj5 = 1;
2312 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2313 continue;
2314 CheckValue<IkReal> x200=IKPowWithIntegerCheck(gconst7,-1);
2315 if(!x200.valid){
2316 continue;
2317 }
2318 if( IKabs((((gconst7*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x200.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst7*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7))))))))))+IKsqr((new_r11*(x200.value)))-1) <= IKFAST_SINCOS_THRESH )
2319  continue;
2320 j5array[0]=IKatan2((((gconst7*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7))))))))), (new_r11*(x200.value)));
2321 sj5array[0]=IKsin(j5array[0]);
2322 cj5array[0]=IKcos(j5array[0]);
2323 if( j5array[0] > IKPI )
2324 {
2325  j5array[0]-=IK2PI;
2326 }
2327 else if( j5array[0] < -IKPI )
2328 { j5array[0]+=IK2PI;
2329 }
2330 j5valid[0] = true;
2331 for(int ij5 = 0; ij5 < 1; ++ij5)
2332 {
2333 if( !j5valid[ij5] )
2334 {
2335  continue;
2336 }
2337 _ij5[0] = ij5; _ij5[1] = -1;
2338 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2339 {
2340 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2341 {
2342  j5valid[iij5]=false; _ij5[1] = iij5; break;
2343 }
2344 }
2345 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2346 {
2347 IkReal evalcond[8];
2348 IkReal x201=IKcos(j5);
2349 IkReal x202=IKsin(j5);
2350 IkReal x203=((1.0)*x202);
2351 IkReal x204=((1.0)*x201);
2352 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2353 continue;
2354 IkReal x205=IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))));
2355 evalcond[0]=x201;
2356 evalcond[1]=((-1.0)*x202);
2357 evalcond[2]=(new_r11+(((-1.0)*gconst7*x204)));
2358 evalcond[3]=(new_r10+(((-1.0)*gconst7*x203)));
2359 evalcond[4]=((((-1.0)*x204*x205))+new_r01);
2360 evalcond[5]=((((-1.0)*x203*x205))+new_r00);
2361 evalcond[6]=(((new_r00*x205))+(((-1.0)*x203))+((gconst7*new_r10)));
2362 evalcond[7]=(((new_r01*x205))+(((-1.0)*x204))+((gconst7*new_r11)));
2363 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2364 {
2365 continue;
2366 }
2367 }
2368 
2369 {
2370 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2371 vinfos[0].jointtype = 1;
2372 vinfos[0].foffset = j0;
2373 vinfos[0].indices[0] = _ij0[0];
2374 vinfos[0].indices[1] = _ij0[1];
2375 vinfos[0].maxsolutions = _nj0;
2376 vinfos[1].jointtype = 1;
2377 vinfos[1].foffset = j1;
2378 vinfos[1].indices[0] = _ij1[0];
2379 vinfos[1].indices[1] = _ij1[1];
2380 vinfos[1].maxsolutions = _nj1;
2381 vinfos[2].jointtype = 1;
2382 vinfos[2].foffset = j2;
2383 vinfos[2].indices[0] = _ij2[0];
2384 vinfos[2].indices[1] = _ij2[1];
2385 vinfos[2].maxsolutions = _nj2;
2386 vinfos[3].jointtype = 1;
2387 vinfos[3].foffset = j3;
2388 vinfos[3].indices[0] = _ij3[0];
2389 vinfos[3].indices[1] = _ij3[1];
2390 vinfos[3].maxsolutions = _nj3;
2391 vinfos[4].jointtype = 1;
2392 vinfos[4].foffset = j4;
2393 vinfos[4].indices[0] = _ij4[0];
2394 vinfos[4].indices[1] = _ij4[1];
2395 vinfos[4].maxsolutions = _nj4;
2396 vinfos[5].jointtype = 1;
2397 vinfos[5].foffset = j5;
2398 vinfos[5].indices[0] = _ij5[0];
2399 vinfos[5].indices[1] = _ij5[1];
2400 vinfos[5].maxsolutions = _nj5;
2401 std::vector<int> vfree(0);
2402 solutions.AddSolution(vinfos,vfree);
2403 }
2404 }
2405 }
2406 
2407 } else
2408 {
2409 {
2410 IkReal j5array[1], cj5array[1], sj5array[1];
2411 bool j5valid[1]={false};
2412 _nj5 = 1;
2413 CheckValue<IkReal> x206=IKPowWithIntegerCheck(IKsign(gconst7),-1);
2414 if(!x206.valid){
2415 continue;
2416 }
2417 CheckValue<IkReal> x207 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2418 if(!x207.valid){
2419 continue;
2420 }
2421 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x206.value)))+(x207.value));
2422 sj5array[0]=IKsin(j5array[0]);
2423 cj5array[0]=IKcos(j5array[0]);
2424 if( j5array[0] > IKPI )
2425 {
2426  j5array[0]-=IK2PI;
2427 }
2428 else if( j5array[0] < -IKPI )
2429 { j5array[0]+=IK2PI;
2430 }
2431 j5valid[0] = true;
2432 for(int ij5 = 0; ij5 < 1; ++ij5)
2433 {
2434 if( !j5valid[ij5] )
2435 {
2436  continue;
2437 }
2438 _ij5[0] = ij5; _ij5[1] = -1;
2439 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2440 {
2441 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2442 {
2443  j5valid[iij5]=false; _ij5[1] = iij5; break;
2444 }
2445 }
2446 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2447 {
2448 IkReal evalcond[8];
2449 IkReal x208=IKcos(j5);
2450 IkReal x209=IKsin(j5);
2451 IkReal x210=((1.0)*x209);
2452 IkReal x211=((1.0)*x208);
2453 if((((1.0)+(((-1.0)*(gconst7*gconst7))))) < -0.00001)
2454 continue;
2455 IkReal x212=IKsqrt(((1.0)+(((-1.0)*(gconst7*gconst7)))));
2456 evalcond[0]=x208;
2457 evalcond[1]=((-1.0)*x209);
2458 evalcond[2]=((((-1.0)*gconst7*x211))+new_r11);
2459 evalcond[3]=((((-1.0)*gconst7*x210))+new_r10);
2460 evalcond[4]=((((-1.0)*x211*x212))+new_r01);
2461 evalcond[5]=((((-1.0)*x210*x212))+new_r00);
2462 evalcond[6]=(((new_r00*x212))+(((-1.0)*x210))+((gconst7*new_r10)));
2463 evalcond[7]=(((new_r01*x212))+(((-1.0)*x211))+((gconst7*new_r11)));
2464 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2465 {
2466 continue;
2467 }
2468 }
2469 
2470 {
2471 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2472 vinfos[0].jointtype = 1;
2473 vinfos[0].foffset = j0;
2474 vinfos[0].indices[0] = _ij0[0];
2475 vinfos[0].indices[1] = _ij0[1];
2476 vinfos[0].maxsolutions = _nj0;
2477 vinfos[1].jointtype = 1;
2478 vinfos[1].foffset = j1;
2479 vinfos[1].indices[0] = _ij1[0];
2480 vinfos[1].indices[1] = _ij1[1];
2481 vinfos[1].maxsolutions = _nj1;
2482 vinfos[2].jointtype = 1;
2483 vinfos[2].foffset = j2;
2484 vinfos[2].indices[0] = _ij2[0];
2485 vinfos[2].indices[1] = _ij2[1];
2486 vinfos[2].maxsolutions = _nj2;
2487 vinfos[3].jointtype = 1;
2488 vinfos[3].foffset = j3;
2489 vinfos[3].indices[0] = _ij3[0];
2490 vinfos[3].indices[1] = _ij3[1];
2491 vinfos[3].maxsolutions = _nj3;
2492 vinfos[4].jointtype = 1;
2493 vinfos[4].foffset = j4;
2494 vinfos[4].indices[0] = _ij4[0];
2495 vinfos[4].indices[1] = _ij4[1];
2496 vinfos[4].maxsolutions = _nj4;
2497 vinfos[5].jointtype = 1;
2498 vinfos[5].foffset = j5;
2499 vinfos[5].indices[0] = _ij5[0];
2500 vinfos[5].indices[1] = _ij5[1];
2501 vinfos[5].maxsolutions = _nj5;
2502 std::vector<int> vfree(0);
2503 solutions.AddSolution(vinfos,vfree);
2504 }
2505 }
2506 }
2507 
2508 }
2509 
2510 }
2511 
2512 }
2513 } while(0);
2514 if( bgotonextstatement )
2515 {
2516 bool bgotonextstatement = true;
2517 do
2518 {
2519 if( 1 )
2520 {
2521 bgotonextstatement=false;
2522 continue; // branch miss [j5]
2523 
2524 }
2525 } while(0);
2526 if( bgotonextstatement )
2527 {
2528 }
2529 }
2530 }
2531 }
2532 }
2533 }
2534 }
2535 }
2536 }
2537 }
2538 
2539 } else
2540 {
2541 {
2542 IkReal j5array[1], cj5array[1], sj5array[1];
2543 bool j5valid[1]={false};
2544 _nj5 = 1;
2545 IkReal x213=new_r22*new_r22;
2546 CheckValue<IkReal> x214=IKPowWithIntegerCheck((((cj3*x213))+(((-1.0)*cj3))),-1);
2547 if(!x214.valid){
2548 continue;
2549 }
2550 CheckValue<IkReal> x215=IKPowWithIntegerCheck(((((-1.0)*sj3))+((sj3*x213))),-1);
2551 if(!x215.valid){
2552 continue;
2553 }
2554 if( IKabs(((x214.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x215.value)*((((new_r10*new_r22))+new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x214.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))))+IKsqr(((x215.value)*((((new_r10*new_r22))+new_r01))))-1) <= IKFAST_SINCOS_THRESH )
2555  continue;
2556 j5array[0]=IKatan2(((x214.value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))), ((x215.value)*((((new_r10*new_r22))+new_r01))));
2557 sj5array[0]=IKsin(j5array[0]);
2558 cj5array[0]=IKcos(j5array[0]);
2559 if( j5array[0] > IKPI )
2560 {
2561  j5array[0]-=IK2PI;
2562 }
2563 else if( j5array[0] < -IKPI )
2564 { j5array[0]+=IK2PI;
2565 }
2566 j5valid[0] = true;
2567 for(int ij5 = 0; ij5 < 1; ++ij5)
2568 {
2569 if( !j5valid[ij5] )
2570 {
2571  continue;
2572 }
2573 _ij5[0] = ij5; _ij5[1] = -1;
2574 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2575 {
2576 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2577 {
2578  j5valid[iij5]=false; _ij5[1] = iij5; break;
2579 }
2580 }
2581 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2582 {
2583 IkReal evalcond[10];
2584 IkReal x216=IKsin(j5);
2585 IkReal x217=IKcos(j5);
2586 IkReal x218=((1.0)*sj3);
2587 IkReal x219=((1.0)*x216);
2588 IkReal x220=((1.0)*cj3*new_r22);
2589 IkReal x221=(sj3*x216);
2590 IkReal x222=((1.0)*x217);
2591 IkReal x223=(new_r22*x216);
2592 evalcond[0]=(((new_r11*sj3))+x223+((cj3*new_r01)));
2593 evalcond[1]=(((cj3*new_r10))+(((-1.0)*x219))+(((-1.0)*new_r00*x218)));
2594 evalcond[2]=((((-1.0)*new_r01*x218))+((cj3*new_r11))+(((-1.0)*x222)));
2595 evalcond[3]=(((cj3*x223))+((sj3*x217))+new_r01);
2596 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*new_r22*x222)));
2597 evalcond[5]=(x221+(((-1.0)*x217*x220))+new_r00);
2598 evalcond[6]=(((new_r22*x221))+(((-1.0)*cj3*x222))+new_r11);
2599 evalcond[7]=(x217+(((-1.0)*new_r00*x220))+(((-1.0)*new_r10*new_r22*x218)));
2600 evalcond[8]=((((-1.0)*cj3*x219))+new_r10+(((-1.0)*new_r22*x217*x218)));
2601 evalcond[9]=((((-1.0)*new_r01*x220))+(((-1.0)*new_r11*new_r22*x218))+(((-1.0)*x219)));
2602 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2603 {
2604 continue;
2605 }
2606 }
2607 
2608 {
2609 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2610 vinfos[0].jointtype = 1;
2611 vinfos[0].foffset = j0;
2612 vinfos[0].indices[0] = _ij0[0];
2613 vinfos[0].indices[1] = _ij0[1];
2614 vinfos[0].maxsolutions = _nj0;
2615 vinfos[1].jointtype = 1;
2616 vinfos[1].foffset = j1;
2617 vinfos[1].indices[0] = _ij1[0];
2618 vinfos[1].indices[1] = _ij1[1];
2619 vinfos[1].maxsolutions = _nj1;
2620 vinfos[2].jointtype = 1;
2621 vinfos[2].foffset = j2;
2622 vinfos[2].indices[0] = _ij2[0];
2623 vinfos[2].indices[1] = _ij2[1];
2624 vinfos[2].maxsolutions = _nj2;
2625 vinfos[3].jointtype = 1;
2626 vinfos[3].foffset = j3;
2627 vinfos[3].indices[0] = _ij3[0];
2628 vinfos[3].indices[1] = _ij3[1];
2629 vinfos[3].maxsolutions = _nj3;
2630 vinfos[4].jointtype = 1;
2631 vinfos[4].foffset = j4;
2632 vinfos[4].indices[0] = _ij4[0];
2633 vinfos[4].indices[1] = _ij4[1];
2634 vinfos[4].maxsolutions = _nj4;
2635 vinfos[5].jointtype = 1;
2636 vinfos[5].foffset = j5;
2637 vinfos[5].indices[0] = _ij5[0];
2638 vinfos[5].indices[1] = _ij5[1];
2639 vinfos[5].maxsolutions = _nj5;
2640 std::vector<int> vfree(0);
2641 solutions.AddSolution(vinfos,vfree);
2642 }
2643 }
2644 }
2645 
2646 }
2647 
2648 }
2649 
2650 } else
2651 {
2652 {
2653 IkReal j5array[1], cj5array[1], sj5array[1];
2654 bool j5valid[1]={false};
2655 _nj5 = 1;
2656 IkReal x224=((1.0)*new_r01);
2657 CheckValue<IkReal> x225=IKPowWithIntegerCheck(new_r22,-1);
2658 if(!x225.valid){
2659 continue;
2660 }
2661 if( IKabs(((x225.value)*(((((-1.0)*cj3*x224))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x224))+((cj3*new_r11)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x225.value)*(((((-1.0)*cj3*x224))+(((-1.0)*new_r11*sj3))))))+IKsqr(((((-1.0)*sj3*x224))+((cj3*new_r11))))-1) <= IKFAST_SINCOS_THRESH )
2662  continue;
2663 j5array[0]=IKatan2(((x225.value)*(((((-1.0)*cj3*x224))+(((-1.0)*new_r11*sj3))))), ((((-1.0)*sj3*x224))+((cj3*new_r11))));
2664 sj5array[0]=IKsin(j5array[0]);
2665 cj5array[0]=IKcos(j5array[0]);
2666 if( j5array[0] > IKPI )
2667 {
2668  j5array[0]-=IK2PI;
2669 }
2670 else if( j5array[0] < -IKPI )
2671 { j5array[0]+=IK2PI;
2672 }
2673 j5valid[0] = true;
2674 for(int ij5 = 0; ij5 < 1; ++ij5)
2675 {
2676 if( !j5valid[ij5] )
2677 {
2678  continue;
2679 }
2680 _ij5[0] = ij5; _ij5[1] = -1;
2681 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2682 {
2683 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2684 {
2685  j5valid[iij5]=false; _ij5[1] = iij5; break;
2686 }
2687 }
2688 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2689 {
2690 IkReal evalcond[10];
2691 IkReal x226=IKsin(j5);
2692 IkReal x227=IKcos(j5);
2693 IkReal x228=((1.0)*sj3);
2694 IkReal x229=((1.0)*x226);
2695 IkReal x230=((1.0)*cj3*new_r22);
2696 IkReal x231=(sj3*x226);
2697 IkReal x232=((1.0)*x227);
2698 IkReal x233=(new_r22*x226);
2699 evalcond[0]=(((new_r11*sj3))+x233+((cj3*new_r01)));
2700 evalcond[1]=(((cj3*new_r10))+(((-1.0)*x229))+(((-1.0)*new_r00*x228)));
2701 evalcond[2]=((((-1.0)*new_r01*x228))+(((-1.0)*x232))+((cj3*new_r11)));
2702 evalcond[3]=(((cj3*x233))+((sj3*x227))+new_r01);
2703 evalcond[4]=((((-1.0)*new_r22*x232))+((new_r10*sj3))+((cj3*new_r00)));
2704 evalcond[5]=((((-1.0)*x227*x230))+x231+new_r00);
2705 evalcond[6]=(((new_r22*x231))+(((-1.0)*cj3*x232))+new_r11);
2706 evalcond[7]=(x227+(((-1.0)*new_r10*new_r22*x228))+(((-1.0)*new_r00*x230)));
2707 evalcond[8]=((((-1.0)*new_r22*x227*x228))+(((-1.0)*cj3*x229))+new_r10);
2708 evalcond[9]=((((-1.0)*new_r11*new_r22*x228))+(((-1.0)*new_r01*x230))+(((-1.0)*x229)));
2709 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2710 {
2711 continue;
2712 }
2713 }
2714 
2715 {
2716 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2717 vinfos[0].jointtype = 1;
2718 vinfos[0].foffset = j0;
2719 vinfos[0].indices[0] = _ij0[0];
2720 vinfos[0].indices[1] = _ij0[1];
2721 vinfos[0].maxsolutions = _nj0;
2722 vinfos[1].jointtype = 1;
2723 vinfos[1].foffset = j1;
2724 vinfos[1].indices[0] = _ij1[0];
2725 vinfos[1].indices[1] = _ij1[1];
2726 vinfos[1].maxsolutions = _nj1;
2727 vinfos[2].jointtype = 1;
2728 vinfos[2].foffset = j2;
2729 vinfos[2].indices[0] = _ij2[0];
2730 vinfos[2].indices[1] = _ij2[1];
2731 vinfos[2].maxsolutions = _nj2;
2732 vinfos[3].jointtype = 1;
2733 vinfos[3].foffset = j3;
2734 vinfos[3].indices[0] = _ij3[0];
2735 vinfos[3].indices[1] = _ij3[1];
2736 vinfos[3].maxsolutions = _nj3;
2737 vinfos[4].jointtype = 1;
2738 vinfos[4].foffset = j4;
2739 vinfos[4].indices[0] = _ij4[0];
2740 vinfos[4].indices[1] = _ij4[1];
2741 vinfos[4].maxsolutions = _nj4;
2742 vinfos[5].jointtype = 1;
2743 vinfos[5].foffset = j5;
2744 vinfos[5].indices[0] = _ij5[0];
2745 vinfos[5].indices[1] = _ij5[1];
2746 vinfos[5].maxsolutions = _nj5;
2747 std::vector<int> vfree(0);
2748 solutions.AddSolution(vinfos,vfree);
2749 }
2750 }
2751 }
2752 
2753 }
2754 
2755 }
2756 
2757 } else
2758 {
2759 {
2760 IkReal j5array[1], cj5array[1], sj5array[1];
2761 bool j5valid[1]={false};
2762 _nj5 = 1;
2763 IkReal x234=cj3*cj3;
2764 IkReal x235=(cj3*new_r22);
2765 CheckValue<IkReal> x236 = IKatan2WithCheck(IkReal((((new_r01*x235))+((new_r00*sj3)))),IkReal(((((-1.0)*new_r00*x235))+((new_r01*sj3)))),IKFAST_ATAN2_MAGTHRESH);
2766 if(!x236.valid){
2767 continue;
2768 }
2769 CheckValue<IkReal> x237=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x234*(new_r22*new_r22)))+x234)),-1);
2770 if(!x237.valid){
2771 continue;
2772 }
2773 j5array[0]=((-1.5707963267949)+(x236.value)+(((1.5707963267949)*(x237.value))));
2774 sj5array[0]=IKsin(j5array[0]);
2775 cj5array[0]=IKcos(j5array[0]);
2776 if( j5array[0] > IKPI )
2777 {
2778  j5array[0]-=IK2PI;
2779 }
2780 else if( j5array[0] < -IKPI )
2781 { j5array[0]+=IK2PI;
2782 }
2783 j5valid[0] = true;
2784 for(int ij5 = 0; ij5 < 1; ++ij5)
2785 {
2786 if( !j5valid[ij5] )
2787 {
2788  continue;
2789 }
2790 _ij5[0] = ij5; _ij5[1] = -1;
2791 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2792 {
2793 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2794 {
2795  j5valid[iij5]=false; _ij5[1] = iij5; break;
2796 }
2797 }
2798 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2799 {
2800 IkReal evalcond[10];
2801 IkReal x238=IKsin(j5);
2802 IkReal x239=IKcos(j5);
2803 IkReal x240=((1.0)*sj3);
2804 IkReal x241=((1.0)*x238);
2805 IkReal x242=((1.0)*cj3*new_r22);
2806 IkReal x243=(sj3*x238);
2807 IkReal x244=((1.0)*x239);
2808 IkReal x245=(new_r22*x238);
2809 evalcond[0]=(((new_r11*sj3))+x245+((cj3*new_r01)));
2810 evalcond[1]=((((-1.0)*new_r00*x240))+((cj3*new_r10))+(((-1.0)*x241)));
2811 evalcond[2]=((((-1.0)*new_r01*x240))+((cj3*new_r11))+(((-1.0)*x244)));
2812 evalcond[3]=(new_r01+((cj3*x245))+((sj3*x239)));
2813 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x244))+((cj3*new_r00)));
2814 evalcond[5]=(x243+new_r00+(((-1.0)*x239*x242)));
2815 evalcond[6]=(((new_r22*x243))+(((-1.0)*cj3*x244))+new_r11);
2816 evalcond[7]=((((-1.0)*new_r10*new_r22*x240))+(((-1.0)*new_r00*x242))+x239);
2817 evalcond[8]=((((-1.0)*cj3*x241))+(((-1.0)*new_r22*x239*x240))+new_r10);
2818 evalcond[9]=((((-1.0)*new_r11*new_r22*x240))+(((-1.0)*new_r01*x242))+(((-1.0)*x241)));
2819 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2820 {
2821 continue;
2822 }
2823 }
2824 
2825 {
2826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2827 vinfos[0].jointtype = 1;
2828 vinfos[0].foffset = j0;
2829 vinfos[0].indices[0] = _ij0[0];
2830 vinfos[0].indices[1] = _ij0[1];
2831 vinfos[0].maxsolutions = _nj0;
2832 vinfos[1].jointtype = 1;
2833 vinfos[1].foffset = j1;
2834 vinfos[1].indices[0] = _ij1[0];
2835 vinfos[1].indices[1] = _ij1[1];
2836 vinfos[1].maxsolutions = _nj1;
2837 vinfos[2].jointtype = 1;
2838 vinfos[2].foffset = j2;
2839 vinfos[2].indices[0] = _ij2[0];
2840 vinfos[2].indices[1] = _ij2[1];
2841 vinfos[2].maxsolutions = _nj2;
2842 vinfos[3].jointtype = 1;
2843 vinfos[3].foffset = j3;
2844 vinfos[3].indices[0] = _ij3[0];
2845 vinfos[3].indices[1] = _ij3[1];
2846 vinfos[3].maxsolutions = _nj3;
2847 vinfos[4].jointtype = 1;
2848 vinfos[4].foffset = j4;
2849 vinfos[4].indices[0] = _ij4[0];
2850 vinfos[4].indices[1] = _ij4[1];
2851 vinfos[4].maxsolutions = _nj4;
2852 vinfos[5].jointtype = 1;
2853 vinfos[5].foffset = j5;
2854 vinfos[5].indices[0] = _ij5[0];
2855 vinfos[5].indices[1] = _ij5[1];
2856 vinfos[5].maxsolutions = _nj5;
2857 std::vector<int> vfree(0);
2858 solutions.AddSolution(vinfos,vfree);
2859 }
2860 }
2861 }
2862 
2863 }
2864 
2865 }
2866  }
2867 
2868 }
2869 
2870 }
2871 
2872 }
2873 } while(0);
2874 if( bgotonextstatement )
2875 {
2876 bool bgotonextstatement = true;
2877 do
2878 {
2879 if( 1 )
2880 {
2881 bgotonextstatement=false;
2882 continue; // branch miss [j3, j5]
2883 
2884 }
2885 } while(0);
2886 if( bgotonextstatement )
2887 {
2888 }
2889 }
2890 }
2891 }
2892 }
2893 
2894 } else
2895 {
2896 {
2897 IkReal j3array[1], cj3array[1], sj3array[1];
2898 bool j3valid[1]={false};
2899 _nj3 = 1;
2900 CheckValue<IkReal> x247=IKPowWithIntegerCheck(sj4,-1);
2901 if(!x247.valid){
2902 continue;
2903 }
2904 IkReal x246=x247.value;
2905 CheckValue<IkReal> x248=IKPowWithIntegerCheck(new_r12,-1);
2906 if(!x248.valid){
2907 continue;
2908 }
2909 if( IKabs((x246*(x248.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r02*new_r02))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r02*x246)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x246*(x248.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r02*new_r02)))))))+IKsqr((new_r02*x246))-1) <= IKFAST_SINCOS_THRESH )
2910  continue;
2911 j3array[0]=IKatan2((x246*(x248.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r02*new_r02)))))), (new_r02*x246));
2912 sj3array[0]=IKsin(j3array[0]);
2913 cj3array[0]=IKcos(j3array[0]);
2914 if( j3array[0] > IKPI )
2915 {
2916  j3array[0]-=IK2PI;
2917 }
2918 else if( j3array[0] < -IKPI )
2919 { j3array[0]+=IK2PI;
2920 }
2921 j3valid[0] = true;
2922 for(int ij3 = 0; ij3 < 1; ++ij3)
2923 {
2924 if( !j3valid[ij3] )
2925 {
2926  continue;
2927 }
2928 _ij3[0] = ij3; _ij3[1] = -1;
2929 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2930 {
2931 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2932 {
2933  j3valid[iij3]=false; _ij3[1] = iij3; break;
2934 }
2935 }
2936 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2937 {
2938 IkReal evalcond[8];
2939 IkReal x249=IKcos(j3);
2940 IkReal x250=IKsin(j3);
2941 IkReal x251=((1.0)*sj4);
2942 IkReal x252=((1.0)*cj4);
2943 IkReal x253=(new_r12*x250);
2944 IkReal x254=(new_r02*x249);
2945 evalcond[0]=((((-1.0)*x249*x251))+new_r02);
2946 evalcond[1]=((((-1.0)*x250*x251))+new_r12);
2947 evalcond[2]=(((new_r12*x249))+(((-1.0)*new_r02*x250)));
2948 evalcond[3]=(x254+x253+(((-1.0)*x251)));
2949 evalcond[4]=((((-1.0)*x252*x254))+(((-1.0)*x252*x253))+((new_r22*sj4)));
2950 evalcond[5]=((((-1.0)*new_r10*x250*x251))+(((-1.0)*new_r00*x249*x251))+(((-1.0)*new_r20*x252)));
2951 evalcond[6]=((((-1.0)*new_r21*x252))+(((-1.0)*new_r01*x249*x251))+(((-1.0)*new_r11*x250*x251)));
2952 evalcond[7]=((1.0)+(((-1.0)*x251*x253))+(((-1.0)*x251*x254))+(((-1.0)*new_r22*x252)));
2953 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2954 {
2955 continue;
2956 }
2957 }
2958 
2959 {
2960 IkReal j5eval[3];
2961 j5eval[0]=sj4;
2962 j5eval[1]=IKsign(sj4);
2963 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2964 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2965 {
2966 {
2967 IkReal j5eval[2];
2968 j5eval[0]=sj3;
2969 j5eval[1]=sj4;
2970 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2971 {
2972 {
2973 IkReal j5eval[3];
2974 j5eval[0]=cj3;
2975 j5eval[1]=cj4;
2976 j5eval[2]=sj4;
2977 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2978 {
2979 {
2980 IkReal evalcond[5];
2981 bool bgotonextstatement = true;
2982 do
2983 {
2984 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
2985 evalcond[1]=new_r02;
2986 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
2987 {
2988 bgotonextstatement=false;
2989 {
2990 IkReal j5eval[3];
2991 sj3=1.0;
2992 cj3=0;
2993 j3=1.5707963267949;
2994 j5eval[0]=sj4;
2995 j5eval[1]=IKsign(sj4);
2996 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2997 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2998 {
2999 {
3000 IkReal j5eval[3];
3001 sj3=1.0;
3002 cj3=0;
3003 j3=1.5707963267949;
3004 j5eval[0]=cj4;
3005 j5eval[1]=IKsign(cj4);
3006 j5eval[2]=((IKabs(new_r11))+(IKabs(new_r10)));
3007 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
3008 {
3009 {
3010 IkReal j5eval[1];
3011 sj3=1.0;
3012 cj3=0;
3013 j3=1.5707963267949;
3014 j5eval[0]=sj4;
3015 if( IKabs(j5eval[0]) < 0.0000010000000000 )
3016 {
3017 {
3018 IkReal evalcond[4];
3019 bool bgotonextstatement = true;
3020 do
3021 {
3022 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
3023 evalcond[1]=new_r20;
3024 evalcond[2]=new_r12;
3025 evalcond[3]=new_r21;
3026 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3027 {
3028 bgotonextstatement=false;
3029 {
3030 IkReal j5array[1], cj5array[1], sj5array[1];
3031 bool j5valid[1]={false};
3032 _nj5 = 1;
3033 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
3034  continue;
3035 j5array[0]=IKatan2(((-1.0)*new_r11), new_r10);
3036 sj5array[0]=IKsin(j5array[0]);
3037 cj5array[0]=IKcos(j5array[0]);
3038 if( j5array[0] > IKPI )
3039 {
3040  j5array[0]-=IK2PI;
3041 }
3042 else if( j5array[0] < -IKPI )
3043 { j5array[0]+=IK2PI;
3044 }
3045 j5valid[0] = true;
3046 for(int ij5 = 0; ij5 < 1; ++ij5)
3047 {
3048 if( !j5valid[ij5] )
3049 {
3050  continue;
3051 }
3052 _ij5[0] = ij5; _ij5[1] = -1;
3053 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3054 {
3055 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3056 {
3057  j5valid[iij5]=false; _ij5[1] = iij5; break;
3058 }
3059 }
3060 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3061 {
3062 IkReal evalcond[4];
3063 IkReal x255=IKsin(j5);
3064 IkReal x256=((1.0)*(IKcos(j5)));
3065 evalcond[0]=(x255+new_r11);
3066 evalcond[1]=(new_r10+(((-1.0)*x256)));
3067 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x255)));
3068 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x256)));
3069 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3070 {
3071 continue;
3072 }
3073 }
3074 
3075 {
3076 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3077 vinfos[0].jointtype = 1;
3078 vinfos[0].foffset = j0;
3079 vinfos[0].indices[0] = _ij0[0];
3080 vinfos[0].indices[1] = _ij0[1];
3081 vinfos[0].maxsolutions = _nj0;
3082 vinfos[1].jointtype = 1;
3083 vinfos[1].foffset = j1;
3084 vinfos[1].indices[0] = _ij1[0];
3085 vinfos[1].indices[1] = _ij1[1];
3086 vinfos[1].maxsolutions = _nj1;
3087 vinfos[2].jointtype = 1;
3088 vinfos[2].foffset = j2;
3089 vinfos[2].indices[0] = _ij2[0];
3090 vinfos[2].indices[1] = _ij2[1];
3091 vinfos[2].maxsolutions = _nj2;
3092 vinfos[3].jointtype = 1;
3093 vinfos[3].foffset = j3;
3094 vinfos[3].indices[0] = _ij3[0];
3095 vinfos[3].indices[1] = _ij3[1];
3096 vinfos[3].maxsolutions = _nj3;
3097 vinfos[4].jointtype = 1;
3098 vinfos[4].foffset = j4;
3099 vinfos[4].indices[0] = _ij4[0];
3100 vinfos[4].indices[1] = _ij4[1];
3101 vinfos[4].maxsolutions = _nj4;
3102 vinfos[5].jointtype = 1;
3103 vinfos[5].foffset = j5;
3104 vinfos[5].indices[0] = _ij5[0];
3105 vinfos[5].indices[1] = _ij5[1];
3106 vinfos[5].maxsolutions = _nj5;
3107 std::vector<int> vfree(0);
3108 solutions.AddSolution(vinfos,vfree);
3109 }
3110 }
3111 }
3112 
3113 }
3114 } while(0);
3115 if( bgotonextstatement )
3116 {
3117 bool bgotonextstatement = true;
3118 do
3119 {
3120 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
3121 evalcond[1]=new_r20;
3122 evalcond[2]=new_r12;
3123 evalcond[3]=new_r21;
3124 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3125 {
3126 bgotonextstatement=false;
3127 {
3128 IkReal j5array[1], cj5array[1], sj5array[1];
3129 bool j5valid[1]={false};
3130 _nj5 = 1;
3131 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
3132  continue;
3133 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
3134 sj5array[0]=IKsin(j5array[0]);
3135 cj5array[0]=IKcos(j5array[0]);
3136 if( j5array[0] > IKPI )
3137 {
3138  j5array[0]-=IK2PI;
3139 }
3140 else if( j5array[0] < -IKPI )
3141 { j5array[0]+=IK2PI;
3142 }
3143 j5valid[0] = true;
3144 for(int ij5 = 0; ij5 < 1; ++ij5)
3145 {
3146 if( !j5valid[ij5] )
3147 {
3148  continue;
3149 }
3150 _ij5[0] = ij5; _ij5[1] = -1;
3151 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3152 {
3153 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3154 {
3155  j5valid[iij5]=false; _ij5[1] = iij5; break;
3156 }
3157 }
3158 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3159 {
3160 IkReal evalcond[4];
3161 IkReal x257=IKcos(j5);
3162 IkReal x258=((1.0)*(IKsin(j5)));
3163 evalcond[0]=(x257+new_r10);
3164 evalcond[1]=(new_r11+(((-1.0)*x258)));
3165 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x258)));
3166 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x257)));
3167 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3168 {
3169 continue;
3170 }
3171 }
3172 
3173 {
3174 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3175 vinfos[0].jointtype = 1;
3176 vinfos[0].foffset = j0;
3177 vinfos[0].indices[0] = _ij0[0];
3178 vinfos[0].indices[1] = _ij0[1];
3179 vinfos[0].maxsolutions = _nj0;
3180 vinfos[1].jointtype = 1;
3181 vinfos[1].foffset = j1;
3182 vinfos[1].indices[0] = _ij1[0];
3183 vinfos[1].indices[1] = _ij1[1];
3184 vinfos[1].maxsolutions = _nj1;
3185 vinfos[2].jointtype = 1;
3186 vinfos[2].foffset = j2;
3187 vinfos[2].indices[0] = _ij2[0];
3188 vinfos[2].indices[1] = _ij2[1];
3189 vinfos[2].maxsolutions = _nj2;
3190 vinfos[3].jointtype = 1;
3191 vinfos[3].foffset = j3;
3192 vinfos[3].indices[0] = _ij3[0];
3193 vinfos[3].indices[1] = _ij3[1];
3194 vinfos[3].maxsolutions = _nj3;
3195 vinfos[4].jointtype = 1;
3196 vinfos[4].foffset = j4;
3197 vinfos[4].indices[0] = _ij4[0];
3198 vinfos[4].indices[1] = _ij4[1];
3199 vinfos[4].maxsolutions = _nj4;
3200 vinfos[5].jointtype = 1;
3201 vinfos[5].foffset = j5;
3202 vinfos[5].indices[0] = _ij5[0];
3203 vinfos[5].indices[1] = _ij5[1];
3204 vinfos[5].maxsolutions = _nj5;
3205 std::vector<int> vfree(0);
3206 solutions.AddSolution(vinfos,vfree);
3207 }
3208 }
3209 }
3210 
3211 }
3212 } while(0);
3213 if( bgotonextstatement )
3214 {
3215 bool bgotonextstatement = true;
3216 do
3217 {
3218 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
3219 evalcond[1]=new_r22;
3220 evalcond[2]=new_r11;
3221 evalcond[3]=new_r10;
3222 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3223 {
3224 bgotonextstatement=false;
3225 {
3226 IkReal j5array[1], cj5array[1], sj5array[1];
3227 bool j5valid[1]={false};
3228 _nj5 = 1;
3229 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
3230  continue;
3231 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
3232 sj5array[0]=IKsin(j5array[0]);
3233 cj5array[0]=IKcos(j5array[0]);
3234 if( j5array[0] > IKPI )
3235 {
3236  j5array[0]-=IK2PI;
3237 }
3238 else if( j5array[0] < -IKPI )
3239 { j5array[0]+=IK2PI;
3240 }
3241 j5valid[0] = true;
3242 for(int ij5 = 0; ij5 < 1; ++ij5)
3243 {
3244 if( !j5valid[ij5] )
3245 {
3246  continue;
3247 }
3248 _ij5[0] = ij5; _ij5[1] = -1;
3249 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3250 {
3251 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3252 {
3253  j5valid[iij5]=false; _ij5[1] = iij5; break;
3254 }
3255 }
3256 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3257 {
3258 IkReal evalcond[4];
3259 IkReal x259=IKcos(j5);
3260 IkReal x260=((1.0)*(IKsin(j5)));
3261 evalcond[0]=(x259+new_r20);
3262 evalcond[1]=(new_r21+(((-1.0)*x260)));
3263 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x260)));
3264 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x259)));
3265 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3266 {
3267 continue;
3268 }
3269 }
3270 
3271 {
3272 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3273 vinfos[0].jointtype = 1;
3274 vinfos[0].foffset = j0;
3275 vinfos[0].indices[0] = _ij0[0];
3276 vinfos[0].indices[1] = _ij0[1];
3277 vinfos[0].maxsolutions = _nj0;
3278 vinfos[1].jointtype = 1;
3279 vinfos[1].foffset = j1;
3280 vinfos[1].indices[0] = _ij1[0];
3281 vinfos[1].indices[1] = _ij1[1];
3282 vinfos[1].maxsolutions = _nj1;
3283 vinfos[2].jointtype = 1;
3284 vinfos[2].foffset = j2;
3285 vinfos[2].indices[0] = _ij2[0];
3286 vinfos[2].indices[1] = _ij2[1];
3287 vinfos[2].maxsolutions = _nj2;
3288 vinfos[3].jointtype = 1;
3289 vinfos[3].foffset = j3;
3290 vinfos[3].indices[0] = _ij3[0];
3291 vinfos[3].indices[1] = _ij3[1];
3292 vinfos[3].maxsolutions = _nj3;
3293 vinfos[4].jointtype = 1;
3294 vinfos[4].foffset = j4;
3295 vinfos[4].indices[0] = _ij4[0];
3296 vinfos[4].indices[1] = _ij4[1];
3297 vinfos[4].maxsolutions = _nj4;
3298 vinfos[5].jointtype = 1;
3299 vinfos[5].foffset = j5;
3300 vinfos[5].indices[0] = _ij5[0];
3301 vinfos[5].indices[1] = _ij5[1];
3302 vinfos[5].maxsolutions = _nj5;
3303 std::vector<int> vfree(0);
3304 solutions.AddSolution(vinfos,vfree);
3305 }
3306 }
3307 }
3308 
3309 }
3310 } while(0);
3311 if( bgotonextstatement )
3312 {
3313 bool bgotonextstatement = true;
3314 do
3315 {
3316 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
3317 evalcond[1]=new_r22;
3318 evalcond[2]=new_r11;
3319 evalcond[3]=new_r10;
3320 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3321 {
3322 bgotonextstatement=false;
3323 {
3324 IkReal j5array[1], cj5array[1], sj5array[1];
3325 bool j5valid[1]={false};
3326 _nj5 = 1;
3327 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
3328  continue;
3329 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
3330 sj5array[0]=IKsin(j5array[0]);
3331 cj5array[0]=IKcos(j5array[0]);
3332 if( j5array[0] > IKPI )
3333 {
3334  j5array[0]-=IK2PI;
3335 }
3336 else if( j5array[0] < -IKPI )
3337 { j5array[0]+=IK2PI;
3338 }
3339 j5valid[0] = true;
3340 for(int ij5 = 0; ij5 < 1; ++ij5)
3341 {
3342 if( !j5valid[ij5] )
3343 {
3344  continue;
3345 }
3346 _ij5[0] = ij5; _ij5[1] = -1;
3347 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3348 {
3349 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3350 {
3351  j5valid[iij5]=false; _ij5[1] = iij5; break;
3352 }
3353 }
3354 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3355 {
3356 IkReal evalcond[4];
3357 IkReal x261=IKsin(j5);
3358 IkReal x262=((1.0)*(IKcos(j5)));
3359 evalcond[0]=(x261+new_r21);
3360 evalcond[1]=(new_r20+(((-1.0)*x262)));
3361 evalcond[2]=((((-1.0)*x261))+(((-1.0)*new_r00)));
3362 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x262)));
3363 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3364 {
3365 continue;
3366 }
3367 }
3368 
3369 {
3370 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3371 vinfos[0].jointtype = 1;
3372 vinfos[0].foffset = j0;
3373 vinfos[0].indices[0] = _ij0[0];
3374 vinfos[0].indices[1] = _ij0[1];
3375 vinfos[0].maxsolutions = _nj0;
3376 vinfos[1].jointtype = 1;
3377 vinfos[1].foffset = j1;
3378 vinfos[1].indices[0] = _ij1[0];
3379 vinfos[1].indices[1] = _ij1[1];
3380 vinfos[1].maxsolutions = _nj1;
3381 vinfos[2].jointtype = 1;
3382 vinfos[2].foffset = j2;
3383 vinfos[2].indices[0] = _ij2[0];
3384 vinfos[2].indices[1] = _ij2[1];
3385 vinfos[2].maxsolutions = _nj2;
3386 vinfos[3].jointtype = 1;
3387 vinfos[3].foffset = j3;
3388 vinfos[3].indices[0] = _ij3[0];
3389 vinfos[3].indices[1] = _ij3[1];
3390 vinfos[3].maxsolutions = _nj3;
3391 vinfos[4].jointtype = 1;
3392 vinfos[4].foffset = j4;
3393 vinfos[4].indices[0] = _ij4[0];
3394 vinfos[4].indices[1] = _ij4[1];
3395 vinfos[4].maxsolutions = _nj4;
3396 vinfos[5].jointtype = 1;
3397 vinfos[5].foffset = j5;
3398 vinfos[5].indices[0] = _ij5[0];
3399 vinfos[5].indices[1] = _ij5[1];
3400 vinfos[5].maxsolutions = _nj5;
3401 std::vector<int> vfree(0);
3402 solutions.AddSolution(vinfos,vfree);
3403 }
3404 }
3405 }
3406 
3407 }
3408 } while(0);
3409 if( bgotonextstatement )
3410 {
3411 bool bgotonextstatement = true;
3412 do
3413 {
3414 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3415 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3416 {
3417 bgotonextstatement=false;
3418 {
3419 IkReal j5array[1], cj5array[1], sj5array[1];
3420 bool j5valid[1]={false};
3421 _nj5 = 1;
3422 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 )
3423  continue;
3424 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3425 sj5array[0]=IKsin(j5array[0]);
3426 cj5array[0]=IKcos(j5array[0]);
3427 if( j5array[0] > IKPI )
3428 {
3429  j5array[0]-=IK2PI;
3430 }
3431 else if( j5array[0] < -IKPI )
3432 { j5array[0]+=IK2PI;
3433 }
3434 j5valid[0] = true;
3435 for(int ij5 = 0; ij5 < 1; ++ij5)
3436 {
3437 if( !j5valid[ij5] )
3438 {
3439  continue;
3440 }
3441 _ij5[0] = ij5; _ij5[1] = -1;
3442 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3443 {
3444 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3445 {
3446  j5valid[iij5]=false; _ij5[1] = iij5; break;
3447 }
3448 }
3449 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3450 {
3451 IkReal evalcond[6];
3452 IkReal x263=IKsin(j5);
3453 IkReal x264=IKcos(j5);
3454 evalcond[0]=x264;
3455 evalcond[1]=(new_r22*x263);
3456 evalcond[2]=((-1.0)*x263);
3457 evalcond[3]=((-1.0)*new_r22*x264);
3458 evalcond[4]=((((-1.0)*x263))+(((-1.0)*new_r00)));
3459 evalcond[5]=((((-1.0)*x264))+(((-1.0)*new_r01)));
3460 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
3461 {
3462 continue;
3463 }
3464 }
3465 
3466 {
3467 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3468 vinfos[0].jointtype = 1;
3469 vinfos[0].foffset = j0;
3470 vinfos[0].indices[0] = _ij0[0];
3471 vinfos[0].indices[1] = _ij0[1];
3472 vinfos[0].maxsolutions = _nj0;
3473 vinfos[1].jointtype = 1;
3474 vinfos[1].foffset = j1;
3475 vinfos[1].indices[0] = _ij1[0];
3476 vinfos[1].indices[1] = _ij1[1];
3477 vinfos[1].maxsolutions = _nj1;
3478 vinfos[2].jointtype = 1;
3479 vinfos[2].foffset = j2;
3480 vinfos[2].indices[0] = _ij2[0];
3481 vinfos[2].indices[1] = _ij2[1];
3482 vinfos[2].maxsolutions = _nj2;
3483 vinfos[3].jointtype = 1;
3484 vinfos[3].foffset = j3;
3485 vinfos[3].indices[0] = _ij3[0];
3486 vinfos[3].indices[1] = _ij3[1];
3487 vinfos[3].maxsolutions = _nj3;
3488 vinfos[4].jointtype = 1;
3489 vinfos[4].foffset = j4;
3490 vinfos[4].indices[0] = _ij4[0];
3491 vinfos[4].indices[1] = _ij4[1];
3492 vinfos[4].maxsolutions = _nj4;
3493 vinfos[5].jointtype = 1;
3494 vinfos[5].foffset = j5;
3495 vinfos[5].indices[0] = _ij5[0];
3496 vinfos[5].indices[1] = _ij5[1];
3497 vinfos[5].maxsolutions = _nj5;
3498 std::vector<int> vfree(0);
3499 solutions.AddSolution(vinfos,vfree);
3500 }
3501 }
3502 }
3503 
3504 }
3505 } while(0);
3506 if( bgotonextstatement )
3507 {
3508 bool bgotonextstatement = true;
3509 do
3510 {
3511 if( 1 )
3512 {
3513 bgotonextstatement=false;
3514 continue; // branch miss [j5]
3515 
3516 }
3517 } while(0);
3518 if( bgotonextstatement )
3519 {
3520 }
3521 }
3522 }
3523 }
3524 }
3525 }
3526 }
3527 
3528 } else
3529 {
3530 {
3531 IkReal j5array[1], cj5array[1], sj5array[1];
3532 bool j5valid[1]={false};
3533 _nj5 = 1;
3534 CheckValue<IkReal> x265=IKPowWithIntegerCheck(sj4,-1);
3535 if(!x265.valid){
3536 continue;
3537 }
3538 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x265.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r20*(x265.value)))-1) <= IKFAST_SINCOS_THRESH )
3539  continue;
3540 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x265.value)));
3541 sj5array[0]=IKsin(j5array[0]);
3542 cj5array[0]=IKcos(j5array[0]);
3543 if( j5array[0] > IKPI )
3544 {
3545  j5array[0]-=IK2PI;
3546 }
3547 else if( j5array[0] < -IKPI )
3548 { j5array[0]+=IK2PI;
3549 }
3550 j5valid[0] = true;
3551 for(int ij5 = 0; ij5 < 1; ++ij5)
3552 {
3553 if( !j5valid[ij5] )
3554 {
3555  continue;
3556 }
3557 _ij5[0] = ij5; _ij5[1] = -1;
3558 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3559 {
3560 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3561 {
3562  j5valid[iij5]=false; _ij5[1] = iij5; break;
3563 }
3564 }
3565 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3566 {
3567 IkReal evalcond[8];
3568 IkReal x266=IKsin(j5);
3569 IkReal x267=IKcos(j5);
3570 IkReal x268=((1.0)*cj4);
3571 IkReal x269=((1.0)*x266);
3572 evalcond[0]=(new_r20+((sj4*x267)));
3573 evalcond[1]=(((cj4*x266))+new_r11);
3574 evalcond[2]=((((-1.0)*sj4*x269))+new_r21);
3575 evalcond[3]=((((-1.0)*x267*x268))+new_r10);
3576 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x269)));
3577 evalcond[5]=((((-1.0)*x267))+(((-1.0)*new_r01)));
3578 evalcond[6]=(((new_r20*sj4))+x267+(((-1.0)*new_r10*x268)));
3579 evalcond[7]=((((-1.0)*new_r11*x268))+((new_r21*sj4))+(((-1.0)*x269)));
3580 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3581 {
3582 continue;
3583 }
3584 }
3585 
3586 {
3587 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3588 vinfos[0].jointtype = 1;
3589 vinfos[0].foffset = j0;
3590 vinfos[0].indices[0] = _ij0[0];
3591 vinfos[0].indices[1] = _ij0[1];
3592 vinfos[0].maxsolutions = _nj0;
3593 vinfos[1].jointtype = 1;
3594 vinfos[1].foffset = j1;
3595 vinfos[1].indices[0] = _ij1[0];
3596 vinfos[1].indices[1] = _ij1[1];
3597 vinfos[1].maxsolutions = _nj1;
3598 vinfos[2].jointtype = 1;
3599 vinfos[2].foffset = j2;
3600 vinfos[2].indices[0] = _ij2[0];
3601 vinfos[2].indices[1] = _ij2[1];
3602 vinfos[2].maxsolutions = _nj2;
3603 vinfos[3].jointtype = 1;
3604 vinfos[3].foffset = j3;
3605 vinfos[3].indices[0] = _ij3[0];
3606 vinfos[3].indices[1] = _ij3[1];
3607 vinfos[3].maxsolutions = _nj3;
3608 vinfos[4].jointtype = 1;
3609 vinfos[4].foffset = j4;
3610 vinfos[4].indices[0] = _ij4[0];
3611 vinfos[4].indices[1] = _ij4[1];
3612 vinfos[4].maxsolutions = _nj4;
3613 vinfos[5].jointtype = 1;
3614 vinfos[5].foffset = j5;
3615 vinfos[5].indices[0] = _ij5[0];
3616 vinfos[5].indices[1] = _ij5[1];
3617 vinfos[5].maxsolutions = _nj5;
3618 std::vector<int> vfree(0);
3619 solutions.AddSolution(vinfos,vfree);
3620 }
3621 }
3622 }
3623 
3624 }
3625 
3626 }
3627 
3628 } else
3629 {
3630 {
3631 IkReal j5array[1], cj5array[1], sj5array[1];
3632 bool j5valid[1]={false};
3633 _nj5 = 1;
3634 CheckValue<IkReal> x270=IKPowWithIntegerCheck(IKsign(cj4),-1);
3635 if(!x270.valid){
3636 continue;
3637 }
3638 CheckValue<IkReal> x271 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
3639 if(!x271.valid){
3640 continue;
3641 }
3642 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x270.value)))+(x271.value));
3643 sj5array[0]=IKsin(j5array[0]);
3644 cj5array[0]=IKcos(j5array[0]);
3645 if( j5array[0] > IKPI )
3646 {
3647  j5array[0]-=IK2PI;
3648 }
3649 else if( j5array[0] < -IKPI )
3650 { j5array[0]+=IK2PI;
3651 }
3652 j5valid[0] = true;
3653 for(int ij5 = 0; ij5 < 1; ++ij5)
3654 {
3655 if( !j5valid[ij5] )
3656 {
3657  continue;
3658 }
3659 _ij5[0] = ij5; _ij5[1] = -1;
3660 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3661 {
3662 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3663 {
3664  j5valid[iij5]=false; _ij5[1] = iij5; break;
3665 }
3666 }
3667 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3668 {
3669 IkReal evalcond[8];
3670 IkReal x272=IKsin(j5);
3671 IkReal x273=IKcos(j5);
3672 IkReal x274=((1.0)*cj4);
3673 IkReal x275=((1.0)*x272);
3674 evalcond[0]=(new_r20+((sj4*x273)));
3675 evalcond[1]=(((cj4*x272))+new_r11);
3676 evalcond[2]=((((-1.0)*sj4*x275))+new_r21);
3677 evalcond[3]=((((-1.0)*x273*x274))+new_r10);
3678 evalcond[4]=((((-1.0)*x275))+(((-1.0)*new_r00)));
3679 evalcond[5]=((((-1.0)*x273))+(((-1.0)*new_r01)));
3680 evalcond[6]=(((new_r20*sj4))+x273+(((-1.0)*new_r10*x274)));
3681 evalcond[7]=((((-1.0)*new_r11*x274))+(((-1.0)*x275))+((new_r21*sj4)));
3682 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3683 {
3684 continue;
3685 }
3686 }
3687 
3688 {
3689 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3690 vinfos[0].jointtype = 1;
3691 vinfos[0].foffset = j0;
3692 vinfos[0].indices[0] = _ij0[0];
3693 vinfos[0].indices[1] = _ij0[1];
3694 vinfos[0].maxsolutions = _nj0;
3695 vinfos[1].jointtype = 1;
3696 vinfos[1].foffset = j1;
3697 vinfos[1].indices[0] = _ij1[0];
3698 vinfos[1].indices[1] = _ij1[1];
3699 vinfos[1].maxsolutions = _nj1;
3700 vinfos[2].jointtype = 1;
3701 vinfos[2].foffset = j2;
3702 vinfos[2].indices[0] = _ij2[0];
3703 vinfos[2].indices[1] = _ij2[1];
3704 vinfos[2].maxsolutions = _nj2;
3705 vinfos[3].jointtype = 1;
3706 vinfos[3].foffset = j3;
3707 vinfos[3].indices[0] = _ij3[0];
3708 vinfos[3].indices[1] = _ij3[1];
3709 vinfos[3].maxsolutions = _nj3;
3710 vinfos[4].jointtype = 1;
3711 vinfos[4].foffset = j4;
3712 vinfos[4].indices[0] = _ij4[0];
3713 vinfos[4].indices[1] = _ij4[1];
3714 vinfos[4].maxsolutions = _nj4;
3715 vinfos[5].jointtype = 1;
3716 vinfos[5].foffset = j5;
3717 vinfos[5].indices[0] = _ij5[0];
3718 vinfos[5].indices[1] = _ij5[1];
3719 vinfos[5].maxsolutions = _nj5;
3720 std::vector<int> vfree(0);
3721 solutions.AddSolution(vinfos,vfree);
3722 }
3723 }
3724 }
3725 
3726 }
3727 
3728 }
3729 
3730 } else
3731 {
3732 {
3733 IkReal j5array[1], cj5array[1], sj5array[1];
3734 bool j5valid[1]={false};
3735 _nj5 = 1;
3736 CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(sj4),-1);
3737 if(!x276.valid){
3738 continue;
3739 }
3740 CheckValue<IkReal> x277 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
3741 if(!x277.valid){
3742 continue;
3743 }
3744 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x276.value)))+(x277.value));
3745 sj5array[0]=IKsin(j5array[0]);
3746 cj5array[0]=IKcos(j5array[0]);
3747 if( j5array[0] > IKPI )
3748 {
3749  j5array[0]-=IK2PI;
3750 }
3751 else if( j5array[0] < -IKPI )
3752 { j5array[0]+=IK2PI;
3753 }
3754 j5valid[0] = true;
3755 for(int ij5 = 0; ij5 < 1; ++ij5)
3756 {
3757 if( !j5valid[ij5] )
3758 {
3759  continue;
3760 }
3761 _ij5[0] = ij5; _ij5[1] = -1;
3762 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3763 {
3764 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3765 {
3766  j5valid[iij5]=false; _ij5[1] = iij5; break;
3767 }
3768 }
3769 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3770 {
3771 IkReal evalcond[8];
3772 IkReal x278=IKsin(j5);
3773 IkReal x279=IKcos(j5);
3774 IkReal x280=((1.0)*cj4);
3775 IkReal x281=((1.0)*x278);
3776 evalcond[0]=(new_r20+((sj4*x279)));
3777 evalcond[1]=(((cj4*x278))+new_r11);
3778 evalcond[2]=((((-1.0)*sj4*x281))+new_r21);
3779 evalcond[3]=(new_r10+(((-1.0)*x279*x280)));
3780 evalcond[4]=((((-1.0)*x281))+(((-1.0)*new_r00)));
3781 evalcond[5]=((((-1.0)*x279))+(((-1.0)*new_r01)));
3782 evalcond[6]=(((new_r20*sj4))+x279+(((-1.0)*new_r10*x280)));
3783 evalcond[7]=((((-1.0)*new_r11*x280))+(((-1.0)*x281))+((new_r21*sj4)));
3784 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3785 {
3786 continue;
3787 }
3788 }
3789 
3790 {
3791 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3792 vinfos[0].jointtype = 1;
3793 vinfos[0].foffset = j0;
3794 vinfos[0].indices[0] = _ij0[0];
3795 vinfos[0].indices[1] = _ij0[1];
3796 vinfos[0].maxsolutions = _nj0;
3797 vinfos[1].jointtype = 1;
3798 vinfos[1].foffset = j1;
3799 vinfos[1].indices[0] = _ij1[0];
3800 vinfos[1].indices[1] = _ij1[1];
3801 vinfos[1].maxsolutions = _nj1;
3802 vinfos[2].jointtype = 1;
3803 vinfos[2].foffset = j2;
3804 vinfos[2].indices[0] = _ij2[0];
3805 vinfos[2].indices[1] = _ij2[1];
3806 vinfos[2].maxsolutions = _nj2;
3807 vinfos[3].jointtype = 1;
3808 vinfos[3].foffset = j3;
3809 vinfos[3].indices[0] = _ij3[0];
3810 vinfos[3].indices[1] = _ij3[1];
3811 vinfos[3].maxsolutions = _nj3;
3812 vinfos[4].jointtype = 1;
3813 vinfos[4].foffset = j4;
3814 vinfos[4].indices[0] = _ij4[0];
3815 vinfos[4].indices[1] = _ij4[1];
3816 vinfos[4].maxsolutions = _nj4;
3817 vinfos[5].jointtype = 1;
3818 vinfos[5].foffset = j5;
3819 vinfos[5].indices[0] = _ij5[0];
3820 vinfos[5].indices[1] = _ij5[1];
3821 vinfos[5].maxsolutions = _nj5;
3822 std::vector<int> vfree(0);
3823 solutions.AddSolution(vinfos,vfree);
3824 }
3825 }
3826 }
3827 
3828 }
3829 
3830 }
3831 
3832 }
3833 } while(0);
3834 if( bgotonextstatement )
3835 {
3836 bool bgotonextstatement = true;
3837 do
3838 {
3839 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3840 evalcond[1]=new_r02;
3841 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3842 {
3843 bgotonextstatement=false;
3844 {
3845 IkReal j5array[1], cj5array[1], sj5array[1];
3846 bool j5valid[1]={false};
3847 _nj5 = 1;
3848 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3849  continue;
3850 j5array[0]=IKatan2(new_r00, new_r01);
3851 sj5array[0]=IKsin(j5array[0]);
3852 cj5array[0]=IKcos(j5array[0]);
3853 if( j5array[0] > IKPI )
3854 {
3855  j5array[0]-=IK2PI;
3856 }
3857 else if( j5array[0] < -IKPI )
3858 { j5array[0]+=IK2PI;
3859 }
3860 j5valid[0] = true;
3861 for(int ij5 = 0; ij5 < 1; ++ij5)
3862 {
3863 if( !j5valid[ij5] )
3864 {
3865  continue;
3866 }
3867 _ij5[0] = ij5; _ij5[1] = -1;
3868 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3869 {
3870 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3871 {
3872  j5valid[iij5]=false; _ij5[1] = iij5; break;
3873 }
3874 }
3875 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3876 {
3877 IkReal evalcond[8];
3878 IkReal x282=IKcos(j5);
3879 IkReal x283=IKsin(j5);
3880 IkReal x284=((1.0)*x283);
3881 IkReal x285=((1.0)*x282);
3882 evalcond[0]=(new_r20+((sj4*x282)));
3883 evalcond[1]=((((-1.0)*x284))+new_r00);
3884 evalcond[2]=((((-1.0)*x285))+new_r01);
3885 evalcond[3]=((((-1.0)*sj4*x284))+new_r21);
3886 evalcond[4]=((((-1.0)*new_r11))+((cj4*x283)));
3887 evalcond[5]=((((-1.0)*cj4*x285))+(((-1.0)*new_r10)));
3888 evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+x282);
3889 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x284))+((new_r21*sj4)));
3890 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3891 {
3892 continue;
3893 }
3894 }
3895 
3896 {
3897 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3898 vinfos[0].jointtype = 1;
3899 vinfos[0].foffset = j0;
3900 vinfos[0].indices[0] = _ij0[0];
3901 vinfos[0].indices[1] = _ij0[1];
3902 vinfos[0].maxsolutions = _nj0;
3903 vinfos[1].jointtype = 1;
3904 vinfos[1].foffset = j1;
3905 vinfos[1].indices[0] = _ij1[0];
3906 vinfos[1].indices[1] = _ij1[1];
3907 vinfos[1].maxsolutions = _nj1;
3908 vinfos[2].jointtype = 1;
3909 vinfos[2].foffset = j2;
3910 vinfos[2].indices[0] = _ij2[0];
3911 vinfos[2].indices[1] = _ij2[1];
3912 vinfos[2].maxsolutions = _nj2;
3913 vinfos[3].jointtype = 1;
3914 vinfos[3].foffset = j3;
3915 vinfos[3].indices[0] = _ij3[0];
3916 vinfos[3].indices[1] = _ij3[1];
3917 vinfos[3].maxsolutions = _nj3;
3918 vinfos[4].jointtype = 1;
3919 vinfos[4].foffset = j4;
3920 vinfos[4].indices[0] = _ij4[0];
3921 vinfos[4].indices[1] = _ij4[1];
3922 vinfos[4].maxsolutions = _nj4;
3923 vinfos[5].jointtype = 1;
3924 vinfos[5].foffset = j5;
3925 vinfos[5].indices[0] = _ij5[0];
3926 vinfos[5].indices[1] = _ij5[1];
3927 vinfos[5].maxsolutions = _nj5;
3928 std::vector<int> vfree(0);
3929 solutions.AddSolution(vinfos,vfree);
3930 }
3931 }
3932 }
3933 
3934 }
3935 } while(0);
3936 if( bgotonextstatement )
3937 {
3938 bool bgotonextstatement = true;
3939 do
3940 {
3941 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
3942 evalcond[1]=new_r22;
3943 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3944 {
3945 bgotonextstatement=false;
3946 {
3947 IkReal j5array[1], cj5array[1], sj5array[1];
3948 bool j5valid[1]={false};
3949 _nj5 = 1;
3950 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
3951  continue;
3952 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
3953 sj5array[0]=IKsin(j5array[0]);
3954 cj5array[0]=IKcos(j5array[0]);
3955 if( j5array[0] > IKPI )
3956 {
3957  j5array[0]-=IK2PI;
3958 }
3959 else if( j5array[0] < -IKPI )
3960 { j5array[0]+=IK2PI;
3961 }
3962 j5valid[0] = true;
3963 for(int ij5 = 0; ij5 < 1; ++ij5)
3964 {
3965 if( !j5valid[ij5] )
3966 {
3967  continue;
3968 }
3969 _ij5[0] = ij5; _ij5[1] = -1;
3970 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3971 {
3972 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3973 {
3974  j5valid[iij5]=false; _ij5[1] = iij5; break;
3975 }
3976 }
3977 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3978 {
3979 IkReal evalcond[8];
3980 IkReal x286=IKcos(j5);
3981 IkReal x287=IKsin(j5);
3982 IkReal x288=((1.0)*sj3);
3983 IkReal x289=((1.0)*x287);
3984 IkReal x290=((1.0)*x286);
3985 evalcond[0]=(x286+new_r20);
3986 evalcond[1]=((((-1.0)*x289))+new_r21);
3987 evalcond[2]=(((sj3*x286))+new_r01);
3988 evalcond[3]=(((sj3*x287))+new_r00);
3989 evalcond[4]=((((-1.0)*cj3*x290))+new_r11);
3990 evalcond[5]=((((-1.0)*new_r02*x289))+new_r10);
3991 evalcond[6]=((((-1.0)*new_r00*x288))+((cj3*new_r10))+(((-1.0)*x289)));
3992 evalcond[7]=((((-1.0)*x290))+(((-1.0)*new_r01*x288))+((cj3*new_r11)));
3993 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3994 {
3995 continue;
3996 }
3997 }
3998 
3999 {
4000 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4001 vinfos[0].jointtype = 1;
4002 vinfos[0].foffset = j0;
4003 vinfos[0].indices[0] = _ij0[0];
4004 vinfos[0].indices[1] = _ij0[1];
4005 vinfos[0].maxsolutions = _nj0;
4006 vinfos[1].jointtype = 1;
4007 vinfos[1].foffset = j1;
4008 vinfos[1].indices[0] = _ij1[0];
4009 vinfos[1].indices[1] = _ij1[1];
4010 vinfos[1].maxsolutions = _nj1;
4011 vinfos[2].jointtype = 1;
4012 vinfos[2].foffset = j2;
4013 vinfos[2].indices[0] = _ij2[0];
4014 vinfos[2].indices[1] = _ij2[1];
4015 vinfos[2].maxsolutions = _nj2;
4016 vinfos[3].jointtype = 1;
4017 vinfos[3].foffset = j3;
4018 vinfos[3].indices[0] = _ij3[0];
4019 vinfos[3].indices[1] = _ij3[1];
4020 vinfos[3].maxsolutions = _nj3;
4021 vinfos[4].jointtype = 1;
4022 vinfos[4].foffset = j4;
4023 vinfos[4].indices[0] = _ij4[0];
4024 vinfos[4].indices[1] = _ij4[1];
4025 vinfos[4].maxsolutions = _nj4;
4026 vinfos[5].jointtype = 1;
4027 vinfos[5].foffset = j5;
4028 vinfos[5].indices[0] = _ij5[0];
4029 vinfos[5].indices[1] = _ij5[1];
4030 vinfos[5].maxsolutions = _nj5;
4031 std::vector<int> vfree(0);
4032 solutions.AddSolution(vinfos,vfree);
4033 }
4034 }
4035 }
4036 
4037 }
4038 } while(0);
4039 if( bgotonextstatement )
4040 {
4041 bool bgotonextstatement = true;
4042 do
4043 {
4044 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
4045 evalcond[1]=new_r22;
4046 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4047 {
4048 bgotonextstatement=false;
4049 {
4050 IkReal j5array[1], cj5array[1], sj5array[1];
4051 bool j5valid[1]={false};
4052 _nj5 = 1;
4053 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
4054  continue;
4055 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
4056 sj5array[0]=IKsin(j5array[0]);
4057 cj5array[0]=IKcos(j5array[0]);
4058 if( j5array[0] > IKPI )
4059 {
4060  j5array[0]-=IK2PI;
4061 }
4062 else if( j5array[0] < -IKPI )
4063 { j5array[0]+=IK2PI;
4064 }
4065 j5valid[0] = true;
4066 for(int ij5 = 0; ij5 < 1; ++ij5)
4067 {
4068 if( !j5valid[ij5] )
4069 {
4070  continue;
4071 }
4072 _ij5[0] = ij5; _ij5[1] = -1;
4073 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4074 {
4075 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4076 {
4077  j5valid[iij5]=false; _ij5[1] = iij5; break;
4078 }
4079 }
4080 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4081 {
4082 IkReal evalcond[8];
4083 IkReal x291=IKcos(j5);
4084 IkReal x292=IKsin(j5);
4085 IkReal x293=((1.0)*sj3);
4086 IkReal x294=((1.0)*x291);
4087 evalcond[0]=(x292+new_r21);
4088 evalcond[1]=((((-1.0)*x294))+new_r20);
4089 evalcond[2]=(new_r01+((sj3*x291)));
4090 evalcond[3]=(new_r00+((sj3*x292)));
4091 evalcond[4]=(((new_r02*x292))+new_r10);
4092 evalcond[5]=((((-1.0)*cj3*x294))+new_r11);
4093 evalcond[6]=((((-1.0)*x292))+((cj3*new_r10))+(((-1.0)*new_r00*x293)));
4094 evalcond[7]=((((-1.0)*new_r01*x293))+(((-1.0)*x294))+((cj3*new_r11)));
4095 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4096 {
4097 continue;
4098 }
4099 }
4100 
4101 {
4102 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4103 vinfos[0].jointtype = 1;
4104 vinfos[0].foffset = j0;
4105 vinfos[0].indices[0] = _ij0[0];
4106 vinfos[0].indices[1] = _ij0[1];
4107 vinfos[0].maxsolutions = _nj0;
4108 vinfos[1].jointtype = 1;
4109 vinfos[1].foffset = j1;
4110 vinfos[1].indices[0] = _ij1[0];
4111 vinfos[1].indices[1] = _ij1[1];
4112 vinfos[1].maxsolutions = _nj1;
4113 vinfos[2].jointtype = 1;
4114 vinfos[2].foffset = j2;
4115 vinfos[2].indices[0] = _ij2[0];
4116 vinfos[2].indices[1] = _ij2[1];
4117 vinfos[2].maxsolutions = _nj2;
4118 vinfos[3].jointtype = 1;
4119 vinfos[3].foffset = j3;
4120 vinfos[3].indices[0] = _ij3[0];
4121 vinfos[3].indices[1] = _ij3[1];
4122 vinfos[3].maxsolutions = _nj3;
4123 vinfos[4].jointtype = 1;
4124 vinfos[4].foffset = j4;
4125 vinfos[4].indices[0] = _ij4[0];
4126 vinfos[4].indices[1] = _ij4[1];
4127 vinfos[4].maxsolutions = _nj4;
4128 vinfos[5].jointtype = 1;
4129 vinfos[5].foffset = j5;
4130 vinfos[5].indices[0] = _ij5[0];
4131 vinfos[5].indices[1] = _ij5[1];
4132 vinfos[5].maxsolutions = _nj5;
4133 std::vector<int> vfree(0);
4134 solutions.AddSolution(vinfos,vfree);
4135 }
4136 }
4137 }
4138 
4139 }
4140 } while(0);
4141 if( bgotonextstatement )
4142 {
4143 bool bgotonextstatement = true;
4144 do
4145 {
4146 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4147 evalcond[1]=new_r20;
4148 evalcond[2]=new_r02;
4149 evalcond[3]=new_r12;
4150 evalcond[4]=new_r21;
4151 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 )
4152 {
4153 bgotonextstatement=false;
4154 {
4155 IkReal j5array[1], cj5array[1], sj5array[1];
4156 bool j5valid[1]={false};
4157 _nj5 = 1;
4158 IkReal x295=((1.0)*new_r01);
4159 if( IKabs(((((-1.0)*cj3*x295))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x295))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x295))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x295))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
4160  continue;
4161 j5array[0]=IKatan2(((((-1.0)*cj3*x295))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x295))+((cj3*new_r00))));
4162 sj5array[0]=IKsin(j5array[0]);
4163 cj5array[0]=IKcos(j5array[0]);
4164 if( j5array[0] > IKPI )
4165 {
4166  j5array[0]-=IK2PI;
4167 }
4168 else if( j5array[0] < -IKPI )
4169 { j5array[0]+=IK2PI;
4170 }
4171 j5valid[0] = true;
4172 for(int ij5 = 0; ij5 < 1; ++ij5)
4173 {
4174 if( !j5valid[ij5] )
4175 {
4176  continue;
4177 }
4178 _ij5[0] = ij5; _ij5[1] = -1;
4179 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4180 {
4181 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4182 {
4183  j5valid[iij5]=false; _ij5[1] = iij5; break;
4184 }
4185 }
4186 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4187 {
4188 IkReal evalcond[8];
4189 IkReal x296=IKsin(j5);
4190 IkReal x297=IKcos(j5);
4191 IkReal x298=((1.0)*sj3);
4192 IkReal x299=((1.0)*x297);
4193 IkReal x300=(sj3*x296);
4194 IkReal x301=((1.0)*x296);
4195 IkReal x302=(cj3*x299);
4196 evalcond[0]=(((new_r11*sj3))+x296+((cj3*new_r01)));
4197 evalcond[1]=(((cj3*x296))+new_r01+((sj3*x297)));
4198 evalcond[2]=(((new_r10*sj3))+(((-1.0)*x299))+((cj3*new_r00)));
4199 evalcond[3]=(((cj3*new_r10))+(((-1.0)*new_r00*x298))+(((-1.0)*x301)));
4200 evalcond[4]=((((-1.0)*new_r01*x298))+(((-1.0)*x299))+((cj3*new_r11)));
4201 evalcond[5]=(x300+new_r00+(((-1.0)*x302)));
4202 evalcond[6]=(x300+new_r11+(((-1.0)*x302)));
4203 evalcond[7]=((((-1.0)*cj3*x301))+(((-1.0)*x297*x298))+new_r10);
4204 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4205 {
4206 continue;
4207 }
4208 }
4209 
4210 {
4211 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4212 vinfos[0].jointtype = 1;
4213 vinfos[0].foffset = j0;
4214 vinfos[0].indices[0] = _ij0[0];
4215 vinfos[0].indices[1] = _ij0[1];
4216 vinfos[0].maxsolutions = _nj0;
4217 vinfos[1].jointtype = 1;
4218 vinfos[1].foffset = j1;
4219 vinfos[1].indices[0] = _ij1[0];
4220 vinfos[1].indices[1] = _ij1[1];
4221 vinfos[1].maxsolutions = _nj1;
4222 vinfos[2].jointtype = 1;
4223 vinfos[2].foffset = j2;
4224 vinfos[2].indices[0] = _ij2[0];
4225 vinfos[2].indices[1] = _ij2[1];
4226 vinfos[2].maxsolutions = _nj2;
4227 vinfos[3].jointtype = 1;
4228 vinfos[3].foffset = j3;
4229 vinfos[3].indices[0] = _ij3[0];
4230 vinfos[3].indices[1] = _ij3[1];
4231 vinfos[3].maxsolutions = _nj3;
4232 vinfos[4].jointtype = 1;
4233 vinfos[4].foffset = j4;
4234 vinfos[4].indices[0] = _ij4[0];
4235 vinfos[4].indices[1] = _ij4[1];
4236 vinfos[4].maxsolutions = _nj4;
4237 vinfos[5].jointtype = 1;
4238 vinfos[5].foffset = j5;
4239 vinfos[5].indices[0] = _ij5[0];
4240 vinfos[5].indices[1] = _ij5[1];
4241 vinfos[5].maxsolutions = _nj5;
4242 std::vector<int> vfree(0);
4243 solutions.AddSolution(vinfos,vfree);
4244 }
4245 }
4246 }
4247 
4248 }
4249 } while(0);
4250 if( bgotonextstatement )
4251 {
4252 bool bgotonextstatement = true;
4253 do
4254 {
4255 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4256 evalcond[1]=new_r20;
4257 evalcond[2]=new_r02;
4258 evalcond[3]=new_r12;
4259 evalcond[4]=new_r21;
4260 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 )
4261 {
4262 bgotonextstatement=false;
4263 {
4264 IkReal j5array[1], cj5array[1], sj5array[1];
4265 bool j5valid[1]={false};
4266 _nj5 = 1;
4267 IkReal x303=((1.0)*sj3);
4268 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x303)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x303)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x303))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x303))))-1) <= IKFAST_SINCOS_THRESH )
4269  continue;
4270 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x303))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x303))));
4271 sj5array[0]=IKsin(j5array[0]);
4272 cj5array[0]=IKcos(j5array[0]);
4273 if( j5array[0] > IKPI )
4274 {
4275  j5array[0]-=IK2PI;
4276 }
4277 else if( j5array[0] < -IKPI )
4278 { j5array[0]+=IK2PI;
4279 }
4280 j5valid[0] = true;
4281 for(int ij5 = 0; ij5 < 1; ++ij5)
4282 {
4283 if( !j5valid[ij5] )
4284 {
4285  continue;
4286 }
4287 _ij5[0] = ij5; _ij5[1] = -1;
4288 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4289 {
4290 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4291 {
4292  j5valid[iij5]=false; _ij5[1] = iij5; break;
4293 }
4294 }
4295 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4296 {
4297 IkReal evalcond[8];
4298 IkReal x304=IKsin(j5);
4299 IkReal x305=IKcos(j5);
4300 IkReal x306=((1.0)*sj3);
4301 IkReal x307=((1.0)*x304);
4302 IkReal x308=(sj3*x305);
4303 IkReal x309=((1.0)*x305);
4304 IkReal x310=(cj3*x307);
4305 evalcond[0]=(((new_r10*sj3))+x305+((cj3*new_r00)));
4306 evalcond[1]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x307)));
4307 evalcond[2]=(((sj3*x304))+((cj3*x305))+new_r00);
4308 evalcond[3]=(((cj3*new_r10))+(((-1.0)*new_r00*x306))+(((-1.0)*x307)));
4309 evalcond[4]=((((-1.0)*new_r01*x306))+((cj3*new_r11))+(((-1.0)*x309)));
4310 evalcond[5]=(x308+(((-1.0)*x310))+new_r01);
4311 evalcond[6]=(x308+(((-1.0)*x310))+new_r10);
4312 evalcond[7]=((((-1.0)*cj3*x309))+(((-1.0)*x304*x306))+new_r11);
4313 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4314 {
4315 continue;
4316 }
4317 }
4318 
4319 {
4320 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4321 vinfos[0].jointtype = 1;
4322 vinfos[0].foffset = j0;
4323 vinfos[0].indices[0] = _ij0[0];
4324 vinfos[0].indices[1] = _ij0[1];
4325 vinfos[0].maxsolutions = _nj0;
4326 vinfos[1].jointtype = 1;
4327 vinfos[1].foffset = j1;
4328 vinfos[1].indices[0] = _ij1[0];
4329 vinfos[1].indices[1] = _ij1[1];
4330 vinfos[1].maxsolutions = _nj1;
4331 vinfos[2].jointtype = 1;
4332 vinfos[2].foffset = j2;
4333 vinfos[2].indices[0] = _ij2[0];
4334 vinfos[2].indices[1] = _ij2[1];
4335 vinfos[2].maxsolutions = _nj2;
4336 vinfos[3].jointtype = 1;
4337 vinfos[3].foffset = j3;
4338 vinfos[3].indices[0] = _ij3[0];
4339 vinfos[3].indices[1] = _ij3[1];
4340 vinfos[3].maxsolutions = _nj3;
4341 vinfos[4].jointtype = 1;
4342 vinfos[4].foffset = j4;
4343 vinfos[4].indices[0] = _ij4[0];
4344 vinfos[4].indices[1] = _ij4[1];
4345 vinfos[4].maxsolutions = _nj4;
4346 vinfos[5].jointtype = 1;
4347 vinfos[5].foffset = j5;
4348 vinfos[5].indices[0] = _ij5[0];
4349 vinfos[5].indices[1] = _ij5[1];
4350 vinfos[5].maxsolutions = _nj5;
4351 std::vector<int> vfree(0);
4352 solutions.AddSolution(vinfos,vfree);
4353 }
4354 }
4355 }
4356 
4357 }
4358 } while(0);
4359 if( bgotonextstatement )
4360 {
4361 bool bgotonextstatement = true;
4362 do
4363 {
4364 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4365 evalcond[1]=new_r12;
4366 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4367 {
4368 bgotonextstatement=false;
4369 {
4370 IkReal j5array[1], cj5array[1], sj5array[1];
4371 bool j5valid[1]={false};
4372 _nj5 = 1;
4373 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4374  continue;
4375 j5array[0]=IKatan2(new_r10, new_r11);
4376 sj5array[0]=IKsin(j5array[0]);
4377 cj5array[0]=IKcos(j5array[0]);
4378 if( j5array[0] > IKPI )
4379 {
4380  j5array[0]-=IK2PI;
4381 }
4382 else if( j5array[0] < -IKPI )
4383 { j5array[0]+=IK2PI;
4384 }
4385 j5valid[0] = true;
4386 for(int ij5 = 0; ij5 < 1; ++ij5)
4387 {
4388 if( !j5valid[ij5] )
4389 {
4390  continue;
4391 }
4392 _ij5[0] = ij5; _ij5[1] = -1;
4393 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4394 {
4395 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4396 {
4397  j5valid[iij5]=false; _ij5[1] = iij5; break;
4398 }
4399 }
4400 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4401 {
4402 IkReal evalcond[8];
4403 IkReal x311=IKcos(j5);
4404 IkReal x312=IKsin(j5);
4405 IkReal x313=((1.0)*cj4);
4406 IkReal x314=((1.0)*x312);
4407 evalcond[0]=(((new_r02*x311))+new_r20);
4408 evalcond[1]=((((-1.0)*x314))+new_r10);
4409 evalcond[2]=((((-1.0)*x311))+new_r11);
4410 evalcond[3]=(((cj4*x312))+new_r01);
4411 evalcond[4]=((((-1.0)*new_r02*x314))+new_r21);
4412 evalcond[5]=(new_r00+(((-1.0)*x311*x313)));
4413 evalcond[6]=((((-1.0)*new_r00*x313))+((new_r20*sj4))+x311);
4414 evalcond[7]=((((-1.0)*x314))+((new_r21*sj4))+(((-1.0)*new_r01*x313)));
4415 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4416 {
4417 continue;
4418 }
4419 }
4420 
4421 {
4422 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4423 vinfos[0].jointtype = 1;
4424 vinfos[0].foffset = j0;
4425 vinfos[0].indices[0] = _ij0[0];
4426 vinfos[0].indices[1] = _ij0[1];
4427 vinfos[0].maxsolutions = _nj0;
4428 vinfos[1].jointtype = 1;
4429 vinfos[1].foffset = j1;
4430 vinfos[1].indices[0] = _ij1[0];
4431 vinfos[1].indices[1] = _ij1[1];
4432 vinfos[1].maxsolutions = _nj1;
4433 vinfos[2].jointtype = 1;
4434 vinfos[2].foffset = j2;
4435 vinfos[2].indices[0] = _ij2[0];
4436 vinfos[2].indices[1] = _ij2[1];
4437 vinfos[2].maxsolutions = _nj2;
4438 vinfos[3].jointtype = 1;
4439 vinfos[3].foffset = j3;
4440 vinfos[3].indices[0] = _ij3[0];
4441 vinfos[3].indices[1] = _ij3[1];
4442 vinfos[3].maxsolutions = _nj3;
4443 vinfos[4].jointtype = 1;
4444 vinfos[4].foffset = j4;
4445 vinfos[4].indices[0] = _ij4[0];
4446 vinfos[4].indices[1] = _ij4[1];
4447 vinfos[4].maxsolutions = _nj4;
4448 vinfos[5].jointtype = 1;
4449 vinfos[5].foffset = j5;
4450 vinfos[5].indices[0] = _ij5[0];
4451 vinfos[5].indices[1] = _ij5[1];
4452 vinfos[5].maxsolutions = _nj5;
4453 std::vector<int> vfree(0);
4454 solutions.AddSolution(vinfos,vfree);
4455 }
4456 }
4457 }
4458 
4459 }
4460 } while(0);
4461 if( bgotonextstatement )
4462 {
4463 bool bgotonextstatement = true;
4464 do
4465 {
4466 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4467 evalcond[1]=new_r12;
4468 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4469 {
4470 bgotonextstatement=false;
4471 {
4472 IkReal j5eval[3];
4473 sj3=0;
4474 cj3=-1.0;
4475 j3=3.14159265358979;
4476 j5eval[0]=new_r02;
4477 j5eval[1]=IKsign(new_r02);
4478 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4479 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4480 {
4481 {
4482 IkReal j5eval[1];
4483 sj3=0;
4484 cj3=-1.0;
4485 j3=3.14159265358979;
4486 j5eval[0]=new_r02;
4487 if( IKabs(j5eval[0]) < 0.0000010000000000 )
4488 {
4489 {
4490 IkReal j5eval[2];
4491 sj3=0;
4492 cj3=-1.0;
4493 j3=3.14159265358979;
4494 j5eval[0]=new_r02;
4495 j5eval[1]=cj4;
4496 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4497 {
4498 {
4499 IkReal evalcond[4];
4500 bool bgotonextstatement = true;
4501 do
4502 {
4503 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
4504 evalcond[1]=new_r22;
4505 evalcond[2]=new_r01;
4506 evalcond[3]=new_r00;
4507 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
4508 {
4509 bgotonextstatement=false;
4510 {
4511 IkReal j5array[1], cj5array[1], sj5array[1];
4512 bool j5valid[1]={false};
4513 _nj5 = 1;
4514 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
4515  continue;
4516 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
4517 sj5array[0]=IKsin(j5array[0]);
4518 cj5array[0]=IKcos(j5array[0]);
4519 if( j5array[0] > IKPI )
4520 {
4521  j5array[0]-=IK2PI;
4522 }
4523 else if( j5array[0] < -IKPI )
4524 { j5array[0]+=IK2PI;
4525 }
4526 j5valid[0] = true;
4527 for(int ij5 = 0; ij5 < 1; ++ij5)
4528 {
4529 if( !j5valid[ij5] )
4530 {
4531  continue;
4532 }
4533 _ij5[0] = ij5; _ij5[1] = -1;
4534 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4535 {
4536 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4537 {
4538  j5valid[iij5]=false; _ij5[1] = iij5; break;
4539 }
4540 }
4541 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4542 {
4543 IkReal evalcond[4];
4544 IkReal x315=IKcos(j5);
4545 IkReal x316=((1.0)*(IKsin(j5)));
4546 evalcond[0]=(x315+new_r20);
4547 evalcond[1]=((((-1.0)*x316))+new_r21);
4548 evalcond[2]=((((-1.0)*x316))+(((-1.0)*new_r10)));
4549 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x315)));
4550 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
4551 {
4552 continue;
4553 }
4554 }
4555 
4556 {
4557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4558 vinfos[0].jointtype = 1;
4559 vinfos[0].foffset = j0;
4560 vinfos[0].indices[0] = _ij0[0];
4561 vinfos[0].indices[1] = _ij0[1];
4562 vinfos[0].maxsolutions = _nj0;
4563 vinfos[1].jointtype = 1;
4564 vinfos[1].foffset = j1;
4565 vinfos[1].indices[0] = _ij1[0];
4566 vinfos[1].indices[1] = _ij1[1];
4567 vinfos[1].maxsolutions = _nj1;
4568 vinfos[2].jointtype = 1;
4569 vinfos[2].foffset = j2;
4570 vinfos[2].indices[0] = _ij2[0];
4571 vinfos[2].indices[1] = _ij2[1];
4572 vinfos[2].maxsolutions = _nj2;
4573 vinfos[3].jointtype = 1;
4574 vinfos[3].foffset = j3;
4575 vinfos[3].indices[0] = _ij3[0];
4576 vinfos[3].indices[1] = _ij3[1];
4577 vinfos[3].maxsolutions = _nj3;
4578 vinfos[4].jointtype = 1;
4579 vinfos[4].foffset = j4;
4580 vinfos[4].indices[0] = _ij4[0];
4581 vinfos[4].indices[1] = _ij4[1];
4582 vinfos[4].maxsolutions = _nj4;
4583 vinfos[5].jointtype = 1;
4584 vinfos[5].foffset = j5;
4585 vinfos[5].indices[0] = _ij5[0];
4586 vinfos[5].indices[1] = _ij5[1];
4587 vinfos[5].maxsolutions = _nj5;
4588 std::vector<int> vfree(0);
4589 solutions.AddSolution(vinfos,vfree);
4590 }
4591 }
4592 }
4593 
4594 }
4595 } while(0);
4596 if( bgotonextstatement )
4597 {
4598 bool bgotonextstatement = true;
4599 do
4600 {
4601 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
4602 evalcond[1]=new_r22;
4603 evalcond[2]=new_r01;
4604 evalcond[3]=new_r00;
4605 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
4606 {
4607 bgotonextstatement=false;
4608 {
4609 IkReal j5array[1], cj5array[1], sj5array[1];
4610 bool j5valid[1]={false};
4611 _nj5 = 1;
4612 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
4613  continue;
4614 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
4615 sj5array[0]=IKsin(j5array[0]);
4616 cj5array[0]=IKcos(j5array[0]);
4617 if( j5array[0] > IKPI )
4618 {
4619  j5array[0]-=IK2PI;
4620 }
4621 else if( j5array[0] < -IKPI )
4622 { j5array[0]+=IK2PI;
4623 }
4624 j5valid[0] = true;
4625 for(int ij5 = 0; ij5 < 1; ++ij5)
4626 {
4627 if( !j5valid[ij5] )
4628 {
4629  continue;
4630 }
4631 _ij5[0] = ij5; _ij5[1] = -1;
4632 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4633 {
4634 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4635 {
4636  j5valid[iij5]=false; _ij5[1] = iij5; break;
4637 }
4638 }
4639 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4640 {
4641 IkReal evalcond[4];
4642 IkReal x317=IKsin(j5);
4643 IkReal x318=((1.0)*(IKcos(j5)));
4644 evalcond[0]=(x317+new_r21);
4645 evalcond[1]=((((-1.0)*x318))+new_r20);
4646 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x317)));
4647 evalcond[3]=((((-1.0)*x318))+(((-1.0)*new_r11)));
4648 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
4649 {
4650 continue;
4651 }
4652 }
4653 
4654 {
4655 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4656 vinfos[0].jointtype = 1;
4657 vinfos[0].foffset = j0;
4658 vinfos[0].indices[0] = _ij0[0];
4659 vinfos[0].indices[1] = _ij0[1];
4660 vinfos[0].maxsolutions = _nj0;
4661 vinfos[1].jointtype = 1;
4662 vinfos[1].foffset = j1;
4663 vinfos[1].indices[0] = _ij1[0];
4664 vinfos[1].indices[1] = _ij1[1];
4665 vinfos[1].maxsolutions = _nj1;
4666 vinfos[2].jointtype = 1;
4667 vinfos[2].foffset = j2;
4668 vinfos[2].indices[0] = _ij2[0];
4669 vinfos[2].indices[1] = _ij2[1];
4670 vinfos[2].maxsolutions = _nj2;
4671 vinfos[3].jointtype = 1;
4672 vinfos[3].foffset = j3;
4673 vinfos[3].indices[0] = _ij3[0];
4674 vinfos[3].indices[1] = _ij3[1];
4675 vinfos[3].maxsolutions = _nj3;
4676 vinfos[4].jointtype = 1;
4677 vinfos[4].foffset = j4;
4678 vinfos[4].indices[0] = _ij4[0];
4679 vinfos[4].indices[1] = _ij4[1];
4680 vinfos[4].maxsolutions = _nj4;
4681 vinfos[5].jointtype = 1;
4682 vinfos[5].foffset = j5;
4683 vinfos[5].indices[0] = _ij5[0];
4684 vinfos[5].indices[1] = _ij5[1];
4685 vinfos[5].maxsolutions = _nj5;
4686 std::vector<int> vfree(0);
4687 solutions.AddSolution(vinfos,vfree);
4688 }
4689 }
4690 }
4691 
4692 }
4693 } while(0);
4694 if( bgotonextstatement )
4695 {
4696 bool bgotonextstatement = true;
4697 do
4698 {
4699 evalcond[0]=IKabs(new_r02);
4700 evalcond[1]=new_r20;
4701 evalcond[2]=new_r21;
4702 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
4703 {
4704 bgotonextstatement=false;
4705 {
4706 IkReal j5array[1], cj5array[1], sj5array[1];
4707 bool j5valid[1]={false};
4708 _nj5 = 1;
4709 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*cj4*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*cj4*new_r00))-1) <= IKFAST_SINCOS_THRESH )
4710  continue;
4711 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*cj4*new_r00));
4712 sj5array[0]=IKsin(j5array[0]);
4713 cj5array[0]=IKcos(j5array[0]);
4714 if( j5array[0] > IKPI )
4715 {
4716  j5array[0]-=IK2PI;
4717 }
4718 else if( j5array[0] < -IKPI )
4719 { j5array[0]+=IK2PI;
4720 }
4721 j5valid[0] = true;
4722 for(int ij5 = 0; ij5 < 1; ++ij5)
4723 {
4724 if( !j5valid[ij5] )
4725 {
4726  continue;
4727 }
4728 _ij5[0] = ij5; _ij5[1] = -1;
4729 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4730 {
4731 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4732 {
4733  j5valid[iij5]=false; _ij5[1] = iij5; break;
4734 }
4735 }
4736 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4737 {
4738 IkReal evalcond[6];
4739 IkReal x319=IKcos(j5);
4740 IkReal x320=IKsin(j5);
4741 IkReal x321=((1.0)*x320);
4742 IkReal x322=((1.0)*x319);
4743 evalcond[0]=(((cj4*new_r00))+x319);
4744 evalcond[1]=((((-1.0)*x321))+(((-1.0)*new_r10)));
4745 evalcond[2]=((((-1.0)*x322))+(((-1.0)*new_r11)));
4746 evalcond[3]=(((cj4*x320))+(((-1.0)*new_r01)));
4747 evalcond[4]=(((cj4*new_r01))+(((-1.0)*x321)));
4748 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x322)));
4749 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
4750 {
4751 continue;
4752 }
4753 }
4754 
4755 {
4756 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4757 vinfos[0].jointtype = 1;
4758 vinfos[0].foffset = j0;
4759 vinfos[0].indices[0] = _ij0[0];
4760 vinfos[0].indices[1] = _ij0[1];
4761 vinfos[0].maxsolutions = _nj0;
4762 vinfos[1].jointtype = 1;
4763 vinfos[1].foffset = j1;
4764 vinfos[1].indices[0] = _ij1[0];
4765 vinfos[1].indices[1] = _ij1[1];
4766 vinfos[1].maxsolutions = _nj1;
4767 vinfos[2].jointtype = 1;
4768 vinfos[2].foffset = j2;
4769 vinfos[2].indices[0] = _ij2[0];
4770 vinfos[2].indices[1] = _ij2[1];
4771 vinfos[2].maxsolutions = _nj2;
4772 vinfos[3].jointtype = 1;
4773 vinfos[3].foffset = j3;
4774 vinfos[3].indices[0] = _ij3[0];
4775 vinfos[3].indices[1] = _ij3[1];
4776 vinfos[3].maxsolutions = _nj3;
4777 vinfos[4].jointtype = 1;
4778 vinfos[4].foffset = j4;
4779 vinfos[4].indices[0] = _ij4[0];
4780 vinfos[4].indices[1] = _ij4[1];
4781 vinfos[4].maxsolutions = _nj4;
4782 vinfos[5].jointtype = 1;
4783 vinfos[5].foffset = j5;
4784 vinfos[5].indices[0] = _ij5[0];
4785 vinfos[5].indices[1] = _ij5[1];
4786 vinfos[5].maxsolutions = _nj5;
4787 std::vector<int> vfree(0);
4788 solutions.AddSolution(vinfos,vfree);
4789 }
4790 }
4791 }
4792 
4793 }
4794 } while(0);
4795 if( bgotonextstatement )
4796 {
4797 bool bgotonextstatement = true;
4798 do
4799 {
4800 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4801 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4802 {
4803 bgotonextstatement=false;
4804 {
4805 IkReal j5array[1], cj5array[1], sj5array[1];
4806 bool j5valid[1]={false};
4807 _nj5 = 1;
4808 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 )
4809  continue;
4810 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4811 sj5array[0]=IKsin(j5array[0]);
4812 cj5array[0]=IKcos(j5array[0]);
4813 if( j5array[0] > IKPI )
4814 {
4815  j5array[0]-=IK2PI;
4816 }
4817 else if( j5array[0] < -IKPI )
4818 { j5array[0]+=IK2PI;
4819 }
4820 j5valid[0] = true;
4821 for(int ij5 = 0; ij5 < 1; ++ij5)
4822 {
4823 if( !j5valid[ij5] )
4824 {
4825  continue;
4826 }
4827 _ij5[0] = ij5; _ij5[1] = -1;
4828 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4829 {
4830 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4831 {
4832  j5valid[iij5]=false; _ij5[1] = iij5; break;
4833 }
4834 }
4835 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4836 {
4837 IkReal evalcond[6];
4838 IkReal x323=IKsin(j5);
4839 IkReal x324=IKcos(j5);
4840 evalcond[0]=x324;
4841 evalcond[1]=(new_r22*x323);
4842 evalcond[2]=((-1.0)*x323);
4843 evalcond[3]=((-1.0)*new_r22*x324);
4844 evalcond[4]=((((-1.0)*x323))+(((-1.0)*new_r10)));
4845 evalcond[5]=((((-1.0)*x324))+(((-1.0)*new_r11)));
4846 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
4847 {
4848 continue;
4849 }
4850 }
4851 
4852 {
4853 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4854 vinfos[0].jointtype = 1;
4855 vinfos[0].foffset = j0;
4856 vinfos[0].indices[0] = _ij0[0];
4857 vinfos[0].indices[1] = _ij0[1];
4858 vinfos[0].maxsolutions = _nj0;
4859 vinfos[1].jointtype = 1;
4860 vinfos[1].foffset = j1;
4861 vinfos[1].indices[0] = _ij1[0];
4862 vinfos[1].indices[1] = _ij1[1];
4863 vinfos[1].maxsolutions = _nj1;
4864 vinfos[2].jointtype = 1;
4865 vinfos[2].foffset = j2;
4866 vinfos[2].indices[0] = _ij2[0];
4867 vinfos[2].indices[1] = _ij2[1];
4868 vinfos[2].maxsolutions = _nj2;
4869 vinfos[3].jointtype = 1;
4870 vinfos[3].foffset = j3;
4871 vinfos[3].indices[0] = _ij3[0];
4872 vinfos[3].indices[1] = _ij3[1];
4873 vinfos[3].maxsolutions = _nj3;
4874 vinfos[4].jointtype = 1;
4875 vinfos[4].foffset = j4;
4876 vinfos[4].indices[0] = _ij4[0];
4877 vinfos[4].indices[1] = _ij4[1];
4878 vinfos[4].maxsolutions = _nj4;
4879 vinfos[5].jointtype = 1;
4880 vinfos[5].foffset = j5;
4881 vinfos[5].indices[0] = _ij5[0];
4882 vinfos[5].indices[1] = _ij5[1];
4883 vinfos[5].maxsolutions = _nj5;
4884 std::vector<int> vfree(0);
4885 solutions.AddSolution(vinfos,vfree);
4886 }
4887 }
4888 }
4889 
4890 }
4891 } while(0);
4892 if( bgotonextstatement )
4893 {
4894 bool bgotonextstatement = true;
4895 do
4896 {
4897 if( 1 )
4898 {
4899 bgotonextstatement=false;
4900 continue; // branch miss [j5]
4901 
4902 }
4903 } while(0);
4904 if( bgotonextstatement )
4905 {
4906 }
4907 }
4908 }
4909 }
4910 }
4911 }
4912 
4913 } else
4914 {
4915 {
4916 IkReal j5array[1], cj5array[1], sj5array[1];
4917 bool j5valid[1]={false};
4918 _nj5 = 1;
4919 CheckValue<IkReal> x325=IKPowWithIntegerCheck(new_r02,-1);
4920 if(!x325.valid){
4921 continue;
4922 }
4923 CheckValue<IkReal> x326=IKPowWithIntegerCheck(cj4,-1);
4924 if(!x326.valid){
4925 continue;
4926 }
4927 if( IKabs(((-1.0)*new_r21*(x325.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00*(x326.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*(x325.value)))+IKsqr(((-1.0)*new_r00*(x326.value)))-1) <= IKFAST_SINCOS_THRESH )
4928  continue;
4929 j5array[0]=IKatan2(((-1.0)*new_r21*(x325.value)), ((-1.0)*new_r00*(x326.value)));
4930 sj5array[0]=IKsin(j5array[0]);
4931 cj5array[0]=IKcos(j5array[0]);
4932 if( j5array[0] > IKPI )
4933 {
4934  j5array[0]-=IK2PI;
4935 }
4936 else if( j5array[0] < -IKPI )
4937 { j5array[0]+=IK2PI;
4938 }
4939 j5valid[0] = true;
4940 for(int ij5 = 0; ij5 < 1; ++ij5)
4941 {
4942 if( !j5valid[ij5] )
4943 {
4944  continue;
4945 }
4946 _ij5[0] = ij5; _ij5[1] = -1;
4947 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4948 {
4949 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4950 {
4951  j5valid[iij5]=false; _ij5[1] = iij5; break;
4952 }
4953 }
4954 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4955 {
4956 IkReal evalcond[8];
4957 IkReal x327=IKsin(j5);
4958 IkReal x328=IKcos(j5);
4959 IkReal x329=((1.0)*x327);
4960 IkReal x330=((1.0)*x328);
4961 evalcond[0]=(new_r21+((new_r02*x327)));
4962 evalcond[1]=(new_r20+(((-1.0)*new_r02*x330)));
4963 evalcond[2]=((((-1.0)*x329))+(((-1.0)*new_r10)));
4964 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x330)));
4965 evalcond[4]=(((cj4*x327))+(((-1.0)*new_r01)));
4966 evalcond[5]=((((-1.0)*cj4*x330))+(((-1.0)*new_r00)));
4967 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x328);
4968 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x329))+((new_r21*sj4)));
4969 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4970 {
4971 continue;
4972 }
4973 }
4974 
4975 {
4976 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4977 vinfos[0].jointtype = 1;
4978 vinfos[0].foffset = j0;
4979 vinfos[0].indices[0] = _ij0[0];
4980 vinfos[0].indices[1] = _ij0[1];
4981 vinfos[0].maxsolutions = _nj0;
4982 vinfos[1].jointtype = 1;
4983 vinfos[1].foffset = j1;
4984 vinfos[1].indices[0] = _ij1[0];
4985 vinfos[1].indices[1] = _ij1[1];
4986 vinfos[1].maxsolutions = _nj1;
4987 vinfos[2].jointtype = 1;
4988 vinfos[2].foffset = j2;
4989 vinfos[2].indices[0] = _ij2[0];
4990 vinfos[2].indices[1] = _ij2[1];
4991 vinfos[2].maxsolutions = _nj2;
4992 vinfos[3].jointtype = 1;
4993 vinfos[3].foffset = j3;
4994 vinfos[3].indices[0] = _ij3[0];
4995 vinfos[3].indices[1] = _ij3[1];
4996 vinfos[3].maxsolutions = _nj3;
4997 vinfos[4].jointtype = 1;
4998 vinfos[4].foffset = j4;
4999 vinfos[4].indices[0] = _ij4[0];
5000 vinfos[4].indices[1] = _ij4[1];
5001 vinfos[4].maxsolutions = _nj4;
5002 vinfos[5].jointtype = 1;
5003 vinfos[5].foffset = j5;
5004 vinfos[5].indices[0] = _ij5[0];
5005 vinfos[5].indices[1] = _ij5[1];
5006 vinfos[5].maxsolutions = _nj5;
5007 std::vector<int> vfree(0);
5008 solutions.AddSolution(vinfos,vfree);
5009 }
5010 }
5011 }
5012 
5013 }
5014 
5015 }
5016 
5017 } else
5018 {
5019 {
5020 IkReal j5array[1], cj5array[1], sj5array[1];
5021 bool j5valid[1]={false};
5022 _nj5 = 1;
5023 CheckValue<IkReal> x331=IKPowWithIntegerCheck(new_r02,-1);
5024 if(!x331.valid){
5025 continue;
5026 }
5027 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r20*(x331.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr((new_r20*(x331.value)))-1) <= IKFAST_SINCOS_THRESH )
5028  continue;
5029 j5array[0]=IKatan2(((-1.0)*new_r10), (new_r20*(x331.value)));
5030 sj5array[0]=IKsin(j5array[0]);
5031 cj5array[0]=IKcos(j5array[0]);
5032 if( j5array[0] > IKPI )
5033 {
5034  j5array[0]-=IK2PI;
5035 }
5036 else if( j5array[0] < -IKPI )
5037 { j5array[0]+=IK2PI;
5038 }
5039 j5valid[0] = true;
5040 for(int ij5 = 0; ij5 < 1; ++ij5)
5041 {
5042 if( !j5valid[ij5] )
5043 {
5044  continue;
5045 }
5046 _ij5[0] = ij5; _ij5[1] = -1;
5047 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5048 {
5049 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5050 {
5051  j5valid[iij5]=false; _ij5[1] = iij5; break;
5052 }
5053 }
5054 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5055 {
5056 IkReal evalcond[8];
5057 IkReal x332=IKsin(j5);
5058 IkReal x333=IKcos(j5);
5059 IkReal x334=((1.0)*x332);
5060 IkReal x335=((1.0)*x333);
5061 evalcond[0]=(((new_r02*x332))+new_r21);
5062 evalcond[1]=(new_r20+(((-1.0)*new_r02*x335)));
5063 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x334)));
5064 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x335)));
5065 evalcond[4]=(((cj4*x332))+(((-1.0)*new_r01)));
5066 evalcond[5]=((((-1.0)*cj4*x335))+(((-1.0)*new_r00)));
5067 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x333);
5068 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x334))+((new_r21*sj4)));
5069 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
5070 {
5071 continue;
5072 }
5073 }
5074 
5075 {
5076 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5077 vinfos[0].jointtype = 1;
5078 vinfos[0].foffset = j0;
5079 vinfos[0].indices[0] = _ij0[0];
5080 vinfos[0].indices[1] = _ij0[1];
5081 vinfos[0].maxsolutions = _nj0;
5082 vinfos[1].jointtype = 1;
5083 vinfos[1].foffset = j1;
5084 vinfos[1].indices[0] = _ij1[0];
5085 vinfos[1].indices[1] = _ij1[1];
5086 vinfos[1].maxsolutions = _nj1;
5087 vinfos[2].jointtype = 1;
5088 vinfos[2].foffset = j2;
5089 vinfos[2].indices[0] = _ij2[0];
5090 vinfos[2].indices[1] = _ij2[1];
5091 vinfos[2].maxsolutions = _nj2;
5092 vinfos[3].jointtype = 1;
5093 vinfos[3].foffset = j3;
5094 vinfos[3].indices[0] = _ij3[0];
5095 vinfos[3].indices[1] = _ij3[1];
5096 vinfos[3].maxsolutions = _nj3;
5097 vinfos[4].jointtype = 1;
5098 vinfos[4].foffset = j4;
5099 vinfos[4].indices[0] = _ij4[0];
5100 vinfos[4].indices[1] = _ij4[1];
5101 vinfos[4].maxsolutions = _nj4;
5102 vinfos[5].jointtype = 1;
5103 vinfos[5].foffset = j5;
5104 vinfos[5].indices[0] = _ij5[0];
5105 vinfos[5].indices[1] = _ij5[1];
5106 vinfos[5].maxsolutions = _nj5;
5107 std::vector<int> vfree(0);
5108 solutions.AddSolution(vinfos,vfree);
5109 }
5110 }
5111 }
5112 
5113 }
5114 
5115 }
5116 
5117 } else
5118 {
5119 {
5120 IkReal j5array[1], cj5array[1], sj5array[1];
5121 bool j5valid[1]={false};
5122 _nj5 = 1;
5123 CheckValue<IkReal> x336 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5124 if(!x336.valid){
5125 continue;
5126 }
5127 CheckValue<IkReal> x337=IKPowWithIntegerCheck(IKsign(new_r02),-1);
5128 if(!x337.valid){
5129 continue;
5130 }
5131 j5array[0]=((-1.5707963267949)+(x336.value)+(((1.5707963267949)*(x337.value))));
5132 sj5array[0]=IKsin(j5array[0]);
5133 cj5array[0]=IKcos(j5array[0]);
5134 if( j5array[0] > IKPI )
5135 {
5136  j5array[0]-=IK2PI;
5137 }
5138 else if( j5array[0] < -IKPI )
5139 { j5array[0]+=IK2PI;
5140 }
5141 j5valid[0] = true;
5142 for(int ij5 = 0; ij5 < 1; ++ij5)
5143 {
5144 if( !j5valid[ij5] )
5145 {
5146  continue;
5147 }
5148 _ij5[0] = ij5; _ij5[1] = -1;
5149 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5150 {
5151 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5152 {
5153  j5valid[iij5]=false; _ij5[1] = iij5; break;
5154 }
5155 }
5156 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5157 {
5158 IkReal evalcond[8];
5159 IkReal x338=IKsin(j5);
5160 IkReal x339=IKcos(j5);
5161 IkReal x340=((1.0)*x338);
5162 IkReal x341=((1.0)*x339);
5163 evalcond[0]=(((new_r02*x338))+new_r21);
5164 evalcond[1]=((((-1.0)*new_r02*x341))+new_r20);
5165 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x340)));
5166 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x341)));
5167 evalcond[4]=(((cj4*x338))+(((-1.0)*new_r01)));
5168 evalcond[5]=((((-1.0)*cj4*x341))+(((-1.0)*new_r00)));
5169 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x339);
5170 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x340)));
5171 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5172 {
5173 continue;
5174 }
5175 }
5176 
5177 {
5178 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5179 vinfos[0].jointtype = 1;
5180 vinfos[0].foffset = j0;
5181 vinfos[0].indices[0] = _ij0[0];
5182 vinfos[0].indices[1] = _ij0[1];
5183 vinfos[0].maxsolutions = _nj0;
5184 vinfos[1].jointtype = 1;
5185 vinfos[1].foffset = j1;
5186 vinfos[1].indices[0] = _ij1[0];
5187 vinfos[1].indices[1] = _ij1[1];
5188 vinfos[1].maxsolutions = _nj1;
5189 vinfos[2].jointtype = 1;
5190 vinfos[2].foffset = j2;
5191 vinfos[2].indices[0] = _ij2[0];
5192 vinfos[2].indices[1] = _ij2[1];
5193 vinfos[2].maxsolutions = _nj2;
5194 vinfos[3].jointtype = 1;
5195 vinfos[3].foffset = j3;
5196 vinfos[3].indices[0] = _ij3[0];
5197 vinfos[3].indices[1] = _ij3[1];
5198 vinfos[3].maxsolutions = _nj3;
5199 vinfos[4].jointtype = 1;
5200 vinfos[4].foffset = j4;
5201 vinfos[4].indices[0] = _ij4[0];
5202 vinfos[4].indices[1] = _ij4[1];
5203 vinfos[4].maxsolutions = _nj4;
5204 vinfos[5].jointtype = 1;
5205 vinfos[5].foffset = j5;
5206 vinfos[5].indices[0] = _ij5[0];
5207 vinfos[5].indices[1] = _ij5[1];
5208 vinfos[5].maxsolutions = _nj5;
5209 std::vector<int> vfree(0);
5210 solutions.AddSolution(vinfos,vfree);
5211 }
5212 }
5213 }
5214 
5215 }
5216 
5217 }
5218 
5219 }
5220 } while(0);
5221 if( bgotonextstatement )
5222 {
5223 bool bgotonextstatement = true;
5224 do
5225 {
5226 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
5227 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5228 {
5229 bgotonextstatement=false;
5230 {
5231 IkReal j5eval[1];
5232 new_r21=0;
5233 new_r20=0;
5234 new_r02=0;
5235 new_r12=0;
5236 j5eval[0]=1.0;
5237 if( IKabs(j5eval[0]) < 0.0000000100000000 )
5238 {
5239 continue; // no branches [j5]
5240 
5241 } else
5242 {
5243 IkReal op[2+1], zeror[2];
5244 int numroots;
5245 op[0]=-1.0;
5246 op[1]=0;
5247 op[2]=1.0;
5248 polyroots2(op,zeror,numroots);
5249 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
5250 int numsolutions = 0;
5251 for(int ij5 = 0; ij5 < numroots; ++ij5)
5252 {
5253 IkReal htj5 = zeror[ij5];
5254 tempj5array[0]=((2.0)*(atan(htj5)));
5255 for(int kj5 = 0; kj5 < 1; ++kj5)
5256 {
5257 j5array[numsolutions] = tempj5array[kj5];
5258 if( j5array[numsolutions] > IKPI )
5259 {
5260  j5array[numsolutions]-=IK2PI;
5261 }
5262 else if( j5array[numsolutions] < -IKPI )
5263 {
5264  j5array[numsolutions]+=IK2PI;
5265 }
5266 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
5267 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
5268 numsolutions++;
5269 }
5270 }
5271 bool j5valid[2]={true,true};
5272 _nj5 = 2;
5273 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
5274  {
5275 if( !j5valid[ij5] )
5276 {
5277  continue;
5278 }
5279  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5280 htj5 = IKtan(j5/2);
5281 
5282 _ij5[0] = ij5; _ij5[1] = -1;
5283 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
5284 {
5285 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5286 {
5287  j5valid[iij5]=false; _ij5[1] = iij5; break;
5288 }
5289 }
5290 {
5291 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5292 vinfos[0].jointtype = 1;
5293 vinfos[0].foffset = j0;
5294 vinfos[0].indices[0] = _ij0[0];
5295 vinfos[0].indices[1] = _ij0[1];
5296 vinfos[0].maxsolutions = _nj0;
5297 vinfos[1].jointtype = 1;
5298 vinfos[1].foffset = j1;
5299 vinfos[1].indices[0] = _ij1[0];
5300 vinfos[1].indices[1] = _ij1[1];
5301 vinfos[1].maxsolutions = _nj1;
5302 vinfos[2].jointtype = 1;
5303 vinfos[2].foffset = j2;
5304 vinfos[2].indices[0] = _ij2[0];
5305 vinfos[2].indices[1] = _ij2[1];
5306 vinfos[2].maxsolutions = _nj2;
5307 vinfos[3].jointtype = 1;
5308 vinfos[3].foffset = j3;
5309 vinfos[3].indices[0] = _ij3[0];
5310 vinfos[3].indices[1] = _ij3[1];
5311 vinfos[3].maxsolutions = _nj3;
5312 vinfos[4].jointtype = 1;
5313 vinfos[4].foffset = j4;
5314 vinfos[4].indices[0] = _ij4[0];
5315 vinfos[4].indices[1] = _ij4[1];
5316 vinfos[4].maxsolutions = _nj4;
5317 vinfos[5].jointtype = 1;
5318 vinfos[5].foffset = j5;
5319 vinfos[5].indices[0] = _ij5[0];
5320 vinfos[5].indices[1] = _ij5[1];
5321 vinfos[5].maxsolutions = _nj5;
5322 std::vector<int> vfree(0);
5323 solutions.AddSolution(vinfos,vfree);
5324 }
5325  }
5326 
5327 }
5328 
5329 }
5330 
5331 }
5332 } while(0);
5333 if( bgotonextstatement )
5334 {
5335 bool bgotonextstatement = true;
5336 do
5337 {
5338 if( 1 )
5339 {
5340 bgotonextstatement=false;
5341 continue; // branch miss [j5]
5342 
5343 }
5344 } while(0);
5345 if( bgotonextstatement )
5346 {
5347 }
5348 }
5349 }
5350 }
5351 }
5352 }
5353 }
5354 }
5355 }
5356 }
5357 }
5358 
5359 } else
5360 {
5361 {
5362 IkReal j5array[1], cj5array[1], sj5array[1];
5363 bool j5valid[1]={false};
5364 _nj5 = 1;
5365 CheckValue<IkReal> x343=IKPowWithIntegerCheck(sj4,-1);
5366 if(!x343.valid){
5367 continue;
5368 }
5369 IkReal x342=x343.value;
5370 CheckValue<IkReal> x344=IKPowWithIntegerCheck(cj3,-1);
5371 if(!x344.valid){
5372 continue;
5373 }
5374 CheckValue<IkReal> x345=IKPowWithIntegerCheck(cj4,-1);
5375 if(!x345.valid){
5376 continue;
5377 }
5378 if( IKabs((x342*(x344.value)*(x345.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x342)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x342*(x344.value)*(x345.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4))))))+IKsqr(((-1.0)*new_r20*x342))-1) <= IKFAST_SINCOS_THRESH )
5379  continue;
5380 j5array[0]=IKatan2((x342*(x344.value)*(x345.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4))))), ((-1.0)*new_r20*x342));
5381 sj5array[0]=IKsin(j5array[0]);
5382 cj5array[0]=IKcos(j5array[0]);
5383 if( j5array[0] > IKPI )
5384 {
5385  j5array[0]-=IK2PI;
5386 }
5387 else if( j5array[0] < -IKPI )
5388 { j5array[0]+=IK2PI;
5389 }
5390 j5valid[0] = true;
5391 for(int ij5 = 0; ij5 < 1; ++ij5)
5392 {
5393 if( !j5valid[ij5] )
5394 {
5395  continue;
5396 }
5397 _ij5[0] = ij5; _ij5[1] = -1;
5398 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5399 {
5400 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5401 {
5402  j5valid[iij5]=false; _ij5[1] = iij5; break;
5403 }
5404 }
5405 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5406 {
5407 IkReal evalcond[12];
5408 IkReal x346=IKsin(j5);
5409 IkReal x347=IKcos(j5);
5410 IkReal x348=(cj3*new_r00);
5411 IkReal x349=(cj3*cj4);
5412 IkReal x350=((1.0)*sj3);
5413 IkReal x351=((1.0)*x346);
5414 IkReal x352=(sj3*x346);
5415 IkReal x353=((1.0)*x347);
5416 evalcond[0]=(((sj4*x347))+new_r20);
5417 evalcond[1]=(new_r21+(((-1.0)*sj4*x351)));
5418 evalcond[2]=(((new_r11*sj3))+((cj4*x346))+((cj3*new_r01)));
5419 evalcond[3]=((((-1.0)*x351))+((cj3*new_r10))+(((-1.0)*new_r00*x350)));
5420 evalcond[4]=((((-1.0)*x353))+(((-1.0)*new_r01*x350))+((cj3*new_r11)));
5421 evalcond[5]=(((sj3*x347))+((x346*x349))+new_r01);
5422 evalcond[6]=(((new_r10*sj3))+x348+(((-1.0)*cj4*x353)));
5423 evalcond[7]=((((-1.0)*x349*x353))+x352+new_r00);
5424 evalcond[8]=((((-1.0)*cj3*x353))+((cj4*x352))+new_r11);
5425 evalcond[9]=((((-1.0)*cj3*x351))+(((-1.0)*cj4*x347*x350))+new_r10);
5426 evalcond[10]=(((new_r20*sj4))+x347+(((-1.0)*cj4*x348))+(((-1.0)*cj4*new_r10*x350)));
5427 evalcond[11]=((((-1.0)*x351))+(((-1.0)*cj4*new_r11*x350))+(((-1.0)*new_r01*x349))+((new_r21*sj4)));
5428 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
5429 {
5430 continue;
5431 }
5432 }
5433 
5434 {
5435 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5436 vinfos[0].jointtype = 1;
5437 vinfos[0].foffset = j0;
5438 vinfos[0].indices[0] = _ij0[0];
5439 vinfos[0].indices[1] = _ij0[1];
5440 vinfos[0].maxsolutions = _nj0;
5441 vinfos[1].jointtype = 1;
5442 vinfos[1].foffset = j1;
5443 vinfos[1].indices[0] = _ij1[0];
5444 vinfos[1].indices[1] = _ij1[1];
5445 vinfos[1].maxsolutions = _nj1;
5446 vinfos[2].jointtype = 1;
5447 vinfos[2].foffset = j2;
5448 vinfos[2].indices[0] = _ij2[0];
5449 vinfos[2].indices[1] = _ij2[1];
5450 vinfos[2].maxsolutions = _nj2;
5451 vinfos[3].jointtype = 1;
5452 vinfos[3].foffset = j3;
5453 vinfos[3].indices[0] = _ij3[0];
5454 vinfos[3].indices[1] = _ij3[1];
5455 vinfos[3].maxsolutions = _nj3;
5456 vinfos[4].jointtype = 1;
5457 vinfos[4].foffset = j4;
5458 vinfos[4].indices[0] = _ij4[0];
5459 vinfos[4].indices[1] = _ij4[1];
5460 vinfos[4].maxsolutions = _nj4;
5461 vinfos[5].jointtype = 1;
5462 vinfos[5].foffset = j5;
5463 vinfos[5].indices[0] = _ij5[0];
5464 vinfos[5].indices[1] = _ij5[1];
5465 vinfos[5].maxsolutions = _nj5;
5466 std::vector<int> vfree(0);
5467 solutions.AddSolution(vinfos,vfree);
5468 }
5469 }
5470 }
5471 
5472 }
5473 
5474 }
5475 
5476 } else
5477 {
5478 {
5479 IkReal j5array[1], cj5array[1], sj5array[1];
5480 bool j5valid[1]={false};
5481 _nj5 = 1;
5482 CheckValue<IkReal> x355=IKPowWithIntegerCheck(sj4,-1);
5483 if(!x355.valid){
5484 continue;
5485 }
5486 IkReal x354=x355.value;
5487 CheckValue<IkReal> x356=IKPowWithIntegerCheck(sj3,-1);
5488 if(!x356.valid){
5489 continue;
5490 }
5491 if( IKabs((x354*(x356.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x354)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x354*(x356.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4))))))+IKsqr(((-1.0)*new_r20*x354))-1) <= IKFAST_SINCOS_THRESH )
5492  continue;
5493 j5array[0]=IKatan2((x354*(x356.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4))))), ((-1.0)*new_r20*x354));
5494 sj5array[0]=IKsin(j5array[0]);
5495 cj5array[0]=IKcos(j5array[0]);
5496 if( j5array[0] > IKPI )
5497 {
5498  j5array[0]-=IK2PI;
5499 }
5500 else if( j5array[0] < -IKPI )
5501 { j5array[0]+=IK2PI;
5502 }
5503 j5valid[0] = true;
5504 for(int ij5 = 0; ij5 < 1; ++ij5)
5505 {
5506 if( !j5valid[ij5] )
5507 {
5508  continue;
5509 }
5510 _ij5[0] = ij5; _ij5[1] = -1;
5511 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5512 {
5513 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5514 {
5515  j5valid[iij5]=false; _ij5[1] = iij5; break;
5516 }
5517 }
5518 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5519 {
5520 IkReal evalcond[12];
5521 IkReal x357=IKsin(j5);
5522 IkReal x358=IKcos(j5);
5523 IkReal x359=(cj3*new_r00);
5524 IkReal x360=(cj3*cj4);
5525 IkReal x361=((1.0)*sj3);
5526 IkReal x362=((1.0)*x357);
5527 IkReal x363=(sj3*x357);
5528 IkReal x364=((1.0)*x358);
5529 evalcond[0]=(((sj4*x358))+new_r20);
5530 evalcond[1]=((((-1.0)*sj4*x362))+new_r21);
5531 evalcond[2]=(((new_r11*sj3))+((cj4*x357))+((cj3*new_r01)));
5532 evalcond[3]=((((-1.0)*new_r00*x361))+(((-1.0)*x362))+((cj3*new_r10)));
5533 evalcond[4]=((((-1.0)*x364))+((cj3*new_r11))+(((-1.0)*new_r01*x361)));
5534 evalcond[5]=(((sj3*x358))+new_r01+((x357*x360)));
5535 evalcond[6]=((((-1.0)*cj4*x364))+((new_r10*sj3))+x359);
5536 evalcond[7]=((((-1.0)*x360*x364))+x363+new_r00);
5537 evalcond[8]=(((cj4*x363))+new_r11+(((-1.0)*cj3*x364)));
5538 evalcond[9]=((((-1.0)*cj4*x358*x361))+new_r10+(((-1.0)*cj3*x362)));
5539 evalcond[10]=(((new_r20*sj4))+x358+(((-1.0)*cj4*new_r10*x361))+(((-1.0)*cj4*x359)));
5540 evalcond[11]=((((-1.0)*x362))+(((-1.0)*cj4*new_r11*x361))+((new_r21*sj4))+(((-1.0)*new_r01*x360)));
5541 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
5542 {
5543 continue;
5544 }
5545 }
5546 
5547 {
5548 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5549 vinfos[0].jointtype = 1;
5550 vinfos[0].foffset = j0;
5551 vinfos[0].indices[0] = _ij0[0];
5552 vinfos[0].indices[1] = _ij0[1];
5553 vinfos[0].maxsolutions = _nj0;
5554 vinfos[1].jointtype = 1;
5555 vinfos[1].foffset = j1;
5556 vinfos[1].indices[0] = _ij1[0];
5557 vinfos[1].indices[1] = _ij1[1];
5558 vinfos[1].maxsolutions = _nj1;
5559 vinfos[2].jointtype = 1;
5560 vinfos[2].foffset = j2;
5561 vinfos[2].indices[0] = _ij2[0];
5562 vinfos[2].indices[1] = _ij2[1];
5563 vinfos[2].maxsolutions = _nj2;
5564 vinfos[3].jointtype = 1;
5565 vinfos[3].foffset = j3;
5566 vinfos[3].indices[0] = _ij3[0];
5567 vinfos[3].indices[1] = _ij3[1];
5568 vinfos[3].maxsolutions = _nj3;
5569 vinfos[4].jointtype = 1;
5570 vinfos[4].foffset = j4;
5571 vinfos[4].indices[0] = _ij4[0];
5572 vinfos[4].indices[1] = _ij4[1];
5573 vinfos[4].maxsolutions = _nj4;
5574 vinfos[5].jointtype = 1;
5575 vinfos[5].foffset = j5;
5576 vinfos[5].indices[0] = _ij5[0];
5577 vinfos[5].indices[1] = _ij5[1];
5578 vinfos[5].maxsolutions = _nj5;
5579 std::vector<int> vfree(0);
5580 solutions.AddSolution(vinfos,vfree);
5581 }
5582 }
5583 }
5584 
5585 }
5586 
5587 }
5588 
5589 } else
5590 {
5591 {
5592 IkReal j5array[1], cj5array[1], sj5array[1];
5593 bool j5valid[1]={false};
5594 _nj5 = 1;
5595 CheckValue<IkReal> x365=IKPowWithIntegerCheck(IKsign(sj4),-1);
5596 if(!x365.valid){
5597 continue;
5598 }
5599 CheckValue<IkReal> x366 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
5600 if(!x366.valid){
5601 continue;
5602 }
5603 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x365.value)))+(x366.value));
5604 sj5array[0]=IKsin(j5array[0]);
5605 cj5array[0]=IKcos(j5array[0]);
5606 if( j5array[0] > IKPI )
5607 {
5608  j5array[0]-=IK2PI;
5609 }
5610 else if( j5array[0] < -IKPI )
5611 { j5array[0]+=IK2PI;
5612 }
5613 j5valid[0] = true;
5614 for(int ij5 = 0; ij5 < 1; ++ij5)
5615 {
5616 if( !j5valid[ij5] )
5617 {
5618  continue;
5619 }
5620 _ij5[0] = ij5; _ij5[1] = -1;
5621 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5622 {
5623 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5624 {
5625  j5valid[iij5]=false; _ij5[1] = iij5; break;
5626 }
5627 }
5628 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5629 {
5630 IkReal evalcond[12];
5631 IkReal x367=IKsin(j5);
5632 IkReal x368=IKcos(j5);
5633 IkReal x369=(cj3*new_r00);
5634 IkReal x370=(cj3*cj4);
5635 IkReal x371=((1.0)*sj3);
5636 IkReal x372=((1.0)*x367);
5637 IkReal x373=(sj3*x367);
5638 IkReal x374=((1.0)*x368);
5639 evalcond[0]=(((sj4*x368))+new_r20);
5640 evalcond[1]=((((-1.0)*sj4*x372))+new_r21);
5641 evalcond[2]=(((new_r11*sj3))+((cj4*x367))+((cj3*new_r01)));
5642 evalcond[3]=((((-1.0)*new_r00*x371))+(((-1.0)*x372))+((cj3*new_r10)));
5643 evalcond[4]=((((-1.0)*x374))+((cj3*new_r11))+(((-1.0)*new_r01*x371)));
5644 evalcond[5]=(((x367*x370))+((sj3*x368))+new_r01);
5645 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x374))+x369);
5646 evalcond[7]=((((-1.0)*x370*x374))+x373+new_r00);
5647 evalcond[8]=(((cj4*x373))+(((-1.0)*cj3*x374))+new_r11);
5648 evalcond[9]=((((-1.0)*cj4*x368*x371))+(((-1.0)*cj3*x372))+new_r10);
5649 evalcond[10]=(((new_r20*sj4))+x368+(((-1.0)*cj4*x369))+(((-1.0)*cj4*new_r10*x371)));
5650 evalcond[11]=((((-1.0)*x372))+(((-1.0)*cj4*new_r11*x371))+((new_r21*sj4))+(((-1.0)*new_r01*x370)));
5651 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
5652 {
5653 continue;
5654 }
5655 }
5656 
5657 {
5658 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5659 vinfos[0].jointtype = 1;
5660 vinfos[0].foffset = j0;
5661 vinfos[0].indices[0] = _ij0[0];
5662 vinfos[0].indices[1] = _ij0[1];
5663 vinfos[0].maxsolutions = _nj0;
5664 vinfos[1].jointtype = 1;
5665 vinfos[1].foffset = j1;
5666 vinfos[1].indices[0] = _ij1[0];
5667 vinfos[1].indices[1] = _ij1[1];
5668 vinfos[1].maxsolutions = _nj1;
5669 vinfos[2].jointtype = 1;
5670 vinfos[2].foffset = j2;
5671 vinfos[2].indices[0] = _ij2[0];
5672 vinfos[2].indices[1] = _ij2[1];
5673 vinfos[2].maxsolutions = _nj2;
5674 vinfos[3].jointtype = 1;
5675 vinfos[3].foffset = j3;
5676 vinfos[3].indices[0] = _ij3[0];
5677 vinfos[3].indices[1] = _ij3[1];
5678 vinfos[3].maxsolutions = _nj3;
5679 vinfos[4].jointtype = 1;
5680 vinfos[4].foffset = j4;
5681 vinfos[4].indices[0] = _ij4[0];
5682 vinfos[4].indices[1] = _ij4[1];
5683 vinfos[4].maxsolutions = _nj4;
5684 vinfos[5].jointtype = 1;
5685 vinfos[5].foffset = j5;
5686 vinfos[5].indices[0] = _ij5[0];
5687 vinfos[5].indices[1] = _ij5[1];
5688 vinfos[5].maxsolutions = _nj5;
5689 std::vector<int> vfree(0);
5690 solutions.AddSolution(vinfos,vfree);
5691 }
5692 }
5693 }
5694 
5695 }
5696 
5697 }
5698 }
5699 }
5700 
5701 }
5702 
5703 }
5704 
5705 } else
5706 {
5707 {
5708 IkReal j5array[1], cj5array[1], sj5array[1];
5709 bool j5valid[1]={false};
5710 _nj5 = 1;
5711 CheckValue<IkReal> x375=IKPowWithIntegerCheck(IKsign(sj4),-1);
5712 if(!x375.valid){
5713 continue;
5714 }
5715 CheckValue<IkReal> x376 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
5716 if(!x376.valid){
5717 continue;
5718 }
5719 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x375.value)))+(x376.value));
5720 sj5array[0]=IKsin(j5array[0]);
5721 cj5array[0]=IKcos(j5array[0]);
5722 if( j5array[0] > IKPI )
5723 {
5724  j5array[0]-=IK2PI;
5725 }
5726 else if( j5array[0] < -IKPI )
5727 { j5array[0]+=IK2PI;
5728 }
5729 j5valid[0] = true;
5730 for(int ij5 = 0; ij5 < 1; ++ij5)
5731 {
5732 if( !j5valid[ij5] )
5733 {
5734  continue;
5735 }
5736 _ij5[0] = ij5; _ij5[1] = -1;
5737 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5738 {
5739 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5740 {
5741  j5valid[iij5]=false; _ij5[1] = iij5; break;
5742 }
5743 }
5744 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5745 {
5746 IkReal evalcond[2];
5747 evalcond[0]=(((sj4*(IKcos(j5))))+new_r20);
5748 evalcond[1]=((((-1.0)*sj4*(IKsin(j5))))+new_r21);
5749 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5750 {
5751 continue;
5752 }
5753 }
5754 
5755 {
5756 IkReal j3eval[3];
5757 j3eval[0]=sj4;
5758 j3eval[1]=IKsign(sj4);
5759 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5760 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5761 {
5762 {
5763 IkReal j3eval[2];
5764 j3eval[0]=cj5;
5765 j3eval[1]=sj4;
5766 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5767 {
5768 {
5769 IkReal evalcond[5];
5770 bool bgotonextstatement = true;
5771 do
5772 {
5773 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
5774 evalcond[1]=new_r20;
5775 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5776 {
5777 bgotonextstatement=false;
5778 {
5779 IkReal j3array[1], cj3array[1], sj3array[1];
5780 bool j3valid[1]={false};
5781 _nj3 = 1;
5782 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
5783  continue;
5784 j3array[0]=IKatan2(((-1.0)*new_r00), new_r10);
5785 sj3array[0]=IKsin(j3array[0]);
5786 cj3array[0]=IKcos(j3array[0]);
5787 if( j3array[0] > IKPI )
5788 {
5789  j3array[0]-=IK2PI;
5790 }
5791 else if( j3array[0] < -IKPI )
5792 { j3array[0]+=IK2PI;
5793 }
5794 j3valid[0] = true;
5795 for(int ij3 = 0; ij3 < 1; ++ij3)
5796 {
5797 if( !j3valid[ij3] )
5798 {
5799  continue;
5800 }
5801 _ij3[0] = ij3; _ij3[1] = -1;
5802 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5803 {
5804 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5805 {
5806  j3valid[iij3]=false; _ij3[1] = iij3; break;
5807 }
5808 }
5809 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5810 {
5811 IkReal evalcond[18];
5812 IkReal x377=IKsin(j3);
5813 IkReal x378=IKcos(j3);
5814 IkReal x379=((1.0)*sj4);
5815 IkReal x380=((1.0)*new_r22);
5816 IkReal x381=(new_r00*x378);
5817 IkReal x382=(new_r11*x377);
5818 IkReal x383=(new_r01*x378);
5819 IkReal x384=(new_r02*x378);
5820 IkReal x385=(new_r12*x377);
5821 IkReal x386=((1.0)*x377);
5822 IkReal x387=(new_r10*x377);
5823 IkReal x388=(x377*x380);
5824 evalcond[0]=(x377+new_r00);
5825 evalcond[1]=(new_r01+((new_r22*x378)));
5826 evalcond[2]=(new_r11+((new_r22*x377)));
5827 evalcond[3]=((((-1.0)*x378))+new_r10);
5828 evalcond[4]=((((-1.0)*x378*x379))+new_r02);
5829 evalcond[5]=((((-1.0)*x377*x379))+new_r12);
5830 evalcond[6]=(x387+x381);
5831 evalcond[7]=(((new_r12*x378))+(((-1.0)*new_r02*x386)));
5832 evalcond[8]=((((-1.0)*new_r01*x386))+((new_r11*x378)));
5833 evalcond[9]=(x382+x383+new_r22);
5834 evalcond[10]=((-1.0)+(((-1.0)*new_r00*x386))+((new_r10*x378)));
5835 evalcond[11]=((((-1.0)*x379))+x384+x385);
5836 evalcond[12]=((((-1.0)*x379*x381))+(((-1.0)*x379*x387)));
5837 evalcond[13]=((((-1.0)*x380*x387))+(((-1.0)*x380*x381)));
5838 evalcond[14]=((((-1.0)*x380*x385))+(((-1.0)*x380*x384))+((new_r22*sj4)));
5839 evalcond[15]=((((-1.0)*cj4*new_r21))+(((-1.0)*x379*x383))+(((-1.0)*x379*x382)));
5840 evalcond[16]=((-1.0)+(((-1.0)*x380*x383))+(((-1.0)*x380*x382))+(sj4*sj4));
5841 evalcond[17]=((1.0)+(((-1.0)*x379*x385))+(((-1.0)*x379*x384))+(((-1.0)*new_r22*x380)));
5842 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
5843 {
5844 continue;
5845 }
5846 }
5847 
5848 {
5849 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5850 vinfos[0].jointtype = 1;
5851 vinfos[0].foffset = j0;
5852 vinfos[0].indices[0] = _ij0[0];
5853 vinfos[0].indices[1] = _ij0[1];
5854 vinfos[0].maxsolutions = _nj0;
5855 vinfos[1].jointtype = 1;
5856 vinfos[1].foffset = j1;
5857 vinfos[1].indices[0] = _ij1[0];
5858 vinfos[1].indices[1] = _ij1[1];
5859 vinfos[1].maxsolutions = _nj1;
5860 vinfos[2].jointtype = 1;
5861 vinfos[2].foffset = j2;
5862 vinfos[2].indices[0] = _ij2[0];
5863 vinfos[2].indices[1] = _ij2[1];
5864 vinfos[2].maxsolutions = _nj2;
5865 vinfos[3].jointtype = 1;
5866 vinfos[3].foffset = j3;
5867 vinfos[3].indices[0] = _ij3[0];
5868 vinfos[3].indices[1] = _ij3[1];
5869 vinfos[3].maxsolutions = _nj3;
5870 vinfos[4].jointtype = 1;
5871 vinfos[4].foffset = j4;
5872 vinfos[4].indices[0] = _ij4[0];
5873 vinfos[4].indices[1] = _ij4[1];
5874 vinfos[4].maxsolutions = _nj4;
5875 vinfos[5].jointtype = 1;
5876 vinfos[5].foffset = j5;
5877 vinfos[5].indices[0] = _ij5[0];
5878 vinfos[5].indices[1] = _ij5[1];
5879 vinfos[5].maxsolutions = _nj5;
5880 std::vector<int> vfree(0);
5881 solutions.AddSolution(vinfos,vfree);
5882 }
5883 }
5884 }
5885 
5886 }
5887 } while(0);
5888 if( bgotonextstatement )
5889 {
5890 bool bgotonextstatement = true;
5891 do
5892 {
5893 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
5894 evalcond[1]=new_r20;
5895 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5896 {
5897 bgotonextstatement=false;
5898 {
5899 IkReal j3array[1], cj3array[1], sj3array[1];
5900 bool j3valid[1]={false};
5901 _nj3 = 1;
5902 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
5903  continue;
5904 j3array[0]=IKatan2(new_r00, ((-1.0)*new_r10));
5905 sj3array[0]=IKsin(j3array[0]);
5906 cj3array[0]=IKcos(j3array[0]);
5907 if( j3array[0] > IKPI )
5908 {
5909  j3array[0]-=IK2PI;
5910 }
5911 else if( j3array[0] < -IKPI )
5912 { j3array[0]+=IK2PI;
5913 }
5914 j3valid[0] = true;
5915 for(int ij3 = 0; ij3 < 1; ++ij3)
5916 {
5917 if( !j3valid[ij3] )
5918 {
5919  continue;
5920 }
5921 _ij3[0] = ij3; _ij3[1] = -1;
5922 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5923 {
5924 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5925 {
5926  j3valid[iij3]=false; _ij3[1] = iij3; break;
5927 }
5928 }
5929 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5930 {
5931 IkReal evalcond[18];
5932 IkReal x389=IKcos(j3);
5933 IkReal x390=IKsin(j3);
5934 IkReal x391=(new_r22*sj4);
5935 IkReal x392=((1.0)*sj4);
5936 IkReal x393=((1.0)*new_r22);
5937 IkReal x394=(new_r00*x389);
5938 IkReal x395=(new_r11*x390);
5939 IkReal x396=(new_r01*x389);
5940 IkReal x397=(new_r02*x389);
5941 IkReal x398=(new_r12*x390);
5942 IkReal x399=((1.0)*x390);
5943 IkReal x400=(new_r10*x390);
5944 IkReal x401=(x390*x393);
5945 evalcond[0]=(x389+new_r10);
5946 evalcond[1]=((((-1.0)*x399))+new_r00);
5947 evalcond[2]=((((-1.0)*x389*x392))+new_r02);
5948 evalcond[3]=((((-1.0)*x390*x392))+new_r12);
5949 evalcond[4]=((((-1.0)*x389*x393))+new_r01);
5950 evalcond[5]=(new_r11+(((-1.0)*x401)));
5951 evalcond[6]=(x394+x400);
5952 evalcond[7]=(((new_r12*x389))+(((-1.0)*new_r02*x399)));
5953 evalcond[8]=((((-1.0)*new_r01*x399))+((new_r11*x389)));
5954 evalcond[9]=((1.0)+((new_r10*x389))+(((-1.0)*new_r00*x399)));
5955 evalcond[10]=((((-1.0)*x392))+x397+x398);
5956 evalcond[11]=((((-1.0)*x393))+x395+x396);
5957 evalcond[12]=((((-1.0)*x392*x400))+(((-1.0)*x392*x394)));
5958 evalcond[13]=((((-1.0)*x393*x400))+(((-1.0)*x393*x394)));
5959 evalcond[14]=(x391+(((-1.0)*x393*x398))+(((-1.0)*x393*x397)));
5960 evalcond[15]=(x391+(((-1.0)*x392*x395))+(((-1.0)*x392*x396)));
5961 evalcond[16]=((1.0)+(((-1.0)*new_r22*x393))+(((-1.0)*x392*x398))+(((-1.0)*x392*x397)));
5962 evalcond[17]=((1.0)+(((-1.0)*sj4*x392))+(((-1.0)*x393*x395))+(((-1.0)*x393*x396)));
5963 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
5964 {
5965 continue;
5966 }
5967 }
5968 
5969 {
5970 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5971 vinfos[0].jointtype = 1;
5972 vinfos[0].foffset = j0;
5973 vinfos[0].indices[0] = _ij0[0];
5974 vinfos[0].indices[1] = _ij0[1];
5975 vinfos[0].maxsolutions = _nj0;
5976 vinfos[1].jointtype = 1;
5977 vinfos[1].foffset = j1;
5978 vinfos[1].indices[0] = _ij1[0];
5979 vinfos[1].indices[1] = _ij1[1];
5980 vinfos[1].maxsolutions = _nj1;
5981 vinfos[2].jointtype = 1;
5982 vinfos[2].foffset = j2;
5983 vinfos[2].indices[0] = _ij2[0];
5984 vinfos[2].indices[1] = _ij2[1];
5985 vinfos[2].maxsolutions = _nj2;
5986 vinfos[3].jointtype = 1;
5987 vinfos[3].foffset = j3;
5988 vinfos[3].indices[0] = _ij3[0];
5989 vinfos[3].indices[1] = _ij3[1];
5990 vinfos[3].maxsolutions = _nj3;
5991 vinfos[4].jointtype = 1;
5992 vinfos[4].foffset = j4;
5993 vinfos[4].indices[0] = _ij4[0];
5994 vinfos[4].indices[1] = _ij4[1];
5995 vinfos[4].maxsolutions = _nj4;
5996 vinfos[5].jointtype = 1;
5997 vinfos[5].foffset = j5;
5998 vinfos[5].indices[0] = _ij5[0];
5999 vinfos[5].indices[1] = _ij5[1];
6000 vinfos[5].maxsolutions = _nj5;
6001 std::vector<int> vfree(0);
6002 solutions.AddSolution(vinfos,vfree);
6003 }
6004 }
6005 }
6006 
6007 }
6008 } while(0);
6009 if( bgotonextstatement )
6010 {
6011 bool bgotonextstatement = true;
6012 do
6013 {
6014 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
6015 evalcond[1]=new_r20;
6016 evalcond[2]=new_r02;
6017 evalcond[3]=new_r12;
6018 evalcond[4]=new_r21;
6019 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 )
6020 {
6021 bgotonextstatement=false;
6022 {
6023 IkReal j3array[1], cj3array[1], sj3array[1];
6024 bool j3valid[1]={false};
6025 _nj3 = 1;
6026 IkReal x402=((1.0)*new_r01);
6027 if( IKabs(((((-1.0)*cj5*x402))+(((-1.0)*new_r00*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj5*x402))+((cj5*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj5*x402))+(((-1.0)*new_r00*sj5))))+IKsqr(((((-1.0)*sj5*x402))+((cj5*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
6028  continue;
6029 j3array[0]=IKatan2(((((-1.0)*cj5*x402))+(((-1.0)*new_r00*sj5))), ((((-1.0)*sj5*x402))+((cj5*new_r00))));
6030 sj3array[0]=IKsin(j3array[0]);
6031 cj3array[0]=IKcos(j3array[0]);
6032 if( j3array[0] > IKPI )
6033 {
6034  j3array[0]-=IK2PI;
6035 }
6036 else if( j3array[0] < -IKPI )
6037 { j3array[0]+=IK2PI;
6038 }
6039 j3valid[0] = true;
6040 for(int ij3 = 0; ij3 < 1; ++ij3)
6041 {
6042 if( !j3valid[ij3] )
6043 {
6044  continue;
6045 }
6046 _ij3[0] = ij3; _ij3[1] = -1;
6047 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6048 {
6049 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6050 {
6051  j3valid[iij3]=false; _ij3[1] = iij3; break;
6052 }
6053 }
6054 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6055 {
6056 IkReal evalcond[8];
6057 IkReal x403=IKcos(j3);
6058 IkReal x404=IKsin(j3);
6059 IkReal x405=((1.0)*cj5);
6060 IkReal x406=(sj5*x404);
6061 IkReal x407=(sj5*x403);
6062 IkReal x408=((1.0)*x404);
6063 IkReal x409=(x403*x405);
6064 evalcond[0]=(sj5+((new_r11*x404))+((new_r01*x403)));
6065 evalcond[1]=(((cj5*x404))+x407+new_r01);
6066 evalcond[2]=(x406+new_r00+(((-1.0)*x409)));
6067 evalcond[3]=(x406+new_r11+(((-1.0)*x409)));
6068 evalcond[4]=(((new_r10*x404))+(((-1.0)*x405))+((new_r00*x403)));
6069 evalcond[5]=((((-1.0)*x404*x405))+new_r10+(((-1.0)*x407)));
6070 evalcond[6]=((((-1.0)*sj5))+((new_r10*x403))+(((-1.0)*new_r00*x408)));
6071 evalcond[7]=(((new_r11*x403))+(((-1.0)*new_r01*x408))+(((-1.0)*x405)));
6072 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
6073 {
6074 continue;
6075 }
6076 }
6077 
6078 {
6079 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6080 vinfos[0].jointtype = 1;
6081 vinfos[0].foffset = j0;
6082 vinfos[0].indices[0] = _ij0[0];
6083 vinfos[0].indices[1] = _ij0[1];
6084 vinfos[0].maxsolutions = _nj0;
6085 vinfos[1].jointtype = 1;
6086 vinfos[1].foffset = j1;
6087 vinfos[1].indices[0] = _ij1[0];
6088 vinfos[1].indices[1] = _ij1[1];
6089 vinfos[1].maxsolutions = _nj1;
6090 vinfos[2].jointtype = 1;
6091 vinfos[2].foffset = j2;
6092 vinfos[2].indices[0] = _ij2[0];
6093 vinfos[2].indices[1] = _ij2[1];
6094 vinfos[2].maxsolutions = _nj2;
6095 vinfos[3].jointtype = 1;
6096 vinfos[3].foffset = j3;
6097 vinfos[3].indices[0] = _ij3[0];
6098 vinfos[3].indices[1] = _ij3[1];
6099 vinfos[3].maxsolutions = _nj3;
6100 vinfos[4].jointtype = 1;
6101 vinfos[4].foffset = j4;
6102 vinfos[4].indices[0] = _ij4[0];
6103 vinfos[4].indices[1] = _ij4[1];
6104 vinfos[4].maxsolutions = _nj4;
6105 vinfos[5].jointtype = 1;
6106 vinfos[5].foffset = j5;
6107 vinfos[5].indices[0] = _ij5[0];
6108 vinfos[5].indices[1] = _ij5[1];
6109 vinfos[5].maxsolutions = _nj5;
6110 std::vector<int> vfree(0);
6111 solutions.AddSolution(vinfos,vfree);
6112 }
6113 }
6114 }
6115 
6116 }
6117 } while(0);
6118 if( bgotonextstatement )
6119 {
6120 bool bgotonextstatement = true;
6121 do
6122 {
6123 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
6124 evalcond[1]=new_r20;
6125 evalcond[2]=new_r02;
6126 evalcond[3]=new_r12;
6127 evalcond[4]=new_r21;
6128 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 )
6129 {
6130 bgotonextstatement=false;
6131 {
6132 IkReal j3array[1], cj3array[1], sj3array[1];
6133 bool j3valid[1]={false};
6134 _nj3 = 1;
6135 IkReal x410=((1.0)*cj5);
6136 if( IKabs(((((-1.0)*new_r00*sj5))+(((-1.0)*new_r01*x410)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj5))+(((-1.0)*new_r00*x410)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*sj5))+(((-1.0)*new_r01*x410))))+IKsqr((((new_r01*sj5))+(((-1.0)*new_r00*x410))))-1) <= IKFAST_SINCOS_THRESH )
6137  continue;
6138 j3array[0]=IKatan2(((((-1.0)*new_r00*sj5))+(((-1.0)*new_r01*x410))), (((new_r01*sj5))+(((-1.0)*new_r00*x410))));
6139 sj3array[0]=IKsin(j3array[0]);
6140 cj3array[0]=IKcos(j3array[0]);
6141 if( j3array[0] > IKPI )
6142 {
6143  j3array[0]-=IK2PI;
6144 }
6145 else if( j3array[0] < -IKPI )
6146 { j3array[0]+=IK2PI;
6147 }
6148 j3valid[0] = true;
6149 for(int ij3 = 0; ij3 < 1; ++ij3)
6150 {
6151 if( !j3valid[ij3] )
6152 {
6153  continue;
6154 }
6155 _ij3[0] = ij3; _ij3[1] = -1;
6156 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6157 {
6158 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6159 {
6160  j3valid[iij3]=false; _ij3[1] = iij3; break;
6161 }
6162 }
6163 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6164 {
6165 IkReal evalcond[8];
6166 IkReal x411=IKsin(j3);
6167 IkReal x412=IKcos(j3);
6168 IkReal x413=((1.0)*sj5);
6169 IkReal x414=((1.0)*cj5);
6170 IkReal x415=(cj5*x411);
6171 IkReal x416=((1.0)*x411);
6172 IkReal x417=(x412*x413);
6173 evalcond[0]=(cj5+((new_r10*x411))+((new_r00*x412)));
6174 evalcond[1]=(((sj5*x411))+((cj5*x412))+new_r00);
6175 evalcond[2]=(x415+new_r01+(((-1.0)*x417)));
6176 evalcond[3]=(x415+new_r10+(((-1.0)*x417)));
6177 evalcond[4]=(((new_r11*x411))+(((-1.0)*x413))+((new_r01*x412)));
6178 evalcond[5]=((((-1.0)*x412*x414))+(((-1.0)*x411*x413))+new_r11);
6179 evalcond[6]=(((new_r10*x412))+(((-1.0)*new_r00*x416))+(((-1.0)*x413)));
6180 evalcond[7]=(((new_r11*x412))+(((-1.0)*x414))+(((-1.0)*new_r01*x416)));
6181 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
6182 {
6183 continue;
6184 }
6185 }
6186 
6187 {
6188 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6189 vinfos[0].jointtype = 1;
6190 vinfos[0].foffset = j0;
6191 vinfos[0].indices[0] = _ij0[0];
6192 vinfos[0].indices[1] = _ij0[1];
6193 vinfos[0].maxsolutions = _nj0;
6194 vinfos[1].jointtype = 1;
6195 vinfos[1].foffset = j1;
6196 vinfos[1].indices[0] = _ij1[0];
6197 vinfos[1].indices[1] = _ij1[1];
6198 vinfos[1].maxsolutions = _nj1;
6199 vinfos[2].jointtype = 1;
6200 vinfos[2].foffset = j2;
6201 vinfos[2].indices[0] = _ij2[0];
6202 vinfos[2].indices[1] = _ij2[1];
6203 vinfos[2].maxsolutions = _nj2;
6204 vinfos[3].jointtype = 1;
6205 vinfos[3].foffset = j3;
6206 vinfos[3].indices[0] = _ij3[0];
6207 vinfos[3].indices[1] = _ij3[1];
6208 vinfos[3].maxsolutions = _nj3;
6209 vinfos[4].jointtype = 1;
6210 vinfos[4].foffset = j4;
6211 vinfos[4].indices[0] = _ij4[0];
6212 vinfos[4].indices[1] = _ij4[1];
6213 vinfos[4].maxsolutions = _nj4;
6214 vinfos[5].jointtype = 1;
6215 vinfos[5].foffset = j5;
6216 vinfos[5].indices[0] = _ij5[0];
6217 vinfos[5].indices[1] = _ij5[1];
6218 vinfos[5].maxsolutions = _nj5;
6219 std::vector<int> vfree(0);
6220 solutions.AddSolution(vinfos,vfree);
6221 }
6222 }
6223 }
6224 
6225 }
6226 } while(0);
6227 if( bgotonextstatement )
6228 {
6229 bool bgotonextstatement = true;
6230 do
6231 {
6232 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
6233 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6234 {
6235 bgotonextstatement=false;
6236 {
6237 IkReal j3eval[1];
6238 new_r02=0;
6239 new_r12=0;
6240 new_r20=0;
6241 new_r21=0;
6242 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
6243 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6244 {
6245 {
6246 IkReal j3eval[1];
6247 new_r02=0;
6248 new_r12=0;
6249 new_r20=0;
6250 new_r21=0;
6251 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6252 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6253 {
6254 {
6255 IkReal j3eval[1];
6256 new_r02=0;
6257 new_r12=0;
6258 new_r20=0;
6259 new_r21=0;
6260 j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
6261 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6262 {
6263 continue; // no branches [j3]
6264 
6265 } else
6266 {
6267 {
6268 IkReal j3array[2], cj3array[2], sj3array[2];
6269 bool j3valid[2]={false};
6270 _nj3 = 2;
6271 IkReal x418=((-1.0)*new_r22);
6272 CheckValue<IkReal> x420 = IKatan2WithCheck(IkReal((new_r00*x418)),IkReal((new_r10*x418)),IKFAST_ATAN2_MAGTHRESH);
6273 if(!x420.valid){
6274 continue;
6275 }
6276 IkReal x419=x420.value;
6277 j3array[0]=((-1.0)*x419);
6278 sj3array[0]=IKsin(j3array[0]);
6279 cj3array[0]=IKcos(j3array[0]);
6280 j3array[1]=((3.14159265358979)+(((-1.0)*x419)));
6281 sj3array[1]=IKsin(j3array[1]);
6282 cj3array[1]=IKcos(j3array[1]);
6283 if( j3array[0] > IKPI )
6284 {
6285  j3array[0]-=IK2PI;
6286 }
6287 else if( j3array[0] < -IKPI )
6288 { j3array[0]+=IK2PI;
6289 }
6290 j3valid[0] = true;
6291 if( j3array[1] > IKPI )
6292 {
6293  j3array[1]-=IK2PI;
6294 }
6295 else if( j3array[1] < -IKPI )
6296 { j3array[1]+=IK2PI;
6297 }
6298 j3valid[1] = true;
6299 for(int ij3 = 0; ij3 < 2; ++ij3)
6300 {
6301 if( !j3valid[ij3] )
6302 {
6303  continue;
6304 }
6305 _ij3[0] = ij3; _ij3[1] = -1;
6306 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6307 {
6308 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6309 {
6310  j3valid[iij3]=false; _ij3[1] = iij3; break;
6311 }
6312 }
6313 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6314 {
6315 IkReal evalcond[5];
6316 IkReal x421=IKsin(j3);
6317 IkReal x422=IKcos(j3);
6318 IkReal x423=((1.0)*new_r22);
6319 IkReal x424=(new_r11*x421);
6320 IkReal x425=(new_r01*x422);
6321 IkReal x426=((1.0)*x421);
6322 evalcond[0]=(x425+x424);
6323 evalcond[1]=(((new_r10*x421))+((new_r00*x422)));
6324 evalcond[2]=((((-1.0)*new_r00*x426))+((new_r10*x422)));
6325 evalcond[3]=(((new_r11*x422))+(((-1.0)*new_r01*x426)));
6326 evalcond[4]=((((-1.0)*x423*x424))+(((-1.0)*x423*x425)));
6327 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
6328 {
6329 continue;
6330 }
6331 }
6332 
6333 {
6334 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6335 vinfos[0].jointtype = 1;
6336 vinfos[0].foffset = j0;
6337 vinfos[0].indices[0] = _ij0[0];
6338 vinfos[0].indices[1] = _ij0[1];
6339 vinfos[0].maxsolutions = _nj0;
6340 vinfos[1].jointtype = 1;
6341 vinfos[1].foffset = j1;
6342 vinfos[1].indices[0] = _ij1[0];
6343 vinfos[1].indices[1] = _ij1[1];
6344 vinfos[1].maxsolutions = _nj1;
6345 vinfos[2].jointtype = 1;
6346 vinfos[2].foffset = j2;
6347 vinfos[2].indices[0] = _ij2[0];
6348 vinfos[2].indices[1] = _ij2[1];
6349 vinfos[2].maxsolutions = _nj2;
6350 vinfos[3].jointtype = 1;
6351 vinfos[3].foffset = j3;
6352 vinfos[3].indices[0] = _ij3[0];
6353 vinfos[3].indices[1] = _ij3[1];
6354 vinfos[3].maxsolutions = _nj3;
6355 vinfos[4].jointtype = 1;
6356 vinfos[4].foffset = j4;
6357 vinfos[4].indices[0] = _ij4[0];
6358 vinfos[4].indices[1] = _ij4[1];
6359 vinfos[4].maxsolutions = _nj4;
6360 vinfos[5].jointtype = 1;
6361 vinfos[5].foffset = j5;
6362 vinfos[5].indices[0] = _ij5[0];
6363 vinfos[5].indices[1] = _ij5[1];
6364 vinfos[5].maxsolutions = _nj5;
6365 std::vector<int> vfree(0);
6366 solutions.AddSolution(vinfos,vfree);
6367 }
6368 }
6369 }
6370 
6371 }
6372 
6373 }
6374 
6375 } else
6376 {
6377 {
6378 IkReal j3array[2], cj3array[2], sj3array[2];
6379 bool j3valid[2]={false};
6380 _nj3 = 2;
6381 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
6382 if(!x428.valid){
6383 continue;
6384 }
6385 IkReal x427=x428.value;
6386 j3array[0]=((-1.0)*x427);
6387 sj3array[0]=IKsin(j3array[0]);
6388 cj3array[0]=IKcos(j3array[0]);
6389 j3array[1]=((3.14159265358979)+(((-1.0)*x427)));
6390 sj3array[1]=IKsin(j3array[1]);
6391 cj3array[1]=IKcos(j3array[1]);
6392 if( j3array[0] > IKPI )
6393 {
6394  j3array[0]-=IK2PI;
6395 }
6396 else if( j3array[0] < -IKPI )
6397 { j3array[0]+=IK2PI;
6398 }
6399 j3valid[0] = true;
6400 if( j3array[1] > IKPI )
6401 {
6402  j3array[1]-=IK2PI;
6403 }
6404 else if( j3array[1] < -IKPI )
6405 { j3array[1]+=IK2PI;
6406 }
6407 j3valid[1] = true;
6408 for(int ij3 = 0; ij3 < 2; ++ij3)
6409 {
6410 if( !j3valid[ij3] )
6411 {
6412  continue;
6413 }
6414 _ij3[0] = ij3; _ij3[1] = -1;
6415 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6416 {
6417 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6418 {
6419  j3valid[iij3]=false; _ij3[1] = iij3; break;
6420 }
6421 }
6422 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6423 {
6424 IkReal evalcond[5];
6425 IkReal x429=IKcos(j3);
6426 IkReal x430=IKsin(j3);
6427 IkReal x431=((1.0)*x430);
6428 IkReal x432=(new_r22*x431);
6429 IkReal x433=((1.0)*new_r22*x429);
6430 evalcond[0]=(((new_r11*x430))+((new_r01*x429)));
6431 evalcond[1]=((((-1.0)*new_r00*x431))+((new_r10*x429)));
6432 evalcond[2]=((((-1.0)*new_r01*x431))+((new_r11*x429)));
6433 evalcond[3]=((((-1.0)*new_r00*x433))+(((-1.0)*new_r10*x432)));
6434 evalcond[4]=((((-1.0)*new_r01*x433))+(((-1.0)*new_r11*x432)));
6435 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
6436 {
6437 continue;
6438 }
6439 }
6440 
6441 {
6442 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6443 vinfos[0].jointtype = 1;
6444 vinfos[0].foffset = j0;
6445 vinfos[0].indices[0] = _ij0[0];
6446 vinfos[0].indices[1] = _ij0[1];
6447 vinfos[0].maxsolutions = _nj0;
6448 vinfos[1].jointtype = 1;
6449 vinfos[1].foffset = j1;
6450 vinfos[1].indices[0] = _ij1[0];
6451 vinfos[1].indices[1] = _ij1[1];
6452 vinfos[1].maxsolutions = _nj1;
6453 vinfos[2].jointtype = 1;
6454 vinfos[2].foffset = j2;
6455 vinfos[2].indices[0] = _ij2[0];
6456 vinfos[2].indices[1] = _ij2[1];
6457 vinfos[2].maxsolutions = _nj2;
6458 vinfos[3].jointtype = 1;
6459 vinfos[3].foffset = j3;
6460 vinfos[3].indices[0] = _ij3[0];
6461 vinfos[3].indices[1] = _ij3[1];
6462 vinfos[3].maxsolutions = _nj3;
6463 vinfos[4].jointtype = 1;
6464 vinfos[4].foffset = j4;
6465 vinfos[4].indices[0] = _ij4[0];
6466 vinfos[4].indices[1] = _ij4[1];
6467 vinfos[4].maxsolutions = _nj4;
6468 vinfos[5].jointtype = 1;
6469 vinfos[5].foffset = j5;
6470 vinfos[5].indices[0] = _ij5[0];
6471 vinfos[5].indices[1] = _ij5[1];
6472 vinfos[5].maxsolutions = _nj5;
6473 std::vector<int> vfree(0);
6474 solutions.AddSolution(vinfos,vfree);
6475 }
6476 }
6477 }
6478 
6479 }
6480 
6481 }
6482 
6483 } else
6484 {
6485 {
6486 IkReal j3array[2], cj3array[2], sj3array[2];
6487 bool j3valid[2]={false};
6488 _nj3 = 2;
6489 CheckValue<IkReal> x435 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
6490 if(!x435.valid){
6491 continue;
6492 }
6493 IkReal x434=x435.value;
6494 j3array[0]=((-1.0)*x434);
6495 sj3array[0]=IKsin(j3array[0]);
6496 cj3array[0]=IKcos(j3array[0]);
6497 j3array[1]=((3.14159265358979)+(((-1.0)*x434)));
6498 sj3array[1]=IKsin(j3array[1]);
6499 cj3array[1]=IKcos(j3array[1]);
6500 if( j3array[0] > IKPI )
6501 {
6502  j3array[0]-=IK2PI;
6503 }
6504 else if( j3array[0] < -IKPI )
6505 { j3array[0]+=IK2PI;
6506 }
6507 j3valid[0] = true;
6508 if( j3array[1] > IKPI )
6509 {
6510  j3array[1]-=IK2PI;
6511 }
6512 else if( j3array[1] < -IKPI )
6513 { j3array[1]+=IK2PI;
6514 }
6515 j3valid[1] = true;
6516 for(int ij3 = 0; ij3 < 2; ++ij3)
6517 {
6518 if( !j3valid[ij3] )
6519 {
6520  continue;
6521 }
6522 _ij3[0] = ij3; _ij3[1] = -1;
6523 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6524 {
6525 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6526 {
6527  j3valid[iij3]=false; _ij3[1] = iij3; break;
6528 }
6529 }
6530 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6531 {
6532 IkReal evalcond[5];
6533 IkReal x436=IKcos(j3);
6534 IkReal x437=IKsin(j3);
6535 IkReal x438=((1.0)*new_r22);
6536 IkReal x439=(new_r10*x437);
6537 IkReal x440=(new_r00*x436);
6538 IkReal x441=((1.0)*x437);
6539 evalcond[0]=(x439+x440);
6540 evalcond[1]=((((-1.0)*new_r00*x441))+((new_r10*x436)));
6541 evalcond[2]=((((-1.0)*new_r01*x441))+((new_r11*x436)));
6542 evalcond[3]=((((-1.0)*x438*x440))+(((-1.0)*x438*x439)));
6543 evalcond[4]=((((-1.0)*new_r01*x436*x438))+(((-1.0)*new_r11*x437*x438)));
6544 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
6545 {
6546 continue;
6547 }
6548 }
6549 
6550 {
6551 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6552 vinfos[0].jointtype = 1;
6553 vinfos[0].foffset = j0;
6554 vinfos[0].indices[0] = _ij0[0];
6555 vinfos[0].indices[1] = _ij0[1];
6556 vinfos[0].maxsolutions = _nj0;
6557 vinfos[1].jointtype = 1;
6558 vinfos[1].foffset = j1;
6559 vinfos[1].indices[0] = _ij1[0];
6560 vinfos[1].indices[1] = _ij1[1];
6561 vinfos[1].maxsolutions = _nj1;
6562 vinfos[2].jointtype = 1;
6563 vinfos[2].foffset = j2;
6564 vinfos[2].indices[0] = _ij2[0];
6565 vinfos[2].indices[1] = _ij2[1];
6566 vinfos[2].maxsolutions = _nj2;
6567 vinfos[3].jointtype = 1;
6568 vinfos[3].foffset = j3;
6569 vinfos[3].indices[0] = _ij3[0];
6570 vinfos[3].indices[1] = _ij3[1];
6571 vinfos[3].maxsolutions = _nj3;
6572 vinfos[4].jointtype = 1;
6573 vinfos[4].foffset = j4;
6574 vinfos[4].indices[0] = _ij4[0];
6575 vinfos[4].indices[1] = _ij4[1];
6576 vinfos[4].maxsolutions = _nj4;
6577 vinfos[5].jointtype = 1;
6578 vinfos[5].foffset = j5;
6579 vinfos[5].indices[0] = _ij5[0];
6580 vinfos[5].indices[1] = _ij5[1];
6581 vinfos[5].maxsolutions = _nj5;
6582 std::vector<int> vfree(0);
6583 solutions.AddSolution(vinfos,vfree);
6584 }
6585 }
6586 }
6587 
6588 }
6589 
6590 }
6591 
6592 }
6593 } while(0);
6594 if( bgotonextstatement )
6595 {
6596 bool bgotonextstatement = true;
6597 do
6598 {
6599 if( 1 )
6600 {
6601 bgotonextstatement=false;
6602 continue; // branch miss [j3]
6603 
6604 }
6605 } while(0);
6606 if( bgotonextstatement )
6607 {
6608 }
6609 }
6610 }
6611 }
6612 }
6613 }
6614 }
6615 
6616 } else
6617 {
6618 {
6619 IkReal j3array[1], cj3array[1], sj3array[1];
6620 bool j3valid[1]={false};
6621 _nj3 = 1;
6622 CheckValue<IkReal> x443=IKPowWithIntegerCheck(sj4,-1);
6623 if(!x443.valid){
6624 continue;
6625 }
6626 IkReal x442=x443.value;
6627 CheckValue<IkReal> x444=IKPowWithIntegerCheck(cj5,-1);
6628 if(!x444.valid){
6629 continue;
6630 }
6631 if( IKabs((x442*(x444.value)*(((((-1.0)*cj4*new_r02*sj5))+(((-1.0)*new_r01*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r02*x442)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x442*(x444.value)*(((((-1.0)*cj4*new_r02*sj5))+(((-1.0)*new_r01*sj4))))))+IKsqr((new_r02*x442))-1) <= IKFAST_SINCOS_THRESH )
6632  continue;
6633 j3array[0]=IKatan2((x442*(x444.value)*(((((-1.0)*cj4*new_r02*sj5))+(((-1.0)*new_r01*sj4))))), (new_r02*x442));
6634 sj3array[0]=IKsin(j3array[0]);
6635 cj3array[0]=IKcos(j3array[0]);
6636 if( j3array[0] > IKPI )
6637 {
6638  j3array[0]-=IK2PI;
6639 }
6640 else if( j3array[0] < -IKPI )
6641 { j3array[0]+=IK2PI;
6642 }
6643 j3valid[0] = true;
6644 for(int ij3 = 0; ij3 < 1; ++ij3)
6645 {
6646 if( !j3valid[ij3] )
6647 {
6648  continue;
6649 }
6650 _ij3[0] = ij3; _ij3[1] = -1;
6651 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6652 {
6653 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6654 {
6655  j3valid[iij3]=false; _ij3[1] = iij3; break;
6656 }
6657 }
6658 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6659 {
6660 IkReal evalcond[18];
6661 IkReal x445=IKcos(j3);
6662 IkReal x446=IKsin(j3);
6663 IkReal x447=((1.0)*sj5);
6664 IkReal x448=((1.0)*cj4);
6665 IkReal x449=((1.0)*sj4);
6666 IkReal x450=(cj4*sj5);
6667 IkReal x451=(new_r10*x446);
6668 IkReal x452=(cj5*x445);
6669 IkReal x453=(cj5*x446);
6670 IkReal x454=(new_r11*x446);
6671 IkReal x455=(new_r01*x445);
6672 IkReal x456=(new_r02*x445);
6673 IkReal x457=(new_r12*x446);
6674 IkReal x458=(new_r00*x445);
6675 IkReal x459=((1.0)*x446);
6676 evalcond[0]=((((-1.0)*x445*x449))+new_r02);
6677 evalcond[1]=(new_r12+(((-1.0)*x446*x449)));
6678 evalcond[2]=(((new_r12*x445))+(((-1.0)*new_r02*x459)));
6679 evalcond[3]=(x453+((x445*x450))+new_r01);
6680 evalcond[4]=((((-1.0)*x449))+x456+x457);
6681 evalcond[5]=(x454+x455+x450);
6682 evalcond[6]=(((sj5*x446))+new_r00+(((-1.0)*x448*x452)));
6683 evalcond[7]=((((-1.0)*x452))+((x446*x450))+new_r11);
6684 evalcond[8]=((((-1.0)*x447))+(((-1.0)*new_r00*x459))+((new_r10*x445)));
6685 evalcond[9]=((((-1.0)*new_r01*x459))+((new_r11*x445))+(((-1.0)*cj5)));
6686 evalcond[10]=((((-1.0)*cj5*x448))+x458+x451);
6687 evalcond[11]=((((-1.0)*x445*x447))+new_r10+(((-1.0)*x448*x453)));
6688 evalcond[12]=(((new_r22*sj4))+(((-1.0)*x448*x456))+(((-1.0)*x448*x457)));
6689 evalcond[13]=((((-1.0)*new_r20*x448))+(((-1.0)*x449*x458))+(((-1.0)*x449*x451)));
6690 evalcond[14]=((((-1.0)*new_r21*x448))+(((-1.0)*x449*x454))+(((-1.0)*x449*x455)));
6691 evalcond[15]=(((new_r20*sj4))+cj5+(((-1.0)*x448*x451))+(((-1.0)*x448*x458)));
6692 evalcond[16]=((1.0)+(((-1.0)*x449*x456))+(((-1.0)*x449*x457))+(((-1.0)*new_r22*x448)));
6693 evalcond[17]=((((-1.0)*x447))+((new_r21*sj4))+(((-1.0)*x448*x454))+(((-1.0)*x448*x455)));
6694 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
6695 {
6696 continue;
6697 }
6698 }
6699 
6700 {
6701 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6702 vinfos[0].jointtype = 1;
6703 vinfos[0].foffset = j0;
6704 vinfos[0].indices[0] = _ij0[0];
6705 vinfos[0].indices[1] = _ij0[1];
6706 vinfos[0].maxsolutions = _nj0;
6707 vinfos[1].jointtype = 1;
6708 vinfos[1].foffset = j1;
6709 vinfos[1].indices[0] = _ij1[0];
6710 vinfos[1].indices[1] = _ij1[1];
6711 vinfos[1].maxsolutions = _nj1;
6712 vinfos[2].jointtype = 1;
6713 vinfos[2].foffset = j2;
6714 vinfos[2].indices[0] = _ij2[0];
6715 vinfos[2].indices[1] = _ij2[1];
6716 vinfos[2].maxsolutions = _nj2;
6717 vinfos[3].jointtype = 1;
6718 vinfos[3].foffset = j3;
6719 vinfos[3].indices[0] = _ij3[0];
6720 vinfos[3].indices[1] = _ij3[1];
6721 vinfos[3].maxsolutions = _nj3;
6722 vinfos[4].jointtype = 1;
6723 vinfos[4].foffset = j4;
6724 vinfos[4].indices[0] = _ij4[0];
6725 vinfos[4].indices[1] = _ij4[1];
6726 vinfos[4].maxsolutions = _nj4;
6727 vinfos[5].jointtype = 1;
6728 vinfos[5].foffset = j5;
6729 vinfos[5].indices[0] = _ij5[0];
6730 vinfos[5].indices[1] = _ij5[1];
6731 vinfos[5].maxsolutions = _nj5;
6732 std::vector<int> vfree(0);
6733 solutions.AddSolution(vinfos,vfree);
6734 }
6735 }
6736 }
6737 
6738 }
6739 
6740 }
6741 
6742 } else
6743 {
6744 {
6745 IkReal j3array[1], cj3array[1], sj3array[1];
6746 bool j3valid[1]={false};
6747 _nj3 = 1;
6748 CheckValue<IkReal> x460=IKPowWithIntegerCheck(IKsign(sj4),-1);
6749 if(!x460.valid){
6750 continue;
6751 }
6752 CheckValue<IkReal> x461 = IKatan2WithCheck(IkReal(new_r12),IkReal(new_r02),IKFAST_ATAN2_MAGTHRESH);
6753 if(!x461.valid){
6754 continue;
6755 }
6756 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x460.value)))+(x461.value));
6757 sj3array[0]=IKsin(j3array[0]);
6758 cj3array[0]=IKcos(j3array[0]);
6759 if( j3array[0] > IKPI )
6760 {
6761  j3array[0]-=IK2PI;
6762 }
6763 else if( j3array[0] < -IKPI )
6764 { j3array[0]+=IK2PI;
6765 }
6766 j3valid[0] = true;
6767 for(int ij3 = 0; ij3 < 1; ++ij3)
6768 {
6769 if( !j3valid[ij3] )
6770 {
6771  continue;
6772 }
6773 _ij3[0] = ij3; _ij3[1] = -1;
6774 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6775 {
6776 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6777 {
6778  j3valid[iij3]=false; _ij3[1] = iij3; break;
6779 }
6780 }
6781 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6782 {
6783 IkReal evalcond[18];
6784 IkReal x462=IKcos(j3);
6785 IkReal x463=IKsin(j3);
6786 IkReal x464=((1.0)*sj5);
6787 IkReal x465=((1.0)*cj4);
6788 IkReal x466=((1.0)*sj4);
6789 IkReal x467=(cj4*sj5);
6790 IkReal x468=(new_r10*x463);
6791 IkReal x469=(cj5*x462);
6792 IkReal x470=(cj5*x463);
6793 IkReal x471=(new_r11*x463);
6794 IkReal x472=(new_r01*x462);
6795 IkReal x473=(new_r02*x462);
6796 IkReal x474=(new_r12*x463);
6797 IkReal x475=(new_r00*x462);
6798 IkReal x476=((1.0)*x463);
6799 evalcond[0]=((((-1.0)*x462*x466))+new_r02);
6800 evalcond[1]=((((-1.0)*x463*x466))+new_r12);
6801 evalcond[2]=(((new_r12*x462))+(((-1.0)*new_r02*x476)));
6802 evalcond[3]=(x470+new_r01+((x462*x467)));
6803 evalcond[4]=((((-1.0)*x466))+x474+x473);
6804 evalcond[5]=(x467+x471+x472);
6805 evalcond[6]=((((-1.0)*x465*x469))+new_r00+((sj5*x463)));
6806 evalcond[7]=(((x463*x467))+(((-1.0)*x469))+new_r11);
6807 evalcond[8]=(((new_r10*x462))+(((-1.0)*new_r00*x476))+(((-1.0)*x464)));
6808 evalcond[9]=(((new_r11*x462))+(((-1.0)*new_r01*x476))+(((-1.0)*cj5)));
6809 evalcond[10]=((((-1.0)*cj5*x465))+x468+x475);
6810 evalcond[11]=((((-1.0)*x462*x464))+new_r10+(((-1.0)*x465*x470)));
6811 evalcond[12]=(((new_r22*sj4))+(((-1.0)*x465*x474))+(((-1.0)*x465*x473)));
6812 evalcond[13]=((((-1.0)*x466*x468))+(((-1.0)*new_r20*x465))+(((-1.0)*x466*x475)));
6813 evalcond[14]=((((-1.0)*new_r21*x465))+(((-1.0)*x466*x472))+(((-1.0)*x466*x471)));
6814 evalcond[15]=((((-1.0)*x465*x468))+((new_r20*sj4))+cj5+(((-1.0)*x465*x475)));
6815 evalcond[16]=((1.0)+(((-1.0)*new_r22*x465))+(((-1.0)*x466*x473))+(((-1.0)*x466*x474)));
6816 evalcond[17]=((((-1.0)*x464))+((new_r21*sj4))+(((-1.0)*x465*x471))+(((-1.0)*x465*x472)));
6817 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
6818 {
6819 continue;
6820 }
6821 }
6822 
6823 {
6824 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6825 vinfos[0].jointtype = 1;
6826 vinfos[0].foffset = j0;
6827 vinfos[0].indices[0] = _ij0[0];
6828 vinfos[0].indices[1] = _ij0[1];
6829 vinfos[0].maxsolutions = _nj0;
6830 vinfos[1].jointtype = 1;
6831 vinfos[1].foffset = j1;
6832 vinfos[1].indices[0] = _ij1[0];
6833 vinfos[1].indices[1] = _ij1[1];
6834 vinfos[1].maxsolutions = _nj1;
6835 vinfos[2].jointtype = 1;
6836 vinfos[2].foffset = j2;
6837 vinfos[2].indices[0] = _ij2[0];
6838 vinfos[2].indices[1] = _ij2[1];
6839 vinfos[2].maxsolutions = _nj2;
6840 vinfos[3].jointtype = 1;
6841 vinfos[3].foffset = j3;
6842 vinfos[3].indices[0] = _ij3[0];
6843 vinfos[3].indices[1] = _ij3[1];
6844 vinfos[3].maxsolutions = _nj3;
6845 vinfos[4].jointtype = 1;
6846 vinfos[4].foffset = j4;
6847 vinfos[4].indices[0] = _ij4[0];
6848 vinfos[4].indices[1] = _ij4[1];
6849 vinfos[4].maxsolutions = _nj4;
6850 vinfos[5].jointtype = 1;
6851 vinfos[5].foffset = j5;
6852 vinfos[5].indices[0] = _ij5[0];
6853 vinfos[5].indices[1] = _ij5[1];
6854 vinfos[5].maxsolutions = _nj5;
6855 std::vector<int> vfree(0);
6856 solutions.AddSolution(vinfos,vfree);
6857 }
6858 }
6859 }
6860 
6861 }
6862 
6863 }
6864 }
6865 }
6866 
6867 }
6868 
6869 }
6870 
6871 } else
6872 {
6873 {
6874 IkReal j3array[1], cj3array[1], sj3array[1];
6875 bool j3valid[1]={false};
6876 _nj3 = 1;
6877 CheckValue<IkReal> x477=IKPowWithIntegerCheck(IKsign(sj4),-1);
6878 if(!x477.valid){
6879 continue;
6880 }
6881 CheckValue<IkReal> x478 = IKatan2WithCheck(IkReal(new_r12),IkReal(new_r02),IKFAST_ATAN2_MAGTHRESH);
6882 if(!x478.valid){
6883 continue;
6884 }
6885 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x477.value)))+(x478.value));
6886 sj3array[0]=IKsin(j3array[0]);
6887 cj3array[0]=IKcos(j3array[0]);
6888 if( j3array[0] > IKPI )
6889 {
6890  j3array[0]-=IK2PI;
6891 }
6892 else if( j3array[0] < -IKPI )
6893 { j3array[0]+=IK2PI;
6894 }
6895 j3valid[0] = true;
6896 for(int ij3 = 0; ij3 < 1; ++ij3)
6897 {
6898 if( !j3valid[ij3] )
6899 {
6900  continue;
6901 }
6902 _ij3[0] = ij3; _ij3[1] = -1;
6903 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6904 {
6905 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6906 {
6907  j3valid[iij3]=false; _ij3[1] = iij3; break;
6908 }
6909 }
6910 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6911 {
6912 IkReal evalcond[8];
6913 IkReal x479=IKcos(j3);
6914 IkReal x480=IKsin(j3);
6915 IkReal x481=((1.0)*sj4);
6916 IkReal x482=((1.0)*cj4);
6917 IkReal x483=(new_r12*x480);
6918 IkReal x484=(new_r02*x479);
6919 evalcond[0]=((((-1.0)*x479*x481))+new_r02);
6920 evalcond[1]=((((-1.0)*x480*x481))+new_r12);
6921 evalcond[2]=(((new_r12*x479))+(((-1.0)*new_r02*x480)));
6922 evalcond[3]=((((-1.0)*x481))+x483+x484);
6923 evalcond[4]=(((new_r22*sj4))+(((-1.0)*x482*x484))+(((-1.0)*x482*x483)));
6924 evalcond[5]=((((-1.0)*new_r10*x480*x481))+(((-1.0)*new_r20*x482))+(((-1.0)*new_r00*x479*x481)));
6925 evalcond[6]=((((-1.0)*new_r21*x482))+(((-1.0)*new_r11*x480*x481))+(((-1.0)*new_r01*x479*x481)));
6926 evalcond[7]=((1.0)+(((-1.0)*x481*x483))+(((-1.0)*x481*x484))+(((-1.0)*new_r22*x482)));
6927 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
6928 {
6929 continue;
6930 }
6931 }
6932 
6933 {
6934 IkReal j5eval[3];
6935 j5eval[0]=sj4;
6936 j5eval[1]=IKsign(sj4);
6937 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
6938 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
6939 {
6940 {
6941 IkReal j5eval[2];
6942 j5eval[0]=sj3;
6943 j5eval[1]=sj4;
6944 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
6945 {
6946 {
6947 IkReal j5eval[3];
6948 j5eval[0]=cj3;
6949 j5eval[1]=cj4;
6950 j5eval[2]=sj4;
6951 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
6952 {
6953 {
6954 IkReal evalcond[5];
6955 bool bgotonextstatement = true;
6956 do
6957 {
6958 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
6959 evalcond[1]=new_r02;
6960 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
6961 {
6962 bgotonextstatement=false;
6963 {
6964 IkReal j5eval[3];
6965 sj3=1.0;
6966 cj3=0;
6967 j3=1.5707963267949;
6968 j5eval[0]=sj4;
6969 j5eval[1]=IKsign(sj4);
6970 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
6971 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
6972 {
6973 {
6974 IkReal j5eval[3];
6975 sj3=1.0;
6976 cj3=0;
6977 j3=1.5707963267949;
6978 j5eval[0]=cj4;
6979 j5eval[1]=IKsign(cj4);
6980 j5eval[2]=((IKabs(new_r11))+(IKabs(new_r10)));
6981 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
6982 {
6983 {
6984 IkReal j5eval[1];
6985 sj3=1.0;
6986 cj3=0;
6987 j3=1.5707963267949;
6988 j5eval[0]=sj4;
6989 if( IKabs(j5eval[0]) < 0.0000010000000000 )
6990 {
6991 {
6992 IkReal evalcond[4];
6993 bool bgotonextstatement = true;
6994 do
6995 {
6996 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
6997 evalcond[1]=new_r20;
6998 evalcond[2]=new_r12;
6999 evalcond[3]=new_r21;
7000 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
7001 {
7002 bgotonextstatement=false;
7003 {
7004 IkReal j5array[1], cj5array[1], sj5array[1];
7005 bool j5valid[1]={false};
7006 _nj5 = 1;
7007 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
7008  continue;
7009 j5array[0]=IKatan2(((-1.0)*new_r11), new_r10);
7010 sj5array[0]=IKsin(j5array[0]);
7011 cj5array[0]=IKcos(j5array[0]);
7012 if( j5array[0] > IKPI )
7013 {
7014  j5array[0]-=IK2PI;
7015 }
7016 else if( j5array[0] < -IKPI )
7017 { j5array[0]+=IK2PI;
7018 }
7019 j5valid[0] = true;
7020 for(int ij5 = 0; ij5 < 1; ++ij5)
7021 {
7022 if( !j5valid[ij5] )
7023 {
7024  continue;
7025 }
7026 _ij5[0] = ij5; _ij5[1] = -1;
7027 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7028 {
7029 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7030 {
7031  j5valid[iij5]=false; _ij5[1] = iij5; break;
7032 }
7033 }
7034 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7035 {
7036 IkReal evalcond[4];
7037 IkReal x485=IKsin(j5);
7038 IkReal x486=((1.0)*(IKcos(j5)));
7039 evalcond[0]=(x485+new_r11);
7040 evalcond[1]=((((-1.0)*x486))+new_r10);
7041 evalcond[2]=((((-1.0)*x485))+(((-1.0)*new_r00)));
7042 evalcond[3]=((((-1.0)*x486))+(((-1.0)*new_r01)));
7043 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
7044 {
7045 continue;
7046 }
7047 }
7048 
7049 {
7050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7051 vinfos[0].jointtype = 1;
7052 vinfos[0].foffset = j0;
7053 vinfos[0].indices[0] = _ij0[0];
7054 vinfos[0].indices[1] = _ij0[1];
7055 vinfos[0].maxsolutions = _nj0;
7056 vinfos[1].jointtype = 1;
7057 vinfos[1].foffset = j1;
7058 vinfos[1].indices[0] = _ij1[0];
7059 vinfos[1].indices[1] = _ij1[1];
7060 vinfos[1].maxsolutions = _nj1;
7061 vinfos[2].jointtype = 1;
7062 vinfos[2].foffset = j2;
7063 vinfos[2].indices[0] = _ij2[0];
7064 vinfos[2].indices[1] = _ij2[1];
7065 vinfos[2].maxsolutions = _nj2;
7066 vinfos[3].jointtype = 1;
7067 vinfos[3].foffset = j3;
7068 vinfos[3].indices[0] = _ij3[0];
7069 vinfos[3].indices[1] = _ij3[1];
7070 vinfos[3].maxsolutions = _nj3;
7071 vinfos[4].jointtype = 1;
7072 vinfos[4].foffset = j4;
7073 vinfos[4].indices[0] = _ij4[0];
7074 vinfos[4].indices[1] = _ij4[1];
7075 vinfos[4].maxsolutions = _nj4;
7076 vinfos[5].jointtype = 1;
7077 vinfos[5].foffset = j5;
7078 vinfos[5].indices[0] = _ij5[0];
7079 vinfos[5].indices[1] = _ij5[1];
7080 vinfos[5].maxsolutions = _nj5;
7081 std::vector<int> vfree(0);
7082 solutions.AddSolution(vinfos,vfree);
7083 }
7084 }
7085 }
7086 
7087 }
7088 } while(0);
7089 if( bgotonextstatement )
7090 {
7091 bool bgotonextstatement = true;
7092 do
7093 {
7094 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
7095 evalcond[1]=new_r20;
7096 evalcond[2]=new_r12;
7097 evalcond[3]=new_r21;
7098 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
7099 {
7100 bgotonextstatement=false;
7101 {
7102 IkReal j5array[1], cj5array[1], sj5array[1];
7103 bool j5valid[1]={false};
7104 _nj5 = 1;
7105 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
7106  continue;
7107 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
7108 sj5array[0]=IKsin(j5array[0]);
7109 cj5array[0]=IKcos(j5array[0]);
7110 if( j5array[0] > IKPI )
7111 {
7112  j5array[0]-=IK2PI;
7113 }
7114 else if( j5array[0] < -IKPI )
7115 { j5array[0]+=IK2PI;
7116 }
7117 j5valid[0] = true;
7118 for(int ij5 = 0; ij5 < 1; ++ij5)
7119 {
7120 if( !j5valid[ij5] )
7121 {
7122  continue;
7123 }
7124 _ij5[0] = ij5; _ij5[1] = -1;
7125 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7126 {
7127 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7128 {
7129  j5valid[iij5]=false; _ij5[1] = iij5; break;
7130 }
7131 }
7132 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7133 {
7134 IkReal evalcond[4];
7135 IkReal x487=IKcos(j5);
7136 IkReal x488=((1.0)*(IKsin(j5)));
7137 evalcond[0]=(x487+new_r10);
7138 evalcond[1]=((((-1.0)*x488))+new_r11);
7139 evalcond[2]=((((-1.0)*x488))+(((-1.0)*new_r00)));
7140 evalcond[3]=((((-1.0)*x487))+(((-1.0)*new_r01)));
7141 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
7142 {
7143 continue;
7144 }
7145 }
7146 
7147 {
7148 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7149 vinfos[0].jointtype = 1;
7150 vinfos[0].foffset = j0;
7151 vinfos[0].indices[0] = _ij0[0];
7152 vinfos[0].indices[1] = _ij0[1];
7153 vinfos[0].maxsolutions = _nj0;
7154 vinfos[1].jointtype = 1;
7155 vinfos[1].foffset = j1;
7156 vinfos[1].indices[0] = _ij1[0];
7157 vinfos[1].indices[1] = _ij1[1];
7158 vinfos[1].maxsolutions = _nj1;
7159 vinfos[2].jointtype = 1;
7160 vinfos[2].foffset = j2;
7161 vinfos[2].indices[0] = _ij2[0];
7162 vinfos[2].indices[1] = _ij2[1];
7163 vinfos[2].maxsolutions = _nj2;
7164 vinfos[3].jointtype = 1;
7165 vinfos[3].foffset = j3;
7166 vinfos[3].indices[0] = _ij3[0];
7167 vinfos[3].indices[1] = _ij3[1];
7168 vinfos[3].maxsolutions = _nj3;
7169 vinfos[4].jointtype = 1;
7170 vinfos[4].foffset = j4;
7171 vinfos[4].indices[0] = _ij4[0];
7172 vinfos[4].indices[1] = _ij4[1];
7173 vinfos[4].maxsolutions = _nj4;
7174 vinfos[5].jointtype = 1;
7175 vinfos[5].foffset = j5;
7176 vinfos[5].indices[0] = _ij5[0];
7177 vinfos[5].indices[1] = _ij5[1];
7178 vinfos[5].maxsolutions = _nj5;
7179 std::vector<int> vfree(0);
7180 solutions.AddSolution(vinfos,vfree);
7181 }
7182 }
7183 }
7184 
7185 }
7186 } while(0);
7187 if( bgotonextstatement )
7188 {
7189 bool bgotonextstatement = true;
7190 do
7191 {
7192 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
7193 evalcond[1]=new_r22;
7194 evalcond[2]=new_r11;
7195 evalcond[3]=new_r10;
7196 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
7197 {
7198 bgotonextstatement=false;
7199 {
7200 IkReal j5array[1], cj5array[1], sj5array[1];
7201 bool j5valid[1]={false};
7202 _nj5 = 1;
7203 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
7204  continue;
7205 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
7206 sj5array[0]=IKsin(j5array[0]);
7207 cj5array[0]=IKcos(j5array[0]);
7208 if( j5array[0] > IKPI )
7209 {
7210  j5array[0]-=IK2PI;
7211 }
7212 else if( j5array[0] < -IKPI )
7213 { j5array[0]+=IK2PI;
7214 }
7215 j5valid[0] = true;
7216 for(int ij5 = 0; ij5 < 1; ++ij5)
7217 {
7218 if( !j5valid[ij5] )
7219 {
7220  continue;
7221 }
7222 _ij5[0] = ij5; _ij5[1] = -1;
7223 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7224 {
7225 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7226 {
7227  j5valid[iij5]=false; _ij5[1] = iij5; break;
7228 }
7229 }
7230 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7231 {
7232 IkReal evalcond[4];
7233 IkReal x489=IKcos(j5);
7234 IkReal x490=((1.0)*(IKsin(j5)));
7235 evalcond[0]=(x489+new_r20);
7236 evalcond[1]=((((-1.0)*x490))+new_r21);
7237 evalcond[2]=((((-1.0)*x490))+(((-1.0)*new_r00)));
7238 evalcond[3]=((((-1.0)*x489))+(((-1.0)*new_r01)));
7239 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
7240 {
7241 continue;
7242 }
7243 }
7244 
7245 {
7246 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7247 vinfos[0].jointtype = 1;
7248 vinfos[0].foffset = j0;
7249 vinfos[0].indices[0] = _ij0[0];
7250 vinfos[0].indices[1] = _ij0[1];
7251 vinfos[0].maxsolutions = _nj0;
7252 vinfos[1].jointtype = 1;
7253 vinfos[1].foffset = j1;
7254 vinfos[1].indices[0] = _ij1[0];
7255 vinfos[1].indices[1] = _ij1[1];
7256 vinfos[1].maxsolutions = _nj1;
7257 vinfos[2].jointtype = 1;
7258 vinfos[2].foffset = j2;
7259 vinfos[2].indices[0] = _ij2[0];
7260 vinfos[2].indices[1] = _ij2[1];
7261 vinfos[2].maxsolutions = _nj2;
7262 vinfos[3].jointtype = 1;
7263 vinfos[3].foffset = j3;
7264 vinfos[3].indices[0] = _ij3[0];
7265 vinfos[3].indices[1] = _ij3[1];
7266 vinfos[3].maxsolutions = _nj3;
7267 vinfos[4].jointtype = 1;
7268 vinfos[4].foffset = j4;
7269 vinfos[4].indices[0] = _ij4[0];
7270 vinfos[4].indices[1] = _ij4[1];
7271 vinfos[4].maxsolutions = _nj4;
7272 vinfos[5].jointtype = 1;
7273 vinfos[5].foffset = j5;
7274 vinfos[5].indices[0] = _ij5[0];
7275 vinfos[5].indices[1] = _ij5[1];
7276 vinfos[5].maxsolutions = _nj5;
7277 std::vector<int> vfree(0);
7278 solutions.AddSolution(vinfos,vfree);
7279 }
7280 }
7281 }
7282 
7283 }
7284 } while(0);
7285 if( bgotonextstatement )
7286 {
7287 bool bgotonextstatement = true;
7288 do
7289 {
7290 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
7291 evalcond[1]=new_r22;
7292 evalcond[2]=new_r11;
7293 evalcond[3]=new_r10;
7294 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
7295 {
7296 bgotonextstatement=false;
7297 {
7298 IkReal j5array[1], cj5array[1], sj5array[1];
7299 bool j5valid[1]={false};
7300 _nj5 = 1;
7301 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
7302  continue;
7303 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
7304 sj5array[0]=IKsin(j5array[0]);
7305 cj5array[0]=IKcos(j5array[0]);
7306 if( j5array[0] > IKPI )
7307 {
7308  j5array[0]-=IK2PI;
7309 }
7310 else if( j5array[0] < -IKPI )
7311 { j5array[0]+=IK2PI;
7312 }
7313 j5valid[0] = true;
7314 for(int ij5 = 0; ij5 < 1; ++ij5)
7315 {
7316 if( !j5valid[ij5] )
7317 {
7318  continue;
7319 }
7320 _ij5[0] = ij5; _ij5[1] = -1;
7321 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7322 {
7323 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7324 {
7325  j5valid[iij5]=false; _ij5[1] = iij5; break;
7326 }
7327 }
7328 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7329 {
7330 IkReal evalcond[4];
7331 IkReal x491=IKsin(j5);
7332 IkReal x492=((1.0)*(IKcos(j5)));
7333 evalcond[0]=(x491+new_r21);
7334 evalcond[1]=((((-1.0)*x492))+new_r20);
7335 evalcond[2]=((((-1.0)*x491))+(((-1.0)*new_r00)));
7336 evalcond[3]=((((-1.0)*x492))+(((-1.0)*new_r01)));
7337 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
7338 {
7339 continue;
7340 }
7341 }
7342 
7343 {
7344 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7345 vinfos[0].jointtype = 1;
7346 vinfos[0].foffset = j0;
7347 vinfos[0].indices[0] = _ij0[0];
7348 vinfos[0].indices[1] = _ij0[1];
7349 vinfos[0].maxsolutions = _nj0;
7350 vinfos[1].jointtype = 1;
7351 vinfos[1].foffset = j1;
7352 vinfos[1].indices[0] = _ij1[0];
7353 vinfos[1].indices[1] = _ij1[1];
7354 vinfos[1].maxsolutions = _nj1;
7355 vinfos[2].jointtype = 1;
7356 vinfos[2].foffset = j2;
7357 vinfos[2].indices[0] = _ij2[0];
7358 vinfos[2].indices[1] = _ij2[1];
7359 vinfos[2].maxsolutions = _nj2;
7360 vinfos[3].jointtype = 1;
7361 vinfos[3].foffset = j3;
7362 vinfos[3].indices[0] = _ij3[0];
7363 vinfos[3].indices[1] = _ij3[1];
7364 vinfos[3].maxsolutions = _nj3;
7365 vinfos[4].jointtype = 1;
7366 vinfos[4].foffset = j4;
7367 vinfos[4].indices[0] = _ij4[0];
7368 vinfos[4].indices[1] = _ij4[1];
7369 vinfos[4].maxsolutions = _nj4;
7370 vinfos[5].jointtype = 1;
7371 vinfos[5].foffset = j5;
7372 vinfos[5].indices[0] = _ij5[0];
7373 vinfos[5].indices[1] = _ij5[1];
7374 vinfos[5].maxsolutions = _nj5;
7375 std::vector<int> vfree(0);
7376 solutions.AddSolution(vinfos,vfree);
7377 }
7378 }
7379 }
7380 
7381 }
7382 } while(0);
7383 if( bgotonextstatement )
7384 {
7385 bool bgotonextstatement = true;
7386 do
7387 {
7388 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
7389 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7390 {
7391 bgotonextstatement=false;
7392 {
7393 IkReal j5array[1], cj5array[1], sj5array[1];
7394 bool j5valid[1]={false};
7395 _nj5 = 1;
7396 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 )
7397  continue;
7398 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
7399 sj5array[0]=IKsin(j5array[0]);
7400 cj5array[0]=IKcos(j5array[0]);
7401 if( j5array[0] > IKPI )
7402 {
7403  j5array[0]-=IK2PI;
7404 }
7405 else if( j5array[0] < -IKPI )
7406 { j5array[0]+=IK2PI;
7407 }
7408 j5valid[0] = true;
7409 for(int ij5 = 0; ij5 < 1; ++ij5)
7410 {
7411 if( !j5valid[ij5] )
7412 {
7413  continue;
7414 }
7415 _ij5[0] = ij5; _ij5[1] = -1;
7416 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7417 {
7418 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7419 {
7420  j5valid[iij5]=false; _ij5[1] = iij5; break;
7421 }
7422 }
7423 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7424 {
7425 IkReal evalcond[6];
7426 IkReal x493=IKsin(j5);
7427 IkReal x494=IKcos(j5);
7428 evalcond[0]=x494;
7429 evalcond[1]=(new_r22*x493);
7430 evalcond[2]=((-1.0)*x493);
7431 evalcond[3]=((-1.0)*new_r22*x494);
7432 evalcond[4]=((((-1.0)*x493))+(((-1.0)*new_r00)));
7433 evalcond[5]=((((-1.0)*x494))+(((-1.0)*new_r01)));
7434 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7435 {
7436 continue;
7437 }
7438 }
7439 
7440 {
7441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7442 vinfos[0].jointtype = 1;
7443 vinfos[0].foffset = j0;
7444 vinfos[0].indices[0] = _ij0[0];
7445 vinfos[0].indices[1] = _ij0[1];
7446 vinfos[0].maxsolutions = _nj0;
7447 vinfos[1].jointtype = 1;
7448 vinfos[1].foffset = j1;
7449 vinfos[1].indices[0] = _ij1[0];
7450 vinfos[1].indices[1] = _ij1[1];
7451 vinfos[1].maxsolutions = _nj1;
7452 vinfos[2].jointtype = 1;
7453 vinfos[2].foffset = j2;
7454 vinfos[2].indices[0] = _ij2[0];
7455 vinfos[2].indices[1] = _ij2[1];
7456 vinfos[2].maxsolutions = _nj2;
7457 vinfos[3].jointtype = 1;
7458 vinfos[3].foffset = j3;
7459 vinfos[3].indices[0] = _ij3[0];
7460 vinfos[3].indices[1] = _ij3[1];
7461 vinfos[3].maxsolutions = _nj3;
7462 vinfos[4].jointtype = 1;
7463 vinfos[4].foffset = j4;
7464 vinfos[4].indices[0] = _ij4[0];
7465 vinfos[4].indices[1] = _ij4[1];
7466 vinfos[4].maxsolutions = _nj4;
7467 vinfos[5].jointtype = 1;
7468 vinfos[5].foffset = j5;
7469 vinfos[5].indices[0] = _ij5[0];
7470 vinfos[5].indices[1] = _ij5[1];
7471 vinfos[5].maxsolutions = _nj5;
7472 std::vector<int> vfree(0);
7473 solutions.AddSolution(vinfos,vfree);
7474 }
7475 }
7476 }
7477 
7478 }
7479 } while(0);
7480 if( bgotonextstatement )
7481 {
7482 bool bgotonextstatement = true;
7483 do
7484 {
7485 if( 1 )
7486 {
7487 bgotonextstatement=false;
7488 continue; // branch miss [j5]
7489 
7490 }
7491 } while(0);
7492 if( bgotonextstatement )
7493 {
7494 }
7495 }
7496 }
7497 }
7498 }
7499 }
7500 }
7501 
7502 } else
7503 {
7504 {
7505 IkReal j5array[1], cj5array[1], sj5array[1];
7506 bool j5valid[1]={false};
7507 _nj5 = 1;
7508 CheckValue<IkReal> x495=IKPowWithIntegerCheck(sj4,-1);
7509 if(!x495.valid){
7510 continue;
7511 }
7512 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*(x495.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r20*(x495.value)))-1) <= IKFAST_SINCOS_THRESH )
7513  continue;
7514 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x495.value)));
7515 sj5array[0]=IKsin(j5array[0]);
7516 cj5array[0]=IKcos(j5array[0]);
7517 if( j5array[0] > IKPI )
7518 {
7519  j5array[0]-=IK2PI;
7520 }
7521 else if( j5array[0] < -IKPI )
7522 { j5array[0]+=IK2PI;
7523 }
7524 j5valid[0] = true;
7525 for(int ij5 = 0; ij5 < 1; ++ij5)
7526 {
7527 if( !j5valid[ij5] )
7528 {
7529  continue;
7530 }
7531 _ij5[0] = ij5; _ij5[1] = -1;
7532 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7533 {
7534 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7535 {
7536  j5valid[iij5]=false; _ij5[1] = iij5; break;
7537 }
7538 }
7539 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7540 {
7541 IkReal evalcond[8];
7542 IkReal x496=IKsin(j5);
7543 IkReal x497=IKcos(j5);
7544 IkReal x498=((1.0)*cj4);
7545 IkReal x499=((1.0)*x496);
7546 evalcond[0]=(((sj4*x497))+new_r20);
7547 evalcond[1]=(((cj4*x496))+new_r11);
7548 evalcond[2]=((((-1.0)*sj4*x499))+new_r21);
7549 evalcond[3]=((((-1.0)*x497*x498))+new_r10);
7550 evalcond[4]=((((-1.0)*x499))+(((-1.0)*new_r00)));
7551 evalcond[5]=((((-1.0)*x497))+(((-1.0)*new_r01)));
7552 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r10*x498))+x497);
7553 evalcond[7]=((((-1.0)*x499))+(((-1.0)*new_r11*x498))+((new_r21*sj4)));
7554 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
7555 {
7556 continue;
7557 }
7558 }
7559 
7560 {
7561 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7562 vinfos[0].jointtype = 1;
7563 vinfos[0].foffset = j0;
7564 vinfos[0].indices[0] = _ij0[0];
7565 vinfos[0].indices[1] = _ij0[1];
7566 vinfos[0].maxsolutions = _nj0;
7567 vinfos[1].jointtype = 1;
7568 vinfos[1].foffset = j1;
7569 vinfos[1].indices[0] = _ij1[0];
7570 vinfos[1].indices[1] = _ij1[1];
7571 vinfos[1].maxsolutions = _nj1;
7572 vinfos[2].jointtype = 1;
7573 vinfos[2].foffset = j2;
7574 vinfos[2].indices[0] = _ij2[0];
7575 vinfos[2].indices[1] = _ij2[1];
7576 vinfos[2].maxsolutions = _nj2;
7577 vinfos[3].jointtype = 1;
7578 vinfos[3].foffset = j3;
7579 vinfos[3].indices[0] = _ij3[0];
7580 vinfos[3].indices[1] = _ij3[1];
7581 vinfos[3].maxsolutions = _nj3;
7582 vinfos[4].jointtype = 1;
7583 vinfos[4].foffset = j4;
7584 vinfos[4].indices[0] = _ij4[0];
7585 vinfos[4].indices[1] = _ij4[1];
7586 vinfos[4].maxsolutions = _nj4;
7587 vinfos[5].jointtype = 1;
7588 vinfos[5].foffset = j5;
7589 vinfos[5].indices[0] = _ij5[0];
7590 vinfos[5].indices[1] = _ij5[1];
7591 vinfos[5].maxsolutions = _nj5;
7592 std::vector<int> vfree(0);
7593 solutions.AddSolution(vinfos,vfree);
7594 }
7595 }
7596 }
7597 
7598 }
7599 
7600 }
7601 
7602 } else
7603 {
7604 {
7605 IkReal j5array[1], cj5array[1], sj5array[1];
7606 bool j5valid[1]={false};
7607 _nj5 = 1;
7608 CheckValue<IkReal> x500=IKPowWithIntegerCheck(IKsign(cj4),-1);
7609 if(!x500.valid){
7610 continue;
7611 }
7612 CheckValue<IkReal> x501 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
7613 if(!x501.valid){
7614 continue;
7615 }
7616 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x500.value)))+(x501.value));
7617 sj5array[0]=IKsin(j5array[0]);
7618 cj5array[0]=IKcos(j5array[0]);
7619 if( j5array[0] > IKPI )
7620 {
7621  j5array[0]-=IK2PI;
7622 }
7623 else if( j5array[0] < -IKPI )
7624 { j5array[0]+=IK2PI;
7625 }
7626 j5valid[0] = true;
7627 for(int ij5 = 0; ij5 < 1; ++ij5)
7628 {
7629 if( !j5valid[ij5] )
7630 {
7631  continue;
7632 }
7633 _ij5[0] = ij5; _ij5[1] = -1;
7634 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7635 {
7636 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7637 {
7638  j5valid[iij5]=false; _ij5[1] = iij5; break;
7639 }
7640 }
7641 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7642 {
7643 IkReal evalcond[8];
7644 IkReal x502=IKsin(j5);
7645 IkReal x503=IKcos(j5);
7646 IkReal x504=((1.0)*cj4);
7647 IkReal x505=((1.0)*x502);
7648 evalcond[0]=(((sj4*x503))+new_r20);
7649 evalcond[1]=(((cj4*x502))+new_r11);
7650 evalcond[2]=((((-1.0)*sj4*x505))+new_r21);
7651 evalcond[3]=((((-1.0)*x503*x504))+new_r10);
7652 evalcond[4]=((((-1.0)*x505))+(((-1.0)*new_r00)));
7653 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x503)));
7654 evalcond[6]=(((new_r20*sj4))+x503+(((-1.0)*new_r10*x504)));
7655 evalcond[7]=((((-1.0)*x505))+(((-1.0)*new_r11*x504))+((new_r21*sj4)));
7656 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
7657 {
7658 continue;
7659 }
7660 }
7661 
7662 {
7663 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7664 vinfos[0].jointtype = 1;
7665 vinfos[0].foffset = j0;
7666 vinfos[0].indices[0] = _ij0[0];
7667 vinfos[0].indices[1] = _ij0[1];
7668 vinfos[0].maxsolutions = _nj0;
7669 vinfos[1].jointtype = 1;
7670 vinfos[1].foffset = j1;
7671 vinfos[1].indices[0] = _ij1[0];
7672 vinfos[1].indices[1] = _ij1[1];
7673 vinfos[1].maxsolutions = _nj1;
7674 vinfos[2].jointtype = 1;
7675 vinfos[2].foffset = j2;
7676 vinfos[2].indices[0] = _ij2[0];
7677 vinfos[2].indices[1] = _ij2[1];
7678 vinfos[2].maxsolutions = _nj2;
7679 vinfos[3].jointtype = 1;
7680 vinfos[3].foffset = j3;
7681 vinfos[3].indices[0] = _ij3[0];
7682 vinfos[3].indices[1] = _ij3[1];
7683 vinfos[3].maxsolutions = _nj3;
7684 vinfos[4].jointtype = 1;
7685 vinfos[4].foffset = j4;
7686 vinfos[4].indices[0] = _ij4[0];
7687 vinfos[4].indices[1] = _ij4[1];
7688 vinfos[4].maxsolutions = _nj4;
7689 vinfos[5].jointtype = 1;
7690 vinfos[5].foffset = j5;
7691 vinfos[5].indices[0] = _ij5[0];
7692 vinfos[5].indices[1] = _ij5[1];
7693 vinfos[5].maxsolutions = _nj5;
7694 std::vector<int> vfree(0);
7695 solutions.AddSolution(vinfos,vfree);
7696 }
7697 }
7698 }
7699 
7700 }
7701 
7702 }
7703 
7704 } else
7705 {
7706 {
7707 IkReal j5array[1], cj5array[1], sj5array[1];
7708 bool j5valid[1]={false};
7709 _nj5 = 1;
7710 CheckValue<IkReal> x506=IKPowWithIntegerCheck(IKsign(sj4),-1);
7711 if(!x506.valid){
7712 continue;
7713 }
7714 CheckValue<IkReal> x507 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
7715 if(!x507.valid){
7716 continue;
7717 }
7718 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x506.value)))+(x507.value));
7719 sj5array[0]=IKsin(j5array[0]);
7720 cj5array[0]=IKcos(j5array[0]);
7721 if( j5array[0] > IKPI )
7722 {
7723  j5array[0]-=IK2PI;
7724 }
7725 else if( j5array[0] < -IKPI )
7726 { j5array[0]+=IK2PI;
7727 }
7728 j5valid[0] = true;
7729 for(int ij5 = 0; ij5 < 1; ++ij5)
7730 {
7731 if( !j5valid[ij5] )
7732 {
7733  continue;
7734 }
7735 _ij5[0] = ij5; _ij5[1] = -1;
7736 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7737 {
7738 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7739 {
7740  j5valid[iij5]=false; _ij5[1] = iij5; break;
7741 }
7742 }
7743 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7744 {
7745 IkReal evalcond[8];
7746 IkReal x508=IKsin(j5);
7747 IkReal x509=IKcos(j5);
7748 IkReal x510=((1.0)*cj4);
7749 IkReal x511=((1.0)*x508);
7750 evalcond[0]=(((sj4*x509))+new_r20);
7751 evalcond[1]=(((cj4*x508))+new_r11);
7752 evalcond[2]=(new_r21+(((-1.0)*sj4*x511)));
7753 evalcond[3]=(new_r10+(((-1.0)*x509*x510)));
7754 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x511)));
7755 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x509)));
7756 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r10*x510))+x509);
7757 evalcond[7]=((((-1.0)*new_r11*x510))+((new_r21*sj4))+(((-1.0)*x511)));
7758 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
7759 {
7760 continue;
7761 }
7762 }
7763 
7764 {
7765 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7766 vinfos[0].jointtype = 1;
7767 vinfos[0].foffset = j0;
7768 vinfos[0].indices[0] = _ij0[0];
7769 vinfos[0].indices[1] = _ij0[1];
7770 vinfos[0].maxsolutions = _nj0;
7771 vinfos[1].jointtype = 1;
7772 vinfos[1].foffset = j1;
7773 vinfos[1].indices[0] = _ij1[0];
7774 vinfos[1].indices[1] = _ij1[1];
7775 vinfos[1].maxsolutions = _nj1;
7776 vinfos[2].jointtype = 1;
7777 vinfos[2].foffset = j2;
7778 vinfos[2].indices[0] = _ij2[0];
7779 vinfos[2].indices[1] = _ij2[1];
7780 vinfos[2].maxsolutions = _nj2;
7781 vinfos[3].jointtype = 1;
7782 vinfos[3].foffset = j3;
7783 vinfos[3].indices[0] = _ij3[0];
7784 vinfos[3].indices[1] = _ij3[1];
7785 vinfos[3].maxsolutions = _nj3;
7786 vinfos[4].jointtype = 1;
7787 vinfos[4].foffset = j4;
7788 vinfos[4].indices[0] = _ij4[0];
7789 vinfos[4].indices[1] = _ij4[1];
7790 vinfos[4].maxsolutions = _nj4;
7791 vinfos[5].jointtype = 1;
7792 vinfos[5].foffset = j5;
7793 vinfos[5].indices[0] = _ij5[0];
7794 vinfos[5].indices[1] = _ij5[1];
7795 vinfos[5].maxsolutions = _nj5;
7796 std::vector<int> vfree(0);
7797 solutions.AddSolution(vinfos,vfree);
7798 }
7799 }
7800 }
7801 
7802 }
7803 
7804 }
7805 
7806 }
7807 } while(0);
7808 if( bgotonextstatement )
7809 {
7810 bool bgotonextstatement = true;
7811 do
7812 {
7813 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
7814 evalcond[1]=new_r02;
7815 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7816 {
7817 bgotonextstatement=false;
7818 {
7819 IkReal j5array[1], cj5array[1], sj5array[1];
7820 bool j5valid[1]={false};
7821 _nj5 = 1;
7822 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
7823  continue;
7824 j5array[0]=IKatan2(new_r00, new_r01);
7825 sj5array[0]=IKsin(j5array[0]);
7826 cj5array[0]=IKcos(j5array[0]);
7827 if( j5array[0] > IKPI )
7828 {
7829  j5array[0]-=IK2PI;
7830 }
7831 else if( j5array[0] < -IKPI )
7832 { j5array[0]+=IK2PI;
7833 }
7834 j5valid[0] = true;
7835 for(int ij5 = 0; ij5 < 1; ++ij5)
7836 {
7837 if( !j5valid[ij5] )
7838 {
7839  continue;
7840 }
7841 _ij5[0] = ij5; _ij5[1] = -1;
7842 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7843 {
7844 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7845 {
7846  j5valid[iij5]=false; _ij5[1] = iij5; break;
7847 }
7848 }
7849 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7850 {
7851 IkReal evalcond[8];
7852 IkReal x512=IKcos(j5);
7853 IkReal x513=IKsin(j5);
7854 IkReal x514=((1.0)*x513);
7855 IkReal x515=((1.0)*x512);
7856 evalcond[0]=(((sj4*x512))+new_r20);
7857 evalcond[1]=(new_r00+(((-1.0)*x514)));
7858 evalcond[2]=(new_r01+(((-1.0)*x515)));
7859 evalcond[3]=(new_r21+(((-1.0)*sj4*x514)));
7860 evalcond[4]=(((cj4*x513))+(((-1.0)*new_r11)));
7861 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*cj4*x515)));
7862 evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+x512);
7863 evalcond[7]=(((cj4*new_r11))+((new_r21*sj4))+(((-1.0)*x514)));
7864 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
7865 {
7866 continue;
7867 }
7868 }
7869 
7870 {
7871 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7872 vinfos[0].jointtype = 1;
7873 vinfos[0].foffset = j0;
7874 vinfos[0].indices[0] = _ij0[0];
7875 vinfos[0].indices[1] = _ij0[1];
7876 vinfos[0].maxsolutions = _nj0;
7877 vinfos[1].jointtype = 1;
7878 vinfos[1].foffset = j1;
7879 vinfos[1].indices[0] = _ij1[0];
7880 vinfos[1].indices[1] = _ij1[1];
7881 vinfos[1].maxsolutions = _nj1;
7882 vinfos[2].jointtype = 1;
7883 vinfos[2].foffset = j2;
7884 vinfos[2].indices[0] = _ij2[0];
7885 vinfos[2].indices[1] = _ij2[1];
7886 vinfos[2].maxsolutions = _nj2;
7887 vinfos[3].jointtype = 1;
7888 vinfos[3].foffset = j3;
7889 vinfos[3].indices[0] = _ij3[0];
7890 vinfos[3].indices[1] = _ij3[1];
7891 vinfos[3].maxsolutions = _nj3;
7892 vinfos[4].jointtype = 1;
7893 vinfos[4].foffset = j4;
7894 vinfos[4].indices[0] = _ij4[0];
7895 vinfos[4].indices[1] = _ij4[1];
7896 vinfos[4].maxsolutions = _nj4;
7897 vinfos[5].jointtype = 1;
7898 vinfos[5].foffset = j5;
7899 vinfos[5].indices[0] = _ij5[0];
7900 vinfos[5].indices[1] = _ij5[1];
7901 vinfos[5].maxsolutions = _nj5;
7902 std::vector<int> vfree(0);
7903 solutions.AddSolution(vinfos,vfree);
7904 }
7905 }
7906 }
7907 
7908 }
7909 } while(0);
7910 if( bgotonextstatement )
7911 {
7912 bool bgotonextstatement = true;
7913 do
7914 {
7915 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
7916 evalcond[1]=new_r22;
7917 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7918 {
7919 bgotonextstatement=false;
7920 {
7921 IkReal j5array[1], cj5array[1], sj5array[1];
7922 bool j5valid[1]={false};
7923 _nj5 = 1;
7924 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
7925  continue;
7926 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
7927 sj5array[0]=IKsin(j5array[0]);
7928 cj5array[0]=IKcos(j5array[0]);
7929 if( j5array[0] > IKPI )
7930 {
7931  j5array[0]-=IK2PI;
7932 }
7933 else if( j5array[0] < -IKPI )
7934 { j5array[0]+=IK2PI;
7935 }
7936 j5valid[0] = true;
7937 for(int ij5 = 0; ij5 < 1; ++ij5)
7938 {
7939 if( !j5valid[ij5] )
7940 {
7941  continue;
7942 }
7943 _ij5[0] = ij5; _ij5[1] = -1;
7944 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
7945 {
7946 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
7947 {
7948  j5valid[iij5]=false; _ij5[1] = iij5; break;
7949 }
7950 }
7951 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
7952 {
7953 IkReal evalcond[8];
7954 IkReal x516=IKcos(j5);
7955 IkReal x517=IKsin(j5);
7956 IkReal x518=((1.0)*sj3);
7957 IkReal x519=((1.0)*x517);
7958 IkReal x520=((1.0)*x516);
7959 evalcond[0]=(x516+new_r20);
7960 evalcond[1]=(new_r21+(((-1.0)*x519)));
7961 evalcond[2]=(((sj3*x516))+new_r01);
7962 evalcond[3]=(((sj3*x517))+new_r00);
7963 evalcond[4]=((((-1.0)*cj3*x520))+new_r11);
7964 evalcond[5]=((((-1.0)*new_r02*x519))+new_r10);
7965 evalcond[6]=(((cj3*new_r10))+(((-1.0)*new_r00*x518))+(((-1.0)*x519)));
7966 evalcond[7]=((((-1.0)*x520))+(((-1.0)*new_r01*x518))+((cj3*new_r11)));
7967 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
7968 {
7969 continue;
7970 }
7971 }
7972 
7973 {
7974 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7975 vinfos[0].jointtype = 1;
7976 vinfos[0].foffset = j0;
7977 vinfos[0].indices[0] = _ij0[0];
7978 vinfos[0].indices[1] = _ij0[1];
7979 vinfos[0].maxsolutions = _nj0;
7980 vinfos[1].jointtype = 1;
7981 vinfos[1].foffset = j1;
7982 vinfos[1].indices[0] = _ij1[0];
7983 vinfos[1].indices[1] = _ij1[1];
7984 vinfos[1].maxsolutions = _nj1;
7985 vinfos[2].jointtype = 1;
7986 vinfos[2].foffset = j2;
7987 vinfos[2].indices[0] = _ij2[0];
7988 vinfos[2].indices[1] = _ij2[1];
7989 vinfos[2].maxsolutions = _nj2;
7990 vinfos[3].jointtype = 1;
7991 vinfos[3].foffset = j3;
7992 vinfos[3].indices[0] = _ij3[0];
7993 vinfos[3].indices[1] = _ij3[1];
7994 vinfos[3].maxsolutions = _nj3;
7995 vinfos[4].jointtype = 1;
7996 vinfos[4].foffset = j4;
7997 vinfos[4].indices[0] = _ij4[0];
7998 vinfos[4].indices[1] = _ij4[1];
7999 vinfos[4].maxsolutions = _nj4;
8000 vinfos[5].jointtype = 1;
8001 vinfos[5].foffset = j5;
8002 vinfos[5].indices[0] = _ij5[0];
8003 vinfos[5].indices[1] = _ij5[1];
8004 vinfos[5].maxsolutions = _nj5;
8005 std::vector<int> vfree(0);
8006 solutions.AddSolution(vinfos,vfree);
8007 }
8008 }
8009 }
8010 
8011 }
8012 } while(0);
8013 if( bgotonextstatement )
8014 {
8015 bool bgotonextstatement = true;
8016 do
8017 {
8018 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
8019 evalcond[1]=new_r22;
8020 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
8021 {
8022 bgotonextstatement=false;
8023 {
8024 IkReal j5array[1], cj5array[1], sj5array[1];
8025 bool j5valid[1]={false};
8026 _nj5 = 1;
8027 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
8028  continue;
8029 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
8030 sj5array[0]=IKsin(j5array[0]);
8031 cj5array[0]=IKcos(j5array[0]);
8032 if( j5array[0] > IKPI )
8033 {
8034  j5array[0]-=IK2PI;
8035 }
8036 else if( j5array[0] < -IKPI )
8037 { j5array[0]+=IK2PI;
8038 }
8039 j5valid[0] = true;
8040 for(int ij5 = 0; ij5 < 1; ++ij5)
8041 {
8042 if( !j5valid[ij5] )
8043 {
8044  continue;
8045 }
8046 _ij5[0] = ij5; _ij5[1] = -1;
8047 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8048 {
8049 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8050 {
8051  j5valid[iij5]=false; _ij5[1] = iij5; break;
8052 }
8053 }
8054 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8055 {
8056 IkReal evalcond[8];
8057 IkReal x521=IKcos(j5);
8058 IkReal x522=IKsin(j5);
8059 IkReal x523=((1.0)*sj3);
8060 IkReal x524=((1.0)*x521);
8061 evalcond[0]=(x522+new_r21);
8062 evalcond[1]=((((-1.0)*x524))+new_r20);
8063 evalcond[2]=(((sj3*x521))+new_r01);
8064 evalcond[3]=(((sj3*x522))+new_r00);
8065 evalcond[4]=(((new_r02*x522))+new_r10);
8066 evalcond[5]=((((-1.0)*cj3*x524))+new_r11);
8067 evalcond[6]=((((-1.0)*x522))+(((-1.0)*new_r00*x523))+((cj3*new_r10)));
8068 evalcond[7]=((((-1.0)*x524))+(((-1.0)*new_r01*x523))+((cj3*new_r11)));
8069 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
8070 {
8071 continue;
8072 }
8073 }
8074 
8075 {
8076 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8077 vinfos[0].jointtype = 1;
8078 vinfos[0].foffset = j0;
8079 vinfos[0].indices[0] = _ij0[0];
8080 vinfos[0].indices[1] = _ij0[1];
8081 vinfos[0].maxsolutions = _nj0;
8082 vinfos[1].jointtype = 1;
8083 vinfos[1].foffset = j1;
8084 vinfos[1].indices[0] = _ij1[0];
8085 vinfos[1].indices[1] = _ij1[1];
8086 vinfos[1].maxsolutions = _nj1;
8087 vinfos[2].jointtype = 1;
8088 vinfos[2].foffset = j2;
8089 vinfos[2].indices[0] = _ij2[0];
8090 vinfos[2].indices[1] = _ij2[1];
8091 vinfos[2].maxsolutions = _nj2;
8092 vinfos[3].jointtype = 1;
8093 vinfos[3].foffset = j3;
8094 vinfos[3].indices[0] = _ij3[0];
8095 vinfos[3].indices[1] = _ij3[1];
8096 vinfos[3].maxsolutions = _nj3;
8097 vinfos[4].jointtype = 1;
8098 vinfos[4].foffset = j4;
8099 vinfos[4].indices[0] = _ij4[0];
8100 vinfos[4].indices[1] = _ij4[1];
8101 vinfos[4].maxsolutions = _nj4;
8102 vinfos[5].jointtype = 1;
8103 vinfos[5].foffset = j5;
8104 vinfos[5].indices[0] = _ij5[0];
8105 vinfos[5].indices[1] = _ij5[1];
8106 vinfos[5].maxsolutions = _nj5;
8107 std::vector<int> vfree(0);
8108 solutions.AddSolution(vinfos,vfree);
8109 }
8110 }
8111 }
8112 
8113 }
8114 } while(0);
8115 if( bgotonextstatement )
8116 {
8117 bool bgotonextstatement = true;
8118 do
8119 {
8120 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
8121 evalcond[1]=new_r20;
8122 evalcond[2]=new_r02;
8123 evalcond[3]=new_r12;
8124 evalcond[4]=new_r21;
8125 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 )
8126 {
8127 bgotonextstatement=false;
8128 {
8129 IkReal j5array[1], cj5array[1], sj5array[1];
8130 bool j5valid[1]={false};
8131 _nj5 = 1;
8132 IkReal x525=((1.0)*new_r01);
8133 if( IKabs(((((-1.0)*cj3*x525))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x525))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x525))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x525))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
8134  continue;
8135 j5array[0]=IKatan2(((((-1.0)*cj3*x525))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x525))+((cj3*new_r00))));
8136 sj5array[0]=IKsin(j5array[0]);
8137 cj5array[0]=IKcos(j5array[0]);
8138 if( j5array[0] > IKPI )
8139 {
8140  j5array[0]-=IK2PI;
8141 }
8142 else if( j5array[0] < -IKPI )
8143 { j5array[0]+=IK2PI;
8144 }
8145 j5valid[0] = true;
8146 for(int ij5 = 0; ij5 < 1; ++ij5)
8147 {
8148 if( !j5valid[ij5] )
8149 {
8150  continue;
8151 }
8152 _ij5[0] = ij5; _ij5[1] = -1;
8153 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8154 {
8155 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8156 {
8157  j5valid[iij5]=false; _ij5[1] = iij5; break;
8158 }
8159 }
8160 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8161 {
8162 IkReal evalcond[8];
8163 IkReal x526=IKsin(j5);
8164 IkReal x527=IKcos(j5);
8165 IkReal x528=((1.0)*sj3);
8166 IkReal x529=((1.0)*x527);
8167 IkReal x530=(sj3*x526);
8168 IkReal x531=((1.0)*x526);
8169 IkReal x532=(cj3*x529);
8170 evalcond[0]=(((new_r11*sj3))+x526+((cj3*new_r01)));
8171 evalcond[1]=(((sj3*x527))+new_r01+((cj3*x526)));
8172 evalcond[2]=(((new_r10*sj3))+(((-1.0)*x529))+((cj3*new_r00)));
8173 evalcond[3]=((((-1.0)*new_r00*x528))+(((-1.0)*x531))+((cj3*new_r10)));
8174 evalcond[4]=((((-1.0)*x529))+(((-1.0)*new_r01*x528))+((cj3*new_r11)));
8175 evalcond[5]=((((-1.0)*x532))+x530+new_r00);
8176 evalcond[6]=((((-1.0)*x532))+x530+new_r11);
8177 evalcond[7]=((((-1.0)*x527*x528))+(((-1.0)*cj3*x531))+new_r10);
8178 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
8179 {
8180 continue;
8181 }
8182 }
8183 
8184 {
8185 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8186 vinfos[0].jointtype = 1;
8187 vinfos[0].foffset = j0;
8188 vinfos[0].indices[0] = _ij0[0];
8189 vinfos[0].indices[1] = _ij0[1];
8190 vinfos[0].maxsolutions = _nj0;
8191 vinfos[1].jointtype = 1;
8192 vinfos[1].foffset = j1;
8193 vinfos[1].indices[0] = _ij1[0];
8194 vinfos[1].indices[1] = _ij1[1];
8195 vinfos[1].maxsolutions = _nj1;
8196 vinfos[2].jointtype = 1;
8197 vinfos[2].foffset = j2;
8198 vinfos[2].indices[0] = _ij2[0];
8199 vinfos[2].indices[1] = _ij2[1];
8200 vinfos[2].maxsolutions = _nj2;
8201 vinfos[3].jointtype = 1;
8202 vinfos[3].foffset = j3;
8203 vinfos[3].indices[0] = _ij3[0];
8204 vinfos[3].indices[1] = _ij3[1];
8205 vinfos[3].maxsolutions = _nj3;
8206 vinfos[4].jointtype = 1;
8207 vinfos[4].foffset = j4;
8208 vinfos[4].indices[0] = _ij4[0];
8209 vinfos[4].indices[1] = _ij4[1];
8210 vinfos[4].maxsolutions = _nj4;
8211 vinfos[5].jointtype = 1;
8212 vinfos[5].foffset = j5;
8213 vinfos[5].indices[0] = _ij5[0];
8214 vinfos[5].indices[1] = _ij5[1];
8215 vinfos[5].maxsolutions = _nj5;
8216 std::vector<int> vfree(0);
8217 solutions.AddSolution(vinfos,vfree);
8218 }
8219 }
8220 }
8221 
8222 }
8223 } while(0);
8224 if( bgotonextstatement )
8225 {
8226 bool bgotonextstatement = true;
8227 do
8228 {
8229 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
8230 evalcond[1]=new_r20;
8231 evalcond[2]=new_r02;
8232 evalcond[3]=new_r12;
8233 evalcond[4]=new_r21;
8234 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 )
8235 {
8236 bgotonextstatement=false;
8237 {
8238 IkReal j5array[1], cj5array[1], sj5array[1];
8239 bool j5valid[1]={false};
8240 _nj5 = 1;
8241 IkReal x533=((1.0)*sj3);
8242 if( IKabs(((((-1.0)*new_r00*x533))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x533)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x533))+((cj3*new_r01))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x533))))-1) <= IKFAST_SINCOS_THRESH )
8243  continue;
8244 j5array[0]=IKatan2(((((-1.0)*new_r00*x533))+((cj3*new_r01))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x533))));
8245 sj5array[0]=IKsin(j5array[0]);
8246 cj5array[0]=IKcos(j5array[0]);
8247 if( j5array[0] > IKPI )
8248 {
8249  j5array[0]-=IK2PI;
8250 }
8251 else if( j5array[0] < -IKPI )
8252 { j5array[0]+=IK2PI;
8253 }
8254 j5valid[0] = true;
8255 for(int ij5 = 0; ij5 < 1; ++ij5)
8256 {
8257 if( !j5valid[ij5] )
8258 {
8259  continue;
8260 }
8261 _ij5[0] = ij5; _ij5[1] = -1;
8262 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8263 {
8264 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8265 {
8266  j5valid[iij5]=false; _ij5[1] = iij5; break;
8267 }
8268 }
8269 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8270 {
8271 IkReal evalcond[8];
8272 IkReal x534=IKsin(j5);
8273 IkReal x535=IKcos(j5);
8274 IkReal x536=((1.0)*sj3);
8275 IkReal x537=((1.0)*x534);
8276 IkReal x538=(sj3*x535);
8277 IkReal x539=((1.0)*x535);
8278 IkReal x540=(cj3*x537);
8279 evalcond[0]=(((new_r10*sj3))+x535+((cj3*new_r00)));
8280 evalcond[1]=(((new_r11*sj3))+(((-1.0)*x537))+((cj3*new_r01)));
8281 evalcond[2]=(((sj3*x534))+new_r00+((cj3*x535)));
8282 evalcond[3]=((((-1.0)*new_r00*x536))+(((-1.0)*x537))+((cj3*new_r10)));
8283 evalcond[4]=((((-1.0)*x539))+(((-1.0)*new_r01*x536))+((cj3*new_r11)));
8284 evalcond[5]=((((-1.0)*x540))+x538+new_r01);
8285 evalcond[6]=((((-1.0)*x540))+x538+new_r10);
8286 evalcond[7]=((((-1.0)*x534*x536))+(((-1.0)*cj3*x539))+new_r11);
8287 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
8288 {
8289 continue;
8290 }
8291 }
8292 
8293 {
8294 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8295 vinfos[0].jointtype = 1;
8296 vinfos[0].foffset = j0;
8297 vinfos[0].indices[0] = _ij0[0];
8298 vinfos[0].indices[1] = _ij0[1];
8299 vinfos[0].maxsolutions = _nj0;
8300 vinfos[1].jointtype = 1;
8301 vinfos[1].foffset = j1;
8302 vinfos[1].indices[0] = _ij1[0];
8303 vinfos[1].indices[1] = _ij1[1];
8304 vinfos[1].maxsolutions = _nj1;
8305 vinfos[2].jointtype = 1;
8306 vinfos[2].foffset = j2;
8307 vinfos[2].indices[0] = _ij2[0];
8308 vinfos[2].indices[1] = _ij2[1];
8309 vinfos[2].maxsolutions = _nj2;
8310 vinfos[3].jointtype = 1;
8311 vinfos[3].foffset = j3;
8312 vinfos[3].indices[0] = _ij3[0];
8313 vinfos[3].indices[1] = _ij3[1];
8314 vinfos[3].maxsolutions = _nj3;
8315 vinfos[4].jointtype = 1;
8316 vinfos[4].foffset = j4;
8317 vinfos[4].indices[0] = _ij4[0];
8318 vinfos[4].indices[1] = _ij4[1];
8319 vinfos[4].maxsolutions = _nj4;
8320 vinfos[5].jointtype = 1;
8321 vinfos[5].foffset = j5;
8322 vinfos[5].indices[0] = _ij5[0];
8323 vinfos[5].indices[1] = _ij5[1];
8324 vinfos[5].maxsolutions = _nj5;
8325 std::vector<int> vfree(0);
8326 solutions.AddSolution(vinfos,vfree);
8327 }
8328 }
8329 }
8330 
8331 }
8332 } while(0);
8333 if( bgotonextstatement )
8334 {
8335 bool bgotonextstatement = true;
8336 do
8337 {
8338 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
8339 evalcond[1]=new_r12;
8340 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
8341 {
8342 bgotonextstatement=false;
8343 {
8344 IkReal j5array[1], cj5array[1], sj5array[1];
8345 bool j5valid[1]={false};
8346 _nj5 = 1;
8347 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
8348  continue;
8349 j5array[0]=IKatan2(new_r10, new_r11);
8350 sj5array[0]=IKsin(j5array[0]);
8351 cj5array[0]=IKcos(j5array[0]);
8352 if( j5array[0] > IKPI )
8353 {
8354  j5array[0]-=IK2PI;
8355 }
8356 else if( j5array[0] < -IKPI )
8357 { j5array[0]+=IK2PI;
8358 }
8359 j5valid[0] = true;
8360 for(int ij5 = 0; ij5 < 1; ++ij5)
8361 {
8362 if( !j5valid[ij5] )
8363 {
8364  continue;
8365 }
8366 _ij5[0] = ij5; _ij5[1] = -1;
8367 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8368 {
8369 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8370 {
8371  j5valid[iij5]=false; _ij5[1] = iij5; break;
8372 }
8373 }
8374 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8375 {
8376 IkReal evalcond[8];
8377 IkReal x541=IKcos(j5);
8378 IkReal x542=IKsin(j5);
8379 IkReal x543=((1.0)*cj4);
8380 IkReal x544=((1.0)*x542);
8381 evalcond[0]=(((new_r02*x541))+new_r20);
8382 evalcond[1]=((((-1.0)*x544))+new_r10);
8383 evalcond[2]=((((-1.0)*x541))+new_r11);
8384 evalcond[3]=(((cj4*x542))+new_r01);
8385 evalcond[4]=(new_r21+(((-1.0)*new_r02*x544)));
8386 evalcond[5]=((((-1.0)*x541*x543))+new_r00);
8387 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r00*x543))+x541);
8388 evalcond[7]=((((-1.0)*new_r01*x543))+(((-1.0)*x544))+((new_r21*sj4)));
8389 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
8390 {
8391 continue;
8392 }
8393 }
8394 
8395 {
8396 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8397 vinfos[0].jointtype = 1;
8398 vinfos[0].foffset = j0;
8399 vinfos[0].indices[0] = _ij0[0];
8400 vinfos[0].indices[1] = _ij0[1];
8401 vinfos[0].maxsolutions = _nj0;
8402 vinfos[1].jointtype = 1;
8403 vinfos[1].foffset = j1;
8404 vinfos[1].indices[0] = _ij1[0];
8405 vinfos[1].indices[1] = _ij1[1];
8406 vinfos[1].maxsolutions = _nj1;
8407 vinfos[2].jointtype = 1;
8408 vinfos[2].foffset = j2;
8409 vinfos[2].indices[0] = _ij2[0];
8410 vinfos[2].indices[1] = _ij2[1];
8411 vinfos[2].maxsolutions = _nj2;
8412 vinfos[3].jointtype = 1;
8413 vinfos[3].foffset = j3;
8414 vinfos[3].indices[0] = _ij3[0];
8415 vinfos[3].indices[1] = _ij3[1];
8416 vinfos[3].maxsolutions = _nj3;
8417 vinfos[4].jointtype = 1;
8418 vinfos[4].foffset = j4;
8419 vinfos[4].indices[0] = _ij4[0];
8420 vinfos[4].indices[1] = _ij4[1];
8421 vinfos[4].maxsolutions = _nj4;
8422 vinfos[5].jointtype = 1;
8423 vinfos[5].foffset = j5;
8424 vinfos[5].indices[0] = _ij5[0];
8425 vinfos[5].indices[1] = _ij5[1];
8426 vinfos[5].maxsolutions = _nj5;
8427 std::vector<int> vfree(0);
8428 solutions.AddSolution(vinfos,vfree);
8429 }
8430 }
8431 }
8432 
8433 }
8434 } while(0);
8435 if( bgotonextstatement )
8436 {
8437 bool bgotonextstatement = true;
8438 do
8439 {
8440 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
8441 evalcond[1]=new_r12;
8442 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
8443 {
8444 bgotonextstatement=false;
8445 {
8446 IkReal j5eval[3];
8447 sj3=0;
8448 cj3=-1.0;
8449 j3=3.14159265358979;
8450 j5eval[0]=new_r02;
8451 j5eval[1]=IKsign(new_r02);
8452 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
8453 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
8454 {
8455 {
8456 IkReal j5eval[1];
8457 sj3=0;
8458 cj3=-1.0;
8459 j3=3.14159265358979;
8460 j5eval[0]=new_r02;
8461 if( IKabs(j5eval[0]) < 0.0000010000000000 )
8462 {
8463 {
8464 IkReal j5eval[2];
8465 sj3=0;
8466 cj3=-1.0;
8467 j3=3.14159265358979;
8468 j5eval[0]=new_r02;
8469 j5eval[1]=cj4;
8470 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
8471 {
8472 {
8473 IkReal evalcond[4];
8474 bool bgotonextstatement = true;
8475 do
8476 {
8477 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
8478 evalcond[1]=new_r22;
8479 evalcond[2]=new_r01;
8480 evalcond[3]=new_r00;
8481 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
8482 {
8483 bgotonextstatement=false;
8484 {
8485 IkReal j5array[1], cj5array[1], sj5array[1];
8486 bool j5valid[1]={false};
8487 _nj5 = 1;
8488 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((-1.0)*new_r20))-1) <= IKFAST_SINCOS_THRESH )
8489  continue;
8490 j5array[0]=IKatan2(new_r21, ((-1.0)*new_r20));
8491 sj5array[0]=IKsin(j5array[0]);
8492 cj5array[0]=IKcos(j5array[0]);
8493 if( j5array[0] > IKPI )
8494 {
8495  j5array[0]-=IK2PI;
8496 }
8497 else if( j5array[0] < -IKPI )
8498 { j5array[0]+=IK2PI;
8499 }
8500 j5valid[0] = true;
8501 for(int ij5 = 0; ij5 < 1; ++ij5)
8502 {
8503 if( !j5valid[ij5] )
8504 {
8505  continue;
8506 }
8507 _ij5[0] = ij5; _ij5[1] = -1;
8508 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8509 {
8510 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8511 {
8512  j5valid[iij5]=false; _ij5[1] = iij5; break;
8513 }
8514 }
8515 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8516 {
8517 IkReal evalcond[4];
8518 IkReal x545=IKcos(j5);
8519 IkReal x546=((1.0)*(IKsin(j5)));
8520 evalcond[0]=(x545+new_r20);
8521 evalcond[1]=((((-1.0)*x546))+new_r21);
8522 evalcond[2]=((((-1.0)*x546))+(((-1.0)*new_r10)));
8523 evalcond[3]=((((-1.0)*x545))+(((-1.0)*new_r11)));
8524 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
8525 {
8526 continue;
8527 }
8528 }
8529 
8530 {
8531 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8532 vinfos[0].jointtype = 1;
8533 vinfos[0].foffset = j0;
8534 vinfos[0].indices[0] = _ij0[0];
8535 vinfos[0].indices[1] = _ij0[1];
8536 vinfos[0].maxsolutions = _nj0;
8537 vinfos[1].jointtype = 1;
8538 vinfos[1].foffset = j1;
8539 vinfos[1].indices[0] = _ij1[0];
8540 vinfos[1].indices[1] = _ij1[1];
8541 vinfos[1].maxsolutions = _nj1;
8542 vinfos[2].jointtype = 1;
8543 vinfos[2].foffset = j2;
8544 vinfos[2].indices[0] = _ij2[0];
8545 vinfos[2].indices[1] = _ij2[1];
8546 vinfos[2].maxsolutions = _nj2;
8547 vinfos[3].jointtype = 1;
8548 vinfos[3].foffset = j3;
8549 vinfos[3].indices[0] = _ij3[0];
8550 vinfos[3].indices[1] = _ij3[1];
8551 vinfos[3].maxsolutions = _nj3;
8552 vinfos[4].jointtype = 1;
8553 vinfos[4].foffset = j4;
8554 vinfos[4].indices[0] = _ij4[0];
8555 vinfos[4].indices[1] = _ij4[1];
8556 vinfos[4].maxsolutions = _nj4;
8557 vinfos[5].jointtype = 1;
8558 vinfos[5].foffset = j5;
8559 vinfos[5].indices[0] = _ij5[0];
8560 vinfos[5].indices[1] = _ij5[1];
8561 vinfos[5].maxsolutions = _nj5;
8562 std::vector<int> vfree(0);
8563 solutions.AddSolution(vinfos,vfree);
8564 }
8565 }
8566 }
8567 
8568 }
8569 } while(0);
8570 if( bgotonextstatement )
8571 {
8572 bool bgotonextstatement = true;
8573 do
8574 {
8575 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
8576 evalcond[1]=new_r22;
8577 evalcond[2]=new_r01;
8578 evalcond[3]=new_r00;
8579 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
8580 {
8581 bgotonextstatement=false;
8582 {
8583 IkReal j5array[1], cj5array[1], sj5array[1];
8584 bool j5valid[1]={false};
8585 _nj5 = 1;
8586 if( IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
8587  continue;
8588 j5array[0]=IKatan2(((-1.0)*new_r21), new_r20);
8589 sj5array[0]=IKsin(j5array[0]);
8590 cj5array[0]=IKcos(j5array[0]);
8591 if( j5array[0] > IKPI )
8592 {
8593  j5array[0]-=IK2PI;
8594 }
8595 else if( j5array[0] < -IKPI )
8596 { j5array[0]+=IK2PI;
8597 }
8598 j5valid[0] = true;
8599 for(int ij5 = 0; ij5 < 1; ++ij5)
8600 {
8601 if( !j5valid[ij5] )
8602 {
8603  continue;
8604 }
8605 _ij5[0] = ij5; _ij5[1] = -1;
8606 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8607 {
8608 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8609 {
8610  j5valid[iij5]=false; _ij5[1] = iij5; break;
8611 }
8612 }
8613 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8614 {
8615 IkReal evalcond[4];
8616 IkReal x547=IKsin(j5);
8617 IkReal x548=((1.0)*(IKcos(j5)));
8618 evalcond[0]=(x547+new_r21);
8619 evalcond[1]=((((-1.0)*x548))+new_r20);
8620 evalcond[2]=((((-1.0)*x547))+(((-1.0)*new_r10)));
8621 evalcond[3]=((((-1.0)*x548))+(((-1.0)*new_r11)));
8622 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
8623 {
8624 continue;
8625 }
8626 }
8627 
8628 {
8629 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8630 vinfos[0].jointtype = 1;
8631 vinfos[0].foffset = j0;
8632 vinfos[0].indices[0] = _ij0[0];
8633 vinfos[0].indices[1] = _ij0[1];
8634 vinfos[0].maxsolutions = _nj0;
8635 vinfos[1].jointtype = 1;
8636 vinfos[1].foffset = j1;
8637 vinfos[1].indices[0] = _ij1[0];
8638 vinfos[1].indices[1] = _ij1[1];
8639 vinfos[1].maxsolutions = _nj1;
8640 vinfos[2].jointtype = 1;
8641 vinfos[2].foffset = j2;
8642 vinfos[2].indices[0] = _ij2[0];
8643 vinfos[2].indices[1] = _ij2[1];
8644 vinfos[2].maxsolutions = _nj2;
8645 vinfos[3].jointtype = 1;
8646 vinfos[3].foffset = j3;
8647 vinfos[3].indices[0] = _ij3[0];
8648 vinfos[3].indices[1] = _ij3[1];
8649 vinfos[3].maxsolutions = _nj3;
8650 vinfos[4].jointtype = 1;
8651 vinfos[4].foffset = j4;
8652 vinfos[4].indices[0] = _ij4[0];
8653 vinfos[4].indices[1] = _ij4[1];
8654 vinfos[4].maxsolutions = _nj4;
8655 vinfos[5].jointtype = 1;
8656 vinfos[5].foffset = j5;
8657 vinfos[5].indices[0] = _ij5[0];
8658 vinfos[5].indices[1] = _ij5[1];
8659 vinfos[5].maxsolutions = _nj5;
8660 std::vector<int> vfree(0);
8661 solutions.AddSolution(vinfos,vfree);
8662 }
8663 }
8664 }
8665 
8666 }
8667 } while(0);
8668 if( bgotonextstatement )
8669 {
8670 bool bgotonextstatement = true;
8671 do
8672 {
8673 evalcond[0]=IKabs(new_r02);
8674 evalcond[1]=new_r20;
8675 evalcond[2]=new_r21;
8676 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
8677 {
8678 bgotonextstatement=false;
8679 {
8680 IkReal j5array[1], cj5array[1], sj5array[1];
8681 bool j5valid[1]={false};
8682 _nj5 = 1;
8683 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*cj4*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*cj4*new_r00))-1) <= IKFAST_SINCOS_THRESH )
8684  continue;
8685 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*cj4*new_r00));
8686 sj5array[0]=IKsin(j5array[0]);
8687 cj5array[0]=IKcos(j5array[0]);
8688 if( j5array[0] > IKPI )
8689 {
8690  j5array[0]-=IK2PI;
8691 }
8692 else if( j5array[0] < -IKPI )
8693 { j5array[0]+=IK2PI;
8694 }
8695 j5valid[0] = true;
8696 for(int ij5 = 0; ij5 < 1; ++ij5)
8697 {
8698 if( !j5valid[ij5] )
8699 {
8700  continue;
8701 }
8702 _ij5[0] = ij5; _ij5[1] = -1;
8703 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8704 {
8705 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8706 {
8707  j5valid[iij5]=false; _ij5[1] = iij5; break;
8708 }
8709 }
8710 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8711 {
8712 IkReal evalcond[6];
8713 IkReal x549=IKcos(j5);
8714 IkReal x550=IKsin(j5);
8715 IkReal x551=((1.0)*x550);
8716 IkReal x552=((1.0)*x549);
8717 evalcond[0]=(((cj4*new_r00))+x549);
8718 evalcond[1]=((((-1.0)*x551))+(((-1.0)*new_r10)));
8719 evalcond[2]=((((-1.0)*x552))+(((-1.0)*new_r11)));
8720 evalcond[3]=(((cj4*x550))+(((-1.0)*new_r01)));
8721 evalcond[4]=(((cj4*new_r01))+(((-1.0)*x551)));
8722 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x552)));
8723 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8724 {
8725 continue;
8726 }
8727 }
8728 
8729 {
8730 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8731 vinfos[0].jointtype = 1;
8732 vinfos[0].foffset = j0;
8733 vinfos[0].indices[0] = _ij0[0];
8734 vinfos[0].indices[1] = _ij0[1];
8735 vinfos[0].maxsolutions = _nj0;
8736 vinfos[1].jointtype = 1;
8737 vinfos[1].foffset = j1;
8738 vinfos[1].indices[0] = _ij1[0];
8739 vinfos[1].indices[1] = _ij1[1];
8740 vinfos[1].maxsolutions = _nj1;
8741 vinfos[2].jointtype = 1;
8742 vinfos[2].foffset = j2;
8743 vinfos[2].indices[0] = _ij2[0];
8744 vinfos[2].indices[1] = _ij2[1];
8745 vinfos[2].maxsolutions = _nj2;
8746 vinfos[3].jointtype = 1;
8747 vinfos[3].foffset = j3;
8748 vinfos[3].indices[0] = _ij3[0];
8749 vinfos[3].indices[1] = _ij3[1];
8750 vinfos[3].maxsolutions = _nj3;
8751 vinfos[4].jointtype = 1;
8752 vinfos[4].foffset = j4;
8753 vinfos[4].indices[0] = _ij4[0];
8754 vinfos[4].indices[1] = _ij4[1];
8755 vinfos[4].maxsolutions = _nj4;
8756 vinfos[5].jointtype = 1;
8757 vinfos[5].foffset = j5;
8758 vinfos[5].indices[0] = _ij5[0];
8759 vinfos[5].indices[1] = _ij5[1];
8760 vinfos[5].maxsolutions = _nj5;
8761 std::vector<int> vfree(0);
8762 solutions.AddSolution(vinfos,vfree);
8763 }
8764 }
8765 }
8766 
8767 }
8768 } while(0);
8769 if( bgotonextstatement )
8770 {
8771 bool bgotonextstatement = true;
8772 do
8773 {
8774 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
8775 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8776 {
8777 bgotonextstatement=false;
8778 {
8779 IkReal j5array[1], cj5array[1], sj5array[1];
8780 bool j5valid[1]={false};
8781 _nj5 = 1;
8782 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 )
8783  continue;
8784 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
8785 sj5array[0]=IKsin(j5array[0]);
8786 cj5array[0]=IKcos(j5array[0]);
8787 if( j5array[0] > IKPI )
8788 {
8789  j5array[0]-=IK2PI;
8790 }
8791 else if( j5array[0] < -IKPI )
8792 { j5array[0]+=IK2PI;
8793 }
8794 j5valid[0] = true;
8795 for(int ij5 = 0; ij5 < 1; ++ij5)
8796 {
8797 if( !j5valid[ij5] )
8798 {
8799  continue;
8800 }
8801 _ij5[0] = ij5; _ij5[1] = -1;
8802 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8803 {
8804 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8805 {
8806  j5valid[iij5]=false; _ij5[1] = iij5; break;
8807 }
8808 }
8809 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8810 {
8811 IkReal evalcond[6];
8812 IkReal x553=IKsin(j5);
8813 IkReal x554=IKcos(j5);
8814 evalcond[0]=x554;
8815 evalcond[1]=(new_r22*x553);
8816 evalcond[2]=((-1.0)*x553);
8817 evalcond[3]=((-1.0)*new_r22*x554);
8818 evalcond[4]=((((-1.0)*x553))+(((-1.0)*new_r10)));
8819 evalcond[5]=((((-1.0)*x554))+(((-1.0)*new_r11)));
8820 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8821 {
8822 continue;
8823 }
8824 }
8825 
8826 {
8827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8828 vinfos[0].jointtype = 1;
8829 vinfos[0].foffset = j0;
8830 vinfos[0].indices[0] = _ij0[0];
8831 vinfos[0].indices[1] = _ij0[1];
8832 vinfos[0].maxsolutions = _nj0;
8833 vinfos[1].jointtype = 1;
8834 vinfos[1].foffset = j1;
8835 vinfos[1].indices[0] = _ij1[0];
8836 vinfos[1].indices[1] = _ij1[1];
8837 vinfos[1].maxsolutions = _nj1;
8838 vinfos[2].jointtype = 1;
8839 vinfos[2].foffset = j2;
8840 vinfos[2].indices[0] = _ij2[0];
8841 vinfos[2].indices[1] = _ij2[1];
8842 vinfos[2].maxsolutions = _nj2;
8843 vinfos[3].jointtype = 1;
8844 vinfos[3].foffset = j3;
8845 vinfos[3].indices[0] = _ij3[0];
8846 vinfos[3].indices[1] = _ij3[1];
8847 vinfos[3].maxsolutions = _nj3;
8848 vinfos[4].jointtype = 1;
8849 vinfos[4].foffset = j4;
8850 vinfos[4].indices[0] = _ij4[0];
8851 vinfos[4].indices[1] = _ij4[1];
8852 vinfos[4].maxsolutions = _nj4;
8853 vinfos[5].jointtype = 1;
8854 vinfos[5].foffset = j5;
8855 vinfos[5].indices[0] = _ij5[0];
8856 vinfos[5].indices[1] = _ij5[1];
8857 vinfos[5].maxsolutions = _nj5;
8858 std::vector<int> vfree(0);
8859 solutions.AddSolution(vinfos,vfree);
8860 }
8861 }
8862 }
8863 
8864 }
8865 } while(0);
8866 if( bgotonextstatement )
8867 {
8868 bool bgotonextstatement = true;
8869 do
8870 {
8871 if( 1 )
8872 {
8873 bgotonextstatement=false;
8874 continue; // branch miss [j5]
8875 
8876 }
8877 } while(0);
8878 if( bgotonextstatement )
8879 {
8880 }
8881 }
8882 }
8883 }
8884 }
8885 }
8886 
8887 } else
8888 {
8889 {
8890 IkReal j5array[1], cj5array[1], sj5array[1];
8891 bool j5valid[1]={false};
8892 _nj5 = 1;
8893 CheckValue<IkReal> x555=IKPowWithIntegerCheck(new_r02,-1);
8894 if(!x555.valid){
8895 continue;
8896 }
8897 CheckValue<IkReal> x556=IKPowWithIntegerCheck(cj4,-1);
8898 if(!x556.valid){
8899 continue;
8900 }
8901 if( IKabs(((-1.0)*new_r21*(x555.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00*(x556.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*(x555.value)))+IKsqr(((-1.0)*new_r00*(x556.value)))-1) <= IKFAST_SINCOS_THRESH )
8902  continue;
8903 j5array[0]=IKatan2(((-1.0)*new_r21*(x555.value)), ((-1.0)*new_r00*(x556.value)));
8904 sj5array[0]=IKsin(j5array[0]);
8905 cj5array[0]=IKcos(j5array[0]);
8906 if( j5array[0] > IKPI )
8907 {
8908  j5array[0]-=IK2PI;
8909 }
8910 else if( j5array[0] < -IKPI )
8911 { j5array[0]+=IK2PI;
8912 }
8913 j5valid[0] = true;
8914 for(int ij5 = 0; ij5 < 1; ++ij5)
8915 {
8916 if( !j5valid[ij5] )
8917 {
8918  continue;
8919 }
8920 _ij5[0] = ij5; _ij5[1] = -1;
8921 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
8922 {
8923 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
8924 {
8925  j5valid[iij5]=false; _ij5[1] = iij5; break;
8926 }
8927 }
8928 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
8929 {
8930 IkReal evalcond[8];
8931 IkReal x557=IKsin(j5);
8932 IkReal x558=IKcos(j5);
8933 IkReal x559=((1.0)*x557);
8934 IkReal x560=((1.0)*x558);
8935 evalcond[0]=(((new_r02*x557))+new_r21);
8936 evalcond[1]=((((-1.0)*new_r02*x560))+new_r20);
8937 evalcond[2]=((((-1.0)*x559))+(((-1.0)*new_r10)));
8938 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x560)));
8939 evalcond[4]=(((cj4*x557))+(((-1.0)*new_r01)));
8940 evalcond[5]=((((-1.0)*cj4*x560))+(((-1.0)*new_r00)));
8941 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x558);
8942 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x559))+((new_r21*sj4)));
8943 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
8944 {
8945 continue;
8946 }
8947 }
8948 
8949 {
8950 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8951 vinfos[0].jointtype = 1;
8952 vinfos[0].foffset = j0;
8953 vinfos[0].indices[0] = _ij0[0];
8954 vinfos[0].indices[1] = _ij0[1];
8955 vinfos[0].maxsolutions = _nj0;
8956 vinfos[1].jointtype = 1;
8957 vinfos[1].foffset = j1;
8958 vinfos[1].indices[0] = _ij1[0];
8959 vinfos[1].indices[1] = _ij1[1];
8960 vinfos[1].maxsolutions = _nj1;
8961 vinfos[2].jointtype = 1;
8962 vinfos[2].foffset = j2;
8963 vinfos[2].indices[0] = _ij2[0];
8964 vinfos[2].indices[1] = _ij2[1];
8965 vinfos[2].maxsolutions = _nj2;
8966 vinfos[3].jointtype = 1;
8967 vinfos[3].foffset = j3;
8968 vinfos[3].indices[0] = _ij3[0];
8969 vinfos[3].indices[1] = _ij3[1];
8970 vinfos[3].maxsolutions = _nj3;
8971 vinfos[4].jointtype = 1;
8972 vinfos[4].foffset = j4;
8973 vinfos[4].indices[0] = _ij4[0];
8974 vinfos[4].indices[1] = _ij4[1];
8975 vinfos[4].maxsolutions = _nj4;
8976 vinfos[5].jointtype = 1;
8977 vinfos[5].foffset = j5;
8978 vinfos[5].indices[0] = _ij5[0];
8979 vinfos[5].indices[1] = _ij5[1];
8980 vinfos[5].maxsolutions = _nj5;
8981 std::vector<int> vfree(0);
8982 solutions.AddSolution(vinfos,vfree);
8983 }
8984 }
8985 }
8986 
8987 }
8988 
8989 }
8990 
8991 } else
8992 {
8993 {
8994 IkReal j5array[1], cj5array[1], sj5array[1];
8995 bool j5valid[1]={false};
8996 _nj5 = 1;
8997 CheckValue<IkReal> x561=IKPowWithIntegerCheck(new_r02,-1);
8998 if(!x561.valid){
8999 continue;
9000 }
9001 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r20*(x561.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr((new_r20*(x561.value)))-1) <= IKFAST_SINCOS_THRESH )
9002  continue;
9003 j5array[0]=IKatan2(((-1.0)*new_r10), (new_r20*(x561.value)));
9004 sj5array[0]=IKsin(j5array[0]);
9005 cj5array[0]=IKcos(j5array[0]);
9006 if( j5array[0] > IKPI )
9007 {
9008  j5array[0]-=IK2PI;
9009 }
9010 else if( j5array[0] < -IKPI )
9011 { j5array[0]+=IK2PI;
9012 }
9013 j5valid[0] = true;
9014 for(int ij5 = 0; ij5 < 1; ++ij5)
9015 {
9016 if( !j5valid[ij5] )
9017 {
9018  continue;
9019 }
9020 _ij5[0] = ij5; _ij5[1] = -1;
9021 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
9022 {
9023 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9024 {
9025  j5valid[iij5]=false; _ij5[1] = iij5; break;
9026 }
9027 }
9028 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9029 {
9030 IkReal evalcond[8];
9031 IkReal x562=IKsin(j5);
9032 IkReal x563=IKcos(j5);
9033 IkReal x564=((1.0)*x562);
9034 IkReal x565=((1.0)*x563);
9035 evalcond[0]=(((new_r02*x562))+new_r21);
9036 evalcond[1]=((((-1.0)*new_r02*x565))+new_r20);
9037 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x564)));
9038 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x565)));
9039 evalcond[4]=((((-1.0)*new_r01))+((cj4*x562)));
9040 evalcond[5]=((((-1.0)*cj4*x565))+(((-1.0)*new_r00)));
9041 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x563);
9042 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x564)));
9043 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
9044 {
9045 continue;
9046 }
9047 }
9048 
9049 {
9050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9051 vinfos[0].jointtype = 1;
9052 vinfos[0].foffset = j0;
9053 vinfos[0].indices[0] = _ij0[0];
9054 vinfos[0].indices[1] = _ij0[1];
9055 vinfos[0].maxsolutions = _nj0;
9056 vinfos[1].jointtype = 1;
9057 vinfos[1].foffset = j1;
9058 vinfos[1].indices[0] = _ij1[0];
9059 vinfos[1].indices[1] = _ij1[1];
9060 vinfos[1].maxsolutions = _nj1;
9061 vinfos[2].jointtype = 1;
9062 vinfos[2].foffset = j2;
9063 vinfos[2].indices[0] = _ij2[0];
9064 vinfos[2].indices[1] = _ij2[1];
9065 vinfos[2].maxsolutions = _nj2;
9066 vinfos[3].jointtype = 1;
9067 vinfos[3].foffset = j3;
9068 vinfos[3].indices[0] = _ij3[0];
9069 vinfos[3].indices[1] = _ij3[1];
9070 vinfos[3].maxsolutions = _nj3;
9071 vinfos[4].jointtype = 1;
9072 vinfos[4].foffset = j4;
9073 vinfos[4].indices[0] = _ij4[0];
9074 vinfos[4].indices[1] = _ij4[1];
9075 vinfos[4].maxsolutions = _nj4;
9076 vinfos[5].jointtype = 1;
9077 vinfos[5].foffset = j5;
9078 vinfos[5].indices[0] = _ij5[0];
9079 vinfos[5].indices[1] = _ij5[1];
9080 vinfos[5].maxsolutions = _nj5;
9081 std::vector<int> vfree(0);
9082 solutions.AddSolution(vinfos,vfree);
9083 }
9084 }
9085 }
9086 
9087 }
9088 
9089 }
9090 
9091 } else
9092 {
9093 {
9094 IkReal j5array[1], cj5array[1], sj5array[1];
9095 bool j5valid[1]={false};
9096 _nj5 = 1;
9097 CheckValue<IkReal> x566 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
9098 if(!x566.valid){
9099 continue;
9100 }
9101 CheckValue<IkReal> x567=IKPowWithIntegerCheck(IKsign(new_r02),-1);
9102 if(!x567.valid){
9103 continue;
9104 }
9105 j5array[0]=((-1.5707963267949)+(x566.value)+(((1.5707963267949)*(x567.value))));
9106 sj5array[0]=IKsin(j5array[0]);
9107 cj5array[0]=IKcos(j5array[0]);
9108 if( j5array[0] > IKPI )
9109 {
9110  j5array[0]-=IK2PI;
9111 }
9112 else if( j5array[0] < -IKPI )
9113 { j5array[0]+=IK2PI;
9114 }
9115 j5valid[0] = true;
9116 for(int ij5 = 0; ij5 < 1; ++ij5)
9117 {
9118 if( !j5valid[ij5] )
9119 {
9120  continue;
9121 }
9122 _ij5[0] = ij5; _ij5[1] = -1;
9123 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
9124 {
9125 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9126 {
9127  j5valid[iij5]=false; _ij5[1] = iij5; break;
9128 }
9129 }
9130 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9131 {
9132 IkReal evalcond[8];
9133 IkReal x568=IKsin(j5);
9134 IkReal x569=IKcos(j5);
9135 IkReal x570=((1.0)*x568);
9136 IkReal x571=((1.0)*x569);
9137 evalcond[0]=(((new_r02*x568))+new_r21);
9138 evalcond[1]=((((-1.0)*new_r02*x571))+new_r20);
9139 evalcond[2]=((((-1.0)*x570))+(((-1.0)*new_r10)));
9140 evalcond[3]=((((-1.0)*x571))+(((-1.0)*new_r11)));
9141 evalcond[4]=((((-1.0)*new_r01))+((cj4*x568)));
9142 evalcond[5]=((((-1.0)*cj4*x571))+(((-1.0)*new_r00)));
9143 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+x569);
9144 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x570))+((new_r21*sj4)));
9145 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
9146 {
9147 continue;
9148 }
9149 }
9150 
9151 {
9152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9153 vinfos[0].jointtype = 1;
9154 vinfos[0].foffset = j0;
9155 vinfos[0].indices[0] = _ij0[0];
9156 vinfos[0].indices[1] = _ij0[1];
9157 vinfos[0].maxsolutions = _nj0;
9158 vinfos[1].jointtype = 1;
9159 vinfos[1].foffset = j1;
9160 vinfos[1].indices[0] = _ij1[0];
9161 vinfos[1].indices[1] = _ij1[1];
9162 vinfos[1].maxsolutions = _nj1;
9163 vinfos[2].jointtype = 1;
9164 vinfos[2].foffset = j2;
9165 vinfos[2].indices[0] = _ij2[0];
9166 vinfos[2].indices[1] = _ij2[1];
9167 vinfos[2].maxsolutions = _nj2;
9168 vinfos[3].jointtype = 1;
9169 vinfos[3].foffset = j3;
9170 vinfos[3].indices[0] = _ij3[0];
9171 vinfos[3].indices[1] = _ij3[1];
9172 vinfos[3].maxsolutions = _nj3;
9173 vinfos[4].jointtype = 1;
9174 vinfos[4].foffset = j4;
9175 vinfos[4].indices[0] = _ij4[0];
9176 vinfos[4].indices[1] = _ij4[1];
9177 vinfos[4].maxsolutions = _nj4;
9178 vinfos[5].jointtype = 1;
9179 vinfos[5].foffset = j5;
9180 vinfos[5].indices[0] = _ij5[0];
9181 vinfos[5].indices[1] = _ij5[1];
9182 vinfos[5].maxsolutions = _nj5;
9183 std::vector<int> vfree(0);
9184 solutions.AddSolution(vinfos,vfree);
9185 }
9186 }
9187 }
9188 
9189 }
9190 
9191 }
9192 
9193 }
9194 } while(0);
9195 if( bgotonextstatement )
9196 {
9197 bool bgotonextstatement = true;
9198 do
9199 {
9200 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
9201 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9202 {
9203 bgotonextstatement=false;
9204 {
9205 IkReal j5eval[1];
9206 new_r21=0;
9207 new_r20=0;
9208 new_r02=0;
9209 new_r12=0;
9210 j5eval[0]=1.0;
9211 if( IKabs(j5eval[0]) < 0.0000000100000000 )
9212 {
9213 continue; // no branches [j5]
9214 
9215 } else
9216 {
9217 IkReal op[2+1], zeror[2];
9218 int numroots;
9219 op[0]=-1.0;
9220 op[1]=0;
9221 op[2]=1.0;
9222 polyroots2(op,zeror,numroots);
9223 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
9224 int numsolutions = 0;
9225 for(int ij5 = 0; ij5 < numroots; ++ij5)
9226 {
9227 IkReal htj5 = zeror[ij5];
9228 tempj5array[0]=((2.0)*(atan(htj5)));
9229 for(int kj5 = 0; kj5 < 1; ++kj5)
9230 {
9231 j5array[numsolutions] = tempj5array[kj5];
9232 if( j5array[numsolutions] > IKPI )
9233 {
9234  j5array[numsolutions]-=IK2PI;
9235 }
9236 else if( j5array[numsolutions] < -IKPI )
9237 {
9238  j5array[numsolutions]+=IK2PI;
9239 }
9240 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
9241 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
9242 numsolutions++;
9243 }
9244 }
9245 bool j5valid[2]={true,true};
9246 _nj5 = 2;
9247 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
9248  {
9249 if( !j5valid[ij5] )
9250 {
9251  continue;
9252 }
9253  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9254 htj5 = IKtan(j5/2);
9255 
9256 _ij5[0] = ij5; _ij5[1] = -1;
9257 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
9258 {
9259 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9260 {
9261  j5valid[iij5]=false; _ij5[1] = iij5; break;
9262 }
9263 }
9264 {
9265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9266 vinfos[0].jointtype = 1;
9267 vinfos[0].foffset = j0;
9268 vinfos[0].indices[0] = _ij0[0];
9269 vinfos[0].indices[1] = _ij0[1];
9270 vinfos[0].maxsolutions = _nj0;
9271 vinfos[1].jointtype = 1;
9272 vinfos[1].foffset = j1;
9273 vinfos[1].indices[0] = _ij1[0];
9274 vinfos[1].indices[1] = _ij1[1];
9275 vinfos[1].maxsolutions = _nj1;
9276 vinfos[2].jointtype = 1;
9277 vinfos[2].foffset = j2;
9278 vinfos[2].indices[0] = _ij2[0];
9279 vinfos[2].indices[1] = _ij2[1];
9280 vinfos[2].maxsolutions = _nj2;
9281 vinfos[3].jointtype = 1;
9282 vinfos[3].foffset = j3;
9283 vinfos[3].indices[0] = _ij3[0];
9284 vinfos[3].indices[1] = _ij3[1];
9285 vinfos[3].maxsolutions = _nj3;
9286 vinfos[4].jointtype = 1;
9287 vinfos[4].foffset = j4;
9288 vinfos[4].indices[0] = _ij4[0];
9289 vinfos[4].indices[1] = _ij4[1];
9290 vinfos[4].maxsolutions = _nj4;
9291 vinfos[5].jointtype = 1;
9292 vinfos[5].foffset = j5;
9293 vinfos[5].indices[0] = _ij5[0];
9294 vinfos[5].indices[1] = _ij5[1];
9295 vinfos[5].maxsolutions = _nj5;
9296 std::vector<int> vfree(0);
9297 solutions.AddSolution(vinfos,vfree);
9298 }
9299  }
9300 
9301 }
9302 
9303 }
9304 
9305 }
9306 } while(0);
9307 if( bgotonextstatement )
9308 {
9309 bool bgotonextstatement = true;
9310 do
9311 {
9312 if( 1 )
9313 {
9314 bgotonextstatement=false;
9315 continue; // branch miss [j5]
9316 
9317 }
9318 } while(0);
9319 if( bgotonextstatement )
9320 {
9321 }
9322 }
9323 }
9324 }
9325 }
9326 }
9327 }
9328 }
9329 }
9330 }
9331 }
9332 
9333 } else
9334 {
9335 {
9336 IkReal j5array[1], cj5array[1], sj5array[1];
9337 bool j5valid[1]={false};
9338 _nj5 = 1;
9339 CheckValue<IkReal> x573=IKPowWithIntegerCheck(sj4,-1);
9340 if(!x573.valid){
9341 continue;
9342 }
9343 IkReal x572=x573.value;
9344 CheckValue<IkReal> x574=IKPowWithIntegerCheck(cj3,-1);
9345 if(!x574.valid){
9346 continue;
9347 }
9348 CheckValue<IkReal> x575=IKPowWithIntegerCheck(cj4,-1);
9349 if(!x575.valid){
9350 continue;
9351 }
9352 if( IKabs((x572*(x574.value)*(x575.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x572)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x572*(x574.value)*(x575.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4))))))+IKsqr(((-1.0)*new_r20*x572))-1) <= IKFAST_SINCOS_THRESH )
9353  continue;
9354 j5array[0]=IKatan2((x572*(x574.value)*(x575.value)*((((new_r20*sj3))+(((-1.0)*new_r01*sj4))))), ((-1.0)*new_r20*x572));
9355 sj5array[0]=IKsin(j5array[0]);
9356 cj5array[0]=IKcos(j5array[0]);
9357 if( j5array[0] > IKPI )
9358 {
9359  j5array[0]-=IK2PI;
9360 }
9361 else if( j5array[0] < -IKPI )
9362 { j5array[0]+=IK2PI;
9363 }
9364 j5valid[0] = true;
9365 for(int ij5 = 0; ij5 < 1; ++ij5)
9366 {
9367 if( !j5valid[ij5] )
9368 {
9369  continue;
9370 }
9371 _ij5[0] = ij5; _ij5[1] = -1;
9372 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
9373 {
9374 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9375 {
9376  j5valid[iij5]=false; _ij5[1] = iij5; break;
9377 }
9378 }
9379 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9380 {
9381 IkReal evalcond[12];
9382 IkReal x576=IKsin(j5);
9383 IkReal x577=IKcos(j5);
9384 IkReal x578=(cj3*new_r00);
9385 IkReal x579=(cj3*cj4);
9386 IkReal x580=((1.0)*sj3);
9387 IkReal x581=((1.0)*x576);
9388 IkReal x582=(sj3*x576);
9389 IkReal x583=((1.0)*x577);
9390 evalcond[0]=(((sj4*x577))+new_r20);
9391 evalcond[1]=((((-1.0)*sj4*x581))+new_r21);
9392 evalcond[2]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x576)));
9393 evalcond[3]=(((cj3*new_r10))+(((-1.0)*x581))+(((-1.0)*new_r00*x580)));
9394 evalcond[4]=((((-1.0)*new_r01*x580))+((cj3*new_r11))+(((-1.0)*x583)));
9395 evalcond[5]=(((x576*x579))+((sj3*x577))+new_r01);
9396 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x583))+x578);
9397 evalcond[7]=((((-1.0)*x579*x583))+x582+new_r00);
9398 evalcond[8]=(((cj4*x582))+new_r11+(((-1.0)*cj3*x583)));
9399 evalcond[9]=((((-1.0)*cj4*x577*x580))+new_r10+(((-1.0)*cj3*x581)));
9400 evalcond[10]=(((new_r20*sj4))+x577+(((-1.0)*cj4*x578))+(((-1.0)*cj4*new_r10*x580)));
9401 evalcond[11]=((((-1.0)*cj4*new_r11*x580))+((new_r21*sj4))+(((-1.0)*x581))+(((-1.0)*new_r01*x579)));
9402 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
9403 {
9404 continue;
9405 }
9406 }
9407 
9408 {
9409 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9410 vinfos[0].jointtype = 1;
9411 vinfos[0].foffset = j0;
9412 vinfos[0].indices[0] = _ij0[0];
9413 vinfos[0].indices[1] = _ij0[1];
9414 vinfos[0].maxsolutions = _nj0;
9415 vinfos[1].jointtype = 1;
9416 vinfos[1].foffset = j1;
9417 vinfos[1].indices[0] = _ij1[0];
9418 vinfos[1].indices[1] = _ij1[1];
9419 vinfos[1].maxsolutions = _nj1;
9420 vinfos[2].jointtype = 1;
9421 vinfos[2].foffset = j2;
9422 vinfos[2].indices[0] = _ij2[0];
9423 vinfos[2].indices[1] = _ij2[1];
9424 vinfos[2].maxsolutions = _nj2;
9425 vinfos[3].jointtype = 1;
9426 vinfos[3].foffset = j3;
9427 vinfos[3].indices[0] = _ij3[0];
9428 vinfos[3].indices[1] = _ij3[1];
9429 vinfos[3].maxsolutions = _nj3;
9430 vinfos[4].jointtype = 1;
9431 vinfos[4].foffset = j4;
9432 vinfos[4].indices[0] = _ij4[0];
9433 vinfos[4].indices[1] = _ij4[1];
9434 vinfos[4].maxsolutions = _nj4;
9435 vinfos[5].jointtype = 1;
9436 vinfos[5].foffset = j5;
9437 vinfos[5].indices[0] = _ij5[0];
9438 vinfos[5].indices[1] = _ij5[1];
9439 vinfos[5].maxsolutions = _nj5;
9440 std::vector<int> vfree(0);
9441 solutions.AddSolution(vinfos,vfree);
9442 }
9443 }
9444 }
9445 
9446 }
9447 
9448 }
9449 
9450 } else
9451 {
9452 {
9453 IkReal j5array[1], cj5array[1], sj5array[1];
9454 bool j5valid[1]={false};
9455 _nj5 = 1;
9456 CheckValue<IkReal> x585=IKPowWithIntegerCheck(sj4,-1);
9457 if(!x585.valid){
9458 continue;
9459 }
9460 IkReal x584=x585.value;
9461 CheckValue<IkReal> x586=IKPowWithIntegerCheck(sj3,-1);
9462 if(!x586.valid){
9463 continue;
9464 }
9465 if( IKabs((x584*(x586.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r20*x584)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x584*(x586.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4))))))+IKsqr(((-1.0)*new_r20*x584))-1) <= IKFAST_SINCOS_THRESH )
9466  continue;
9467 j5array[0]=IKatan2((x584*(x586.value)*(((((-1.0)*cj3*cj4*new_r20))+(((-1.0)*new_r00*sj4))))), ((-1.0)*new_r20*x584));
9468 sj5array[0]=IKsin(j5array[0]);
9469 cj5array[0]=IKcos(j5array[0]);
9470 if( j5array[0] > IKPI )
9471 {
9472  j5array[0]-=IK2PI;
9473 }
9474 else if( j5array[0] < -IKPI )
9475 { j5array[0]+=IK2PI;
9476 }
9477 j5valid[0] = true;
9478 for(int ij5 = 0; ij5 < 1; ++ij5)
9479 {
9480 if( !j5valid[ij5] )
9481 {
9482  continue;
9483 }
9484 _ij5[0] = ij5; _ij5[1] = -1;
9485 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
9486 {
9487 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9488 {
9489  j5valid[iij5]=false; _ij5[1] = iij5; break;
9490 }
9491 }
9492 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9493 {
9494 IkReal evalcond[12];
9495 IkReal x587=IKsin(j5);
9496 IkReal x588=IKcos(j5);
9497 IkReal x589=(cj3*new_r00);
9498 IkReal x590=(cj3*cj4);
9499 IkReal x591=((1.0)*sj3);
9500 IkReal x592=((1.0)*x587);
9501 IkReal x593=(sj3*x587);
9502 IkReal x594=((1.0)*x588);
9503 evalcond[0]=(((sj4*x588))+new_r20);
9504 evalcond[1]=((((-1.0)*sj4*x592))+new_r21);
9505 evalcond[2]=(((new_r11*sj3))+((cj4*x587))+((cj3*new_r01)));
9506 evalcond[3]=((((-1.0)*new_r00*x591))+(((-1.0)*x592))+((cj3*new_r10)));
9507 evalcond[4]=((((-1.0)*new_r01*x591))+(((-1.0)*x594))+((cj3*new_r11)));
9508 evalcond[5]=(((sj3*x588))+new_r01+((x587*x590)));
9509 evalcond[6]=((((-1.0)*cj4*x594))+((new_r10*sj3))+x589);
9510 evalcond[7]=((((-1.0)*x590*x594))+x593+new_r00);
9511 evalcond[8]=((((-1.0)*cj3*x594))+new_r11+((cj4*x593)));
9512 evalcond[9]=((((-1.0)*cj4*x588*x591))+(((-1.0)*cj3*x592))+new_r10);
9513 evalcond[10]=((((-1.0)*cj4*new_r10*x591))+((new_r20*sj4))+x588+(((-1.0)*cj4*x589)));
9514 evalcond[11]=((((-1.0)*cj4*new_r11*x591))+(((-1.0)*x592))+((new_r21*sj4))+(((-1.0)*new_r01*x590)));
9515 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
9516 {
9517 continue;
9518 }
9519 }
9520 
9521 {
9522 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9523 vinfos[0].jointtype = 1;
9524 vinfos[0].foffset = j0;
9525 vinfos[0].indices[0] = _ij0[0];
9526 vinfos[0].indices[1] = _ij0[1];
9527 vinfos[0].maxsolutions = _nj0;
9528 vinfos[1].jointtype = 1;
9529 vinfos[1].foffset = j1;
9530 vinfos[1].indices[0] = _ij1[0];
9531 vinfos[1].indices[1] = _ij1[1];
9532 vinfos[1].maxsolutions = _nj1;
9533 vinfos[2].jointtype = 1;
9534 vinfos[2].foffset = j2;
9535 vinfos[2].indices[0] = _ij2[0];
9536 vinfos[2].indices[1] = _ij2[1];
9537 vinfos[2].maxsolutions = _nj2;
9538 vinfos[3].jointtype = 1;
9539 vinfos[3].foffset = j3;
9540 vinfos[3].indices[0] = _ij3[0];
9541 vinfos[3].indices[1] = _ij3[1];
9542 vinfos[3].maxsolutions = _nj3;
9543 vinfos[4].jointtype = 1;
9544 vinfos[4].foffset = j4;
9545 vinfos[4].indices[0] = _ij4[0];
9546 vinfos[4].indices[1] = _ij4[1];
9547 vinfos[4].maxsolutions = _nj4;
9548 vinfos[5].jointtype = 1;
9549 vinfos[5].foffset = j5;
9550 vinfos[5].indices[0] = _ij5[0];
9551 vinfos[5].indices[1] = _ij5[1];
9552 vinfos[5].maxsolutions = _nj5;
9553 std::vector<int> vfree(0);
9554 solutions.AddSolution(vinfos,vfree);
9555 }
9556 }
9557 }
9558 
9559 }
9560 
9561 }
9562 
9563 } else
9564 {
9565 {
9566 IkReal j5array[1], cj5array[1], sj5array[1];
9567 bool j5valid[1]={false};
9568 _nj5 = 1;
9569 CheckValue<IkReal> x595=IKPowWithIntegerCheck(IKsign(sj4),-1);
9570 if(!x595.valid){
9571 continue;
9572 }
9573 CheckValue<IkReal> x596 = IKatan2WithCheck(IkReal(new_r21),IkReal(((-1.0)*new_r20)),IKFAST_ATAN2_MAGTHRESH);
9574 if(!x596.valid){
9575 continue;
9576 }
9577 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x595.value)))+(x596.value));
9578 sj5array[0]=IKsin(j5array[0]);
9579 cj5array[0]=IKcos(j5array[0]);
9580 if( j5array[0] > IKPI )
9581 {
9582  j5array[0]-=IK2PI;
9583 }
9584 else if( j5array[0] < -IKPI )
9585 { j5array[0]+=IK2PI;
9586 }
9587 j5valid[0] = true;
9588 for(int ij5 = 0; ij5 < 1; ++ij5)
9589 {
9590 if( !j5valid[ij5] )
9591 {
9592  continue;
9593 }
9594 _ij5[0] = ij5; _ij5[1] = -1;
9595 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
9596 {
9597 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
9598 {
9599  j5valid[iij5]=false; _ij5[1] = iij5; break;
9600 }
9601 }
9602 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
9603 {
9604 IkReal evalcond[12];
9605 IkReal x597=IKsin(j5);
9606 IkReal x598=IKcos(j5);
9607 IkReal x599=(cj3*new_r00);
9608 IkReal x600=(cj3*cj4);
9609 IkReal x601=((1.0)*sj3);
9610 IkReal x602=((1.0)*x597);
9611 IkReal x603=(sj3*x597);
9612 IkReal x604=((1.0)*x598);
9613 evalcond[0]=(((sj4*x598))+new_r20);
9614 evalcond[1]=((((-1.0)*sj4*x602))+new_r21);
9615 evalcond[2]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x597)));
9616 evalcond[3]=(((cj3*new_r10))+(((-1.0)*x602))+(((-1.0)*new_r00*x601)));
9617 evalcond[4]=(((cj3*new_r11))+(((-1.0)*x604))+(((-1.0)*new_r01*x601)));
9618 evalcond[5]=(((x597*x600))+new_r01+((sj3*x598)));
9619 evalcond[6]=(((new_r10*sj3))+x599+(((-1.0)*cj4*x604)));
9620 evalcond[7]=((((-1.0)*x600*x604))+x603+new_r00);
9621 evalcond[8]=((((-1.0)*cj3*x604))+new_r11+((cj4*x603)));
9622 evalcond[9]=((((-1.0)*cj3*x602))+(((-1.0)*cj4*x598*x601))+new_r10);
9623 evalcond[10]=(((new_r20*sj4))+(((-1.0)*cj4*new_r10*x601))+x598+(((-1.0)*cj4*x599)));
9624 evalcond[11]=((((-1.0)*new_r01*x600))+(((-1.0)*x602))+((new_r21*sj4))+(((-1.0)*cj4*new_r11*x601)));
9625 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
9626 {
9627 continue;
9628 }
9629 }
9630 
9631 {
9632 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9633 vinfos[0].jointtype = 1;
9634 vinfos[0].foffset = j0;
9635 vinfos[0].indices[0] = _ij0[0];
9636 vinfos[0].indices[1] = _ij0[1];
9637 vinfos[0].maxsolutions = _nj0;
9638 vinfos[1].jointtype = 1;
9639 vinfos[1].foffset = j1;
9640 vinfos[1].indices[0] = _ij1[0];
9641 vinfos[1].indices[1] = _ij1[1];
9642 vinfos[1].maxsolutions = _nj1;
9643 vinfos[2].jointtype = 1;
9644 vinfos[2].foffset = j2;
9645 vinfos[2].indices[0] = _ij2[0];
9646 vinfos[2].indices[1] = _ij2[1];
9647 vinfos[2].maxsolutions = _nj2;
9648 vinfos[3].jointtype = 1;
9649 vinfos[3].foffset = j3;
9650 vinfos[3].indices[0] = _ij3[0];
9651 vinfos[3].indices[1] = _ij3[1];
9652 vinfos[3].maxsolutions = _nj3;
9653 vinfos[4].jointtype = 1;
9654 vinfos[4].foffset = j4;
9655 vinfos[4].indices[0] = _ij4[0];
9656 vinfos[4].indices[1] = _ij4[1];
9657 vinfos[4].maxsolutions = _nj4;
9658 vinfos[5].jointtype = 1;
9659 vinfos[5].foffset = j5;
9660 vinfos[5].indices[0] = _ij5[0];
9661 vinfos[5].indices[1] = _ij5[1];
9662 vinfos[5].maxsolutions = _nj5;
9663 std::vector<int> vfree(0);
9664 solutions.AddSolution(vinfos,vfree);
9665 }
9666 }
9667 }
9668 
9669 }
9670 
9671 }
9672 }
9673 }
9674 
9675 }
9676 
9677 }
9678 }
9679 }
9680 }
9681 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
9682 {
9683  using std::complex;
9684  if( rawcoeffs[0] == 0 ) {
9685  // solve with one reduced degree
9686  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
9687  return;
9688  }
9689  IKFAST_ASSERT(rawcoeffs[0] != 0);
9690  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
9691  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
9692  complex<IkReal> coeffs[3];
9693  const int maxsteps = 110;
9694  for(int i = 0; i < 3; ++i) {
9695  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
9696  }
9697  complex<IkReal> roots[3];
9698  IkReal err[3];
9699  roots[0] = complex<IkReal>(1,0);
9700  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
9701  err[0] = 1.0;
9702  err[1] = 1.0;
9703  for(int i = 2; i < 3; ++i) {
9704  roots[i] = roots[i-1]*roots[1];
9705  err[i] = 1.0;
9706  }
9707  for(int step = 0; step < maxsteps; ++step) {
9708  bool changed = false;
9709  for(int i = 0; i < 3; ++i) {
9710  if ( err[i] >= tol ) {
9711  changed = true;
9712  // evaluate
9713  complex<IkReal> x = roots[i] + coeffs[0];
9714  for(int j = 1; j < 3; ++j) {
9715  x = roots[i] * x + coeffs[j];
9716  }
9717  for(int j = 0; j < 3; ++j) {
9718  if( i != j ) {
9719  if( roots[i] != roots[j] ) {
9720  x /= (roots[i] - roots[j]);
9721  }
9722  }
9723  }
9724  roots[i] -= x;
9725  err[i] = abs(x);
9726  }
9727  }
9728  if( !changed ) {
9729  break;
9730  }
9731  }
9732 
9733  numroots = 0;
9734  bool visited[3] = {false};
9735  for(int i = 0; i < 3; ++i) {
9736  if( !visited[i] ) {
9737  // might be a multiple root, in which case it will have more error than the other roots
9738  // find any neighboring roots, and take the average
9739  complex<IkReal> newroot=roots[i];
9740  int n = 1;
9741  for(int j = i+1; j < 3; ++j) {
9742  // care about error in real much more than imaginary
9743  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
9744  newroot += roots[j];
9745  n += 1;
9746  visited[j] = true;
9747  }
9748  }
9749  if( n > 1 ) {
9750  newroot /= n;
9751  }
9752  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
9753  if( IKabs(imag(newroot)) < tolsqrt ) {
9754  rawroots[numroots++] = real(newroot);
9755  }
9756  }
9757  }
9758 }
9759 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
9760  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
9761  if( det < 0 ) {
9762  numroots=0;
9763  }
9764  else if( det == 0 ) {
9765  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
9766  numroots = 1;
9767  }
9768  else {
9769  det = IKsqrt(det);
9770  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
9771  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
9772  numroots = 2;
9773  }
9774 }
9775 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
9776 {
9777  using std::complex;
9778  if( rawcoeffs[0] == 0 ) {
9779  // solve with one reduced degree
9780  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
9781  return;
9782  }
9783  IKFAST_ASSERT(rawcoeffs[0] != 0);
9784  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
9785  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
9786  complex<IkReal> coeffs[4];
9787  const int maxsteps = 110;
9788  for(int i = 0; i < 4; ++i) {
9789  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
9790  }
9791  complex<IkReal> roots[4];
9792  IkReal err[4];
9793  roots[0] = complex<IkReal>(1,0);
9794  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
9795  err[0] = 1.0;
9796  err[1] = 1.0;
9797  for(int i = 2; i < 4; ++i) {
9798  roots[i] = roots[i-1]*roots[1];
9799  err[i] = 1.0;
9800  }
9801  for(int step = 0; step < maxsteps; ++step) {
9802  bool changed = false;
9803  for(int i = 0; i < 4; ++i) {
9804  if ( err[i] >= tol ) {
9805  changed = true;
9806  // evaluate
9807  complex<IkReal> x = roots[i] + coeffs[0];
9808  for(int j = 1; j < 4; ++j) {
9809  x = roots[i] * x + coeffs[j];
9810  }
9811  for(int j = 0; j < 4; ++j) {
9812  if( i != j ) {
9813  if( roots[i] != roots[j] ) {
9814  x /= (roots[i] - roots[j]);
9815  }
9816  }
9817  }
9818  roots[i] -= x;
9819  err[i] = abs(x);
9820  }
9821  }
9822  if( !changed ) {
9823  break;
9824  }
9825  }
9826 
9827  numroots = 0;
9828  bool visited[4] = {false};
9829  for(int i = 0; i < 4; ++i) {
9830  if( !visited[i] ) {
9831  // might be a multiple root, in which case it will have more error than the other roots
9832  // find any neighboring roots, and take the average
9833  complex<IkReal> newroot=roots[i];
9834  int n = 1;
9835  for(int j = i+1; j < 4; ++j) {
9836  // care about error in real much more than imaginary
9837  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
9838  newroot += roots[j];
9839  n += 1;
9840  visited[j] = true;
9841  }
9842  }
9843  if( n > 1 ) {
9844  newroot /= n;
9845  }
9846  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
9847  if( IKabs(imag(newroot)) < tolsqrt ) {
9848  rawroots[numroots++] = real(newroot);
9849  }
9850  }
9851  }
9852 }
9853 };
9854 
9855 
9858 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
9859 IKSolver solver;
9860 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
9861 }
9862 
9863 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
9864 IKSolver solver;
9865 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
9866 }
9867 
9868 IKFAST_API const char* GetKinematicsHash() { return "c7adf74e994bc2268228960d140bc863"; }
9869 
9870 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
9871 
9872 #ifdef IKFAST_NAMESPACE
9873 } // end namespace
9874 #endif
9875 
9876 #ifndef IKFAST_NO_MAIN
9877 #include <stdio.h>
9878 #include <stdlib.h>
9879 #ifdef IKFAST_NAMESPACE
9880 using namespace IKFAST_NAMESPACE;
9881 #endif
9882 int main(int argc, char** argv)
9883 {
9884  if( argc != 12+GetNumFreeParameters()+1 ) {
9885  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
9886  "Returns the ik solutions given the transformation of the end effector specified by\n"
9887  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
9888  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
9889  return 1;
9890  }
9891 
9892  IkSolutionList<IkReal> solutions;
9893  std::vector<IkReal> vfree(GetNumFreeParameters());
9894  IkReal eerot[9],eetrans[3];
9895  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
9896  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
9897  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
9898  for(std::size_t i = 0; i < vfree.size(); ++i)
9899  vfree[i] = atof(argv[13+i]);
9900  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
9901 
9902  if( !bSuccess ) {
9903  fprintf(stderr,"Failed to get ik solution\n");
9904  return -1;
9905  }
9906 
9907  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
9908  std::vector<IkReal> solvalues(GetNumJoints());
9909  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
9910  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
9911  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
9912  std::vector<IkReal> vsolfree(sol.GetFree().size());
9913  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
9914  for( std::size_t j = 0; j < solvalues.size(); ++j)
9915  printf("%.15f, ", solvalues[j]);
9916  printf("\n");
9917  }
9918  return 0;
9919 }
9920 
9921 #endif
ikfast
Definition: ikfast.h:45
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:108
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9870
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKPI_2
#define IKPI_2
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:62
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:113
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:247
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:369
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
IKacos
float IKacos(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:155
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:300
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:103
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:367
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
IKatan2
float IKatan2(float fy, float fx)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:180
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:822
IKasin
float IKasin(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:122
IKPI
#define IKPI
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:61
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:26
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:366
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:50
main
int main(int argc, char **argv)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9882
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9681
GetIkType
IKFAST_API int GetIkType()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:371
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9858
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
IK2PI
#define IK2PI
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:60
IKsin
float IKsin(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:169
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:177
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9868
IKsign
float IKsign(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:226
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9863
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:365
IKfmod
float IKfmod(float x, float y)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:138
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:380
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9775
step
unsigned int step
IKsqrt
float IKsqrt(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:175
IKcos
float IKcos(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:171
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
IKsqr
float IKsqr(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:95
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:212
IKtan
float IKtan(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:173
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:9759
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:118
dgetrs_
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKlog
float IKlog(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:98
x
double x
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
IKabs
float IKabs(float f)
Definition: kr6_r900_workspace_manipulator_ikfast_solver.hpp:92

kr6_r900_workspace_ikfast_manipulator_plugin
Author(s):
autogenerated on Thu Jul 18 2019 04:03:29