fanuc_lrmate200id7l_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
23 
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
27 
28 #include <cmath>
29 #include <vector>
30 #include <limits>
31 #include <algorithm>
32 #include <complex>
33 
34 #ifndef IKFAST_ASSERT
35 #include <stdexcept>
36 #include <sstream>
37 #include <iostream>
38 
39 #ifdef _MSC_VER
40 #ifndef __PRETTY_FUNCTION__
41 #define __PRETTY_FUNCTION__ __FUNCDNAME__
42 #endif
43 #endif
44 
45 #ifndef __PRETTY_FUNCTION__
46 #define __PRETTY_FUNCTION__ __func__
47 #endif
48 
49 #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()); } }
50 
51 #endif
52 
53 #if defined(_MSC_VER)
54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
55 #else
56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
57 #endif
58 
59 #define IK2PI ((IkReal)6.28318530717959)
60 #define IKPI ((IkReal)3.14159265358979)
61 #define IKPI_2 ((IkReal)1.57079632679490)
62 
63 #ifdef _MSC_VER
64 #ifndef isnan
65 #define isnan _isnan
66 #endif
67 #ifndef isinf
68 #define isinf _isinf
69 #endif
70 //#ifndef isfinite
71 //#define isfinite _isfinite
72 //#endif
73 #endif // _MSC_VER
74 
75 // lapack routines
76 extern "C" {
77  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
78  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
79  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
80  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
81  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);
82  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);
83 }
84 
85 using namespace std; // necessary to get std math routines
86 
87 #ifdef IKFAST_NAMESPACE
88 namespace IKFAST_NAMESPACE {
89 #endif
90 
91 inline float IKabs(float f) { return fabsf(f); }
92 inline double IKabs(double f) { return fabs(f); }
93 
94 inline float IKsqr(float f) { return f*f; }
95 inline double IKsqr(double f) { return f*f; }
96 
97 inline float IKlog(float f) { return logf(f); }
98 inline double IKlog(double f) { return log(f); }
99 
100 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
101 #ifndef IKFAST_SINCOS_THRESH
102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
103 #endif
104 
105 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
106 #ifndef IKFAST_ATAN2_MAGTHRESH
107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
108 #endif
109 
110 // minimum distance of separate solutions
111 #ifndef IKFAST_SOLUTION_THRESH
112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
113 #endif
114 
115 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
116 #ifndef IKFAST_EVALCOND_THRESH
117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
118 #endif
119 
120 
121 inline float IKasin(float f)
122 {
123 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
124 if( f <= -1 ) return float(-IKPI_2);
125 else if( f >= 1 ) return float(IKPI_2);
126 return asinf(f);
127 }
128 inline double IKasin(double f)
129 {
130 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
131 if( f <= -1 ) return -IKPI_2;
132 else if( f >= 1 ) return IKPI_2;
133 return asin(f);
134 }
135 
136 // return positive value in [0,y)
137 inline float IKfmod(float x, float y)
138 {
139  while(x < 0) {
140  x += y;
141  }
142  return fmodf(x,y);
143 }
144 
145 // return positive value in [0,y)
146 inline double IKfmod(double x, double y)
147 {
148  while(x < 0) {
149  x += y;
150  }
151  return fmod(x,y);
152 }
153 
154 inline float IKacos(float f)
155 {
156 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
157 if( f <= -1 ) return float(IKPI);
158 else if( f >= 1 ) return float(0);
159 return acosf(f);
160 }
161 inline double IKacos(double f)
162 {
163 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
164 if( f <= -1 ) return IKPI;
165 else if( f >= 1 ) return 0;
166 return acos(f);
167 }
168 inline float IKsin(float f) { return sinf(f); }
169 inline double IKsin(double f) { return sin(f); }
170 inline float IKcos(float f) { return cosf(f); }
171 inline double IKcos(double f) { return cos(f); }
172 inline float IKtan(float f) { return tanf(f); }
173 inline double IKtan(double f) { return tan(f); }
174 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
175 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
176 inline float IKatan2Simple(float fy, float fx) {
177  return atan2f(fy,fx);
178 }
179 inline float IKatan2(float fy, float fx) {
180  if( isnan(fy) ) {
181  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
182  return float(IKPI_2);
183  }
184  else if( isnan(fx) ) {
185  return 0;
186  }
187  return atan2f(fy,fx);
188 }
189 inline double IKatan2Simple(double fy, double fx) {
190  return atan2(fy,fx);
191 }
192 inline double IKatan2(double fy, double fx) {
193  if( isnan(fy) ) {
194  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
195  return IKPI_2;
196  }
197  else if( isnan(fx) ) {
198  return 0;
199  }
200  return atan2(fy,fx);
201 }
202 
203 template <typename T>
204 struct CheckValue
205 {
206  T value;
207  bool valid;
208 };
209 
210 template <typename T>
211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
212 {
213  CheckValue<T> ret;
214  ret.valid = false;
215  ret.value = 0;
216  if( !isnan(fy) && !isnan(fx) ) {
217  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
218  ret.value = IKatan2Simple(fy,fx);
219  ret.valid = true;
220  }
221  }
222  return ret;
223 }
224 
225 inline float IKsign(float f) {
226  if( f > 0 ) {
227  return float(1);
228  }
229  else if( f < 0 ) {
230  return float(-1);
231  }
232  return 0;
233 }
234 
235 inline double IKsign(double f) {
236  if( f > 0 ) {
237  return 1.0;
238  }
239  else if( f < 0 ) {
240  return -1.0;
241  }
242  return 0;
243 }
244 
245 template <typename T>
246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
247 {
248  CheckValue<T> ret;
249  ret.valid = true;
250  if( n == 0 ) {
251  ret.value = 1.0;
252  return ret;
253  }
254  else if( n == 1 )
255  {
256  ret.value = f;
257  return ret;
258  }
259  else if( n < 0 )
260  {
261  if( f == 0 )
262  {
263  ret.valid = false;
264  ret.value = (T)1.0e30;
265  return ret;
266  }
267  if( n == -1 ) {
268  ret.value = T(1.0)/f;
269  return ret;
270  }
271  }
272 
273  int num = n > 0 ? n : -n;
274  if( num == 2 ) {
275  ret.value = f*f;
276  }
277  else if( num == 3 ) {
278  ret.value = f*f*f;
279  }
280  else {
281  ret.value = 1.0;
282  while(num>0) {
283  if( num & 1 ) {
284  ret.value *= f;
285  }
286  num >>= 1;
287  f *= f;
288  }
289  }
290 
291  if( n < 0 ) {
292  ret.value = T(1.0)/ret.value;
293  }
294  return ret;
295 }
296 
299 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
300 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;
301 x0=IKcos(j[0]);
302 x1=IKcos(j[3]);
303 x2=IKcos(j[2]);
304 x3=IKsin(j[1]);
305 x4=IKsin(j[0]);
306 x5=IKsin(j[3]);
307 x6=IKcos(j[1]);
308 x7=IKsin(j[2]);
309 x8=IKsin(j[5]);
310 x9=IKcos(j[5]);
311 x10=IKcos(j[4]);
312 x11=IKsin(j[4]);
313 x12=((0.08)*x2);
314 x13=((0.42)*x4);
315 x14=((0.035)*x2);
316 x15=((1.0)*x5);
317 x16=((1.0)*x4);
318 x17=((1.0)*x2);
319 x18=((1.0)*x0);
320 x19=((0.42)*x0);
321 x20=((0.08)*x5);
322 x21=((0.08)*x1);
323 x22=(x3*x7);
324 x23=(x2*x6);
325 x24=(x0*x3);
326 x25=(x6*x7);
327 x26=(x3*x4);
328 x27=(x1*x11);
329 x28=(x1*x16);
330 x29=(x17*x3);
331 x30=(x18*x25);
332 x31=(x16*x25);
333 x32=(x22+x23);
334 x33=((((-1.0)*x30))+((x2*x24)));
335 x34=((((-1.0)*x31))+((x2*x26)));
336 x35=((((-1.0)*x17*x24))+x30);
337 x36=((((-1.0)*x16*x2*x3))+x31);
338 x37=(x1*x33);
339 x38=(x1*x34);
340 x39=(x36*x5);
341 x40=(x37+(((-1.0)*x15*x4)));
342 x41=(((x0*x5))+x38);
343 x42=(((x11*(((((-1.0)*x25))+x29))))+((x1*x10*x32)));
344 x43=(((x10*x40))+((x11*(((((-1.0)*x0*x17*x6))+(((-1.0)*x18*x22)))))));
345 IkReal x45=((1.0)*x16);
346 x44=(((x10*x41))+((x11*(((((-1.0)*x22*x45))+(((-1.0)*x23*x45)))))));
347 eerot[0]=(((x8*(((((-1.0)*x28))+((x35*x5))))))+((x43*x9)));
348 eerot[1]=(((x43*x8))+((x9*((x28+(((-1.0)*x15*x35)))))));
349 eerot[2]=(((x11*x40))+((x10*((((x0*x22))+((x0*x23)))))));
350 eetrans[0]=((((-0.035)*x0*x25))+(((0.44)*x24))+((x11*((((x21*x33))+(((-1.0)*x20*x4))))))+((x19*x22))+((x19*x23))+(((0.05)*x0))+((x14*x24))+((x10*(((((0.08)*x0*x22))+((x0*x12*x6)))))));
351 eerot[3]=(((x44*x9))+((x8*((((x0*x1))+x39)))));
352 eerot[4]=(((x9*(((((-1.0)*x1*x18))+(((-1.0)*x15*x36))))))+((x44*x8)));
353 eerot[5]=(((x10*((((x22*x4))+((x23*x4))))))+((x11*x41)));
354 eetrans[1]=(((x10*(((((0.08)*x22*x4))+((x12*x4*x6))))))+(((0.44)*x26))+(((0.05)*x4))+((x14*x26))+(((-0.035)*x25*x4))+((x11*((((x0*x20))+((x21*x34))))))+((x13*x23))+((x13*x22)));
355 eerot[6]=(((x5*x8*(((((-1.0)*x22))+(((-1.0)*x17*x6))))))+((x42*x9)));
356 eerot[7]=(((x42*x8))+((x32*x5*x9)));
357 eerot[8]=(((x27*x32))+((x10*(((((-1.0)*x29))+x25)))));
358 eetrans[2]=((0.33)+((x14*x6))+((x27*(((((0.08)*x22))+((x12*x6))))))+((x10*(((((-1.0)*x12*x3))+(((0.08)*x25))))))+(((0.42)*x25))+(((0.44)*x6))+(((0.035)*x22))+(((-0.42)*x2*x3)));
359 }
360 
361 IKFAST_API int GetNumFreeParameters() { return 0; }
362 IKFAST_API int* GetFreeParameters() { return NULL; }
363 IKFAST_API int GetNumJoints() { return 6; }
364 
365 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
366 
367 IKFAST_API int GetIkType() { return 0x67000001; }
368 
369 class IKSolver {
370 public:
371 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;
372 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
373 
374 IkReal j100, cj100, sj100;
375 unsigned char _ij100[2], _nj100;
376 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
377 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;
378 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
379  solutions.Clear();
380 r00 = eerot[0*3+0];
381 r01 = eerot[0*3+1];
382 r02 = eerot[0*3+2];
383 r10 = eerot[1*3+0];
384 r11 = eerot[1*3+1];
385 r12 = eerot[1*3+2];
386 r20 = eerot[2*3+0];
387 r21 = eerot[2*3+1];
388 r22 = eerot[2*3+2];
389 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
390 
391 new_r00=r00;
392 new_r01=((-1.0)*r01);
393 new_r02=((-1.0)*r02);
394 new_px=((((-0.08)*r02))+px);
395 new_r10=r10;
396 new_r11=((-1.0)*r11);
397 new_r12=((-1.0)*r12);
398 new_py=((((-0.08)*r12))+py);
399 new_r20=r20;
400 new_r21=((-1.0)*r21);
401 new_r22=((-1.0)*r22);
402 new_pz=((-0.33)+(((-0.08)*r22))+pz);
403 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;
404 IkReal x46=((1.0)*px);
405 IkReal x47=((1.0)*pz);
406 IkReal x48=((1.0)*py);
407 pp=((px*px)+(py*py)+(pz*pz));
408 npx=(((px*r00))+((py*r10))+((pz*r20)));
409 npy=(((px*r01))+((py*r11))+((pz*r21)));
410 npz=(((px*r02))+((py*r12))+((pz*r22)));
411 rxp0_0=((((-1.0)*r20*x48))+((pz*r10)));
412 rxp0_1=(((px*r20))+(((-1.0)*r00*x47)));
413 rxp0_2=((((-1.0)*r10*x46))+((py*r00)));
414 rxp1_0=((((-1.0)*r21*x48))+((pz*r11)));
415 rxp1_1=(((px*r21))+(((-1.0)*r01*x47)));
416 rxp1_2=((((-1.0)*r11*x46))+((py*r01)));
417 rxp2_0=((((-1.0)*r22*x48))+((pz*r12)));
418 rxp2_1=((((-1.0)*r02*x47))+((px*r22)));
419 rxp2_2=((((-1.0)*r12*x46))+((py*r02)));
420 {
421 IkReal j0eval[1];
422 j0eval[0]=((IKabs(px))+(IKabs(py)));
423 if( IKabs(j0eval[0]) < 0.0000010000000000 )
424 {
425 continue; // no branches [j0, j1, j2]
426 
427 } else
428 {
429 {
430 IkReal j0array[2], cj0array[2], sj0array[2];
431 bool j0valid[2]={false};
432 _nj0 = 2;
433 CheckValue<IkReal> x50 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
434 if(!x50.valid){
435 continue;
436 }
437 IkReal x49=x50.value;
438 j0array[0]=((-1.0)*x49);
439 sj0array[0]=IKsin(j0array[0]);
440 cj0array[0]=IKcos(j0array[0]);
441 j0array[1]=((3.14159265358979)+(((-1.0)*x49)));
442 sj0array[1]=IKsin(j0array[1]);
443 cj0array[1]=IKcos(j0array[1]);
444 if( j0array[0] > IKPI )
445 {
446  j0array[0]-=IK2PI;
447 }
448 else if( j0array[0] < -IKPI )
449 { j0array[0]+=IK2PI;
450 }
451 j0valid[0] = true;
452 if( j0array[1] > IKPI )
453 {
454  j0array[1]-=IK2PI;
455 }
456 else if( j0array[1] < -IKPI )
457 { j0array[1]+=IK2PI;
458 }
459 j0valid[1] = true;
460 for(int ij0 = 0; ij0 < 2; ++ij0)
461 {
462 if( !j0valid[ij0] )
463 {
464  continue;
465 }
466 _ij0[0] = ij0; _ij0[1] = -1;
467 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
468 {
469 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
470 {
471  j0valid[iij0]=false; _ij0[1] = iij0; break;
472 }
473 }
474 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
475 
476 {
477 IkReal j2array[2], cj2array[2], sj2array[2];
478 bool j2valid[2]={false};
479 _nj2 = 2;
480 if( (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) < -1-IKFAST_SINCOS_THRESH || (((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px)))) > 1+IKFAST_SINCOS_THRESH )
481  continue;
482 IkReal x51=IKasin(((0.994186511658126)+(((-2.69628181343312)*pp))+(((0.269628181343312)*py*sj0))+(((0.269628181343312)*cj0*px))));
483 j2array[0]=((-0.0831412318884412)+(((-1.0)*x51)));
484 sj2array[0]=IKsin(j2array[0]);
485 cj2array[0]=IKcos(j2array[0]);
486 j2array[1]=((3.05845142170135)+x51);
487 sj2array[1]=IKsin(j2array[1]);
488 cj2array[1]=IKcos(j2array[1]);
489 if( j2array[0] > IKPI )
490 {
491  j2array[0]-=IK2PI;
492 }
493 else if( j2array[0] < -IKPI )
494 { j2array[0]+=IK2PI;
495 }
496 j2valid[0] = true;
497 if( j2array[1] > IKPI )
498 {
499  j2array[1]-=IK2PI;
500 }
501 else if( j2array[1] < -IKPI )
502 { j2array[1]+=IK2PI;
503 }
504 j2valid[1] = true;
505 for(int ij2 = 0; ij2 < 2; ++ij2)
506 {
507 if( !j2valid[ij2] )
508 {
509  continue;
510 }
511 _ij2[0] = ij2; _ij2[1] = -1;
512 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
513 {
514 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
515 {
516  j2valid[iij2]=false; _ij2[1] = iij2; break;
517 }
518 }
519 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
520 
521 {
522 IkReal j1eval[3];
523 IkReal x52=cj2*cj2;
524 IkReal x53=(py*sj0);
525 IkReal x54=((240.0)*cj2);
526 IkReal x55=((20.0)*sj2);
527 IkReal x56=((0.42)*cj2);
528 IkReal x57=(cj0*px);
529 IkReal x58=(cj2*sj2);
530 IkReal x59=((0.035)*sj2);
531 IkReal x60=(pz*sj2);
532 IkReal x61=(cj2*pz);
533 j1eval[0]=((((-1.0)*x53*x54))+(((12.0)*cj2))+(((-1.0)*sj2))+(((20.0)*x61))+(((251.428571428571)*pz))+(((-1.0)*x54*x57))+((x53*x55))+(((240.0)*x60))+((x55*x57)));
534 j1eval[1]=((IKabs(((-0.001225)+(((-0.175175)*x52))+(pz*pz)+(((0.0294)*x58)))))+(IKabs(((0.0147)+(((-0.0294)*x52))+(((-0.05)*pz))+(((-0.175175)*x58))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x57))+((pz*x53))))));
535 j1eval[2]=IKsign(((((-1.0)*x53*x56))+(((0.021)*cj2))+(((-1.0)*x56*x57))+(((0.42)*x60))+(((0.035)*x61))+((x57*x59))+(((-0.00175)*sj2))+((x53*x59))+(((0.44)*pz))));
536 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
537 {
538 {
539 IkReal j1eval[2];
540 IkReal x62=(py*sj0);
541 IkReal x63=(pz*sj2);
542 IkReal x64=(cj0*px);
543 IkReal x65=((240.0)*sj2);
544 IkReal x66=((0.035)*cj2);
545 IkReal x67=((20.0)*cj2);
546 IkReal x68=(cj2*pz);
547 IkReal x69=((0.42)*sj2);
548 j1eval[0]=((12.5714285714286)+cj2+(((12.0)*sj2))+(((20.0)*x63))+(((-251.428571428571)*x62))+(((-251.428571428571)*x64))+(((-240.0)*x68))+(((-1.0)*x64*x67))+(((-1.0)*x64*x65))+(((-1.0)*x62*x67))+(((-1.0)*x62*x65)));
549 j1eval[1]=IKsign(((0.022)+(((0.035)*x63))+(((-0.44)*x64))+(((-0.44)*x62))+(((-0.42)*x68))+(((-1.0)*x64*x69))+(((-1.0)*x64*x66))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-1.0)*x62*x69))+(((-1.0)*x62*x66))));
550 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
551 {
552 {
553 IkReal j1eval[2];
554 IkReal x70=py*py;
555 IkReal x71=cj0*cj0;
556 IkReal x72=px*px;
557 IkReal x73=pz*pz;
558 IkReal x74=(py*sj0);
559 IkReal x75=(cj0*px);
560 IkReal x76=(x70*x71);
561 IkReal x77=(x71*x72);
562 j1eval[0]=((-1.0)+(((40.0)*x75))+(((40.0)*x74))+(((-800.0)*x74*x75))+(((400.0)*x76))+(((-400.0)*x70))+(((-400.0)*x77))+(((-400.0)*x73)));
563 j1eval[1]=IKsign(((-0.0025)+(((-2.0)*x74*x75))+(((0.1)*x75))+(((0.1)*x74))+x76+(((-1.0)*x70))+(((-1.0)*x73))+(((-1.0)*x77))));
564 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
565 {
566 continue; // no branches [j1]
567 
568 } else
569 {
570 {
571 IkReal j1array[1], cj1array[1], sj1array[1];
572 bool j1valid[1]={false};
573 _nj1 = 1;
574 IkReal x78=py*py;
575 IkReal x79=cj0*cj0;
576 IkReal x80=(py*sj0);
577 IkReal x81=((0.035)*sj2);
578 IkReal x82=(cj0*px);
579 IkReal x83=((0.035)*cj2);
580 IkReal x84=((0.42)*cj2);
581 IkReal x85=((0.42)*sj2);
582 IkReal x86=((0.42)*x82);
583 CheckValue<IkReal> x87 = IKatan2WithCheck(IkReal(((0.022)+(((-1.0)*x82*x83))+(((-1.0)*x82*x85))+(((-1.0)*x80*x85))+(((-1.0)*x80*x83))+((pz*x84))+(((0.00175)*cj2))+(((0.021)*sj2))+(((-0.44)*x80))+(((-0.44)*x82))+(((-1.0)*pz*x81)))),IkReal(((((0.021)*cj2))+(((-1.0)*x82*x84))+(((-1.0)*x80*x84))+((x81*x82))+(((-0.00175)*sj2))+(((-0.44)*pz))+((x80*x81))+(((-1.0)*pz*x83))+(((-1.0)*pz*x85)))),IKFAST_ATAN2_MAGTHRESH);
584 if(!x87.valid){
585 continue;
586 }
587 CheckValue<IkReal> x88=IKPowWithIntegerCheck(IKsign(((-0.0025)+(((0.1)*x82))+(((0.1)*x80))+((x78*x79))+(((-2.0)*x80*x82))+(((-1.0)*x79*(px*px)))+(((-1.0)*(pz*pz)))+(((-1.0)*x78)))),-1);
588 if(!x88.valid){
589 continue;
590 }
591 j1array[0]=((-1.5707963267949)+(x87.value)+(((1.5707963267949)*(x88.value))));
592 sj1array[0]=IKsin(j1array[0]);
593 cj1array[0]=IKcos(j1array[0]);
594 if( j1array[0] > IKPI )
595 {
596  j1array[0]-=IK2PI;
597 }
598 else if( j1array[0] < -IKPI )
599 { j1array[0]+=IK2PI;
600 }
601 j1valid[0] = true;
602 for(int ij1 = 0; ij1 < 1; ++ij1)
603 {
604 if( !j1valid[ij1] )
605 {
606  continue;
607 }
608 _ij1[0] = ij1; _ij1[1] = -1;
609 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
610 {
611 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
612 {
613  j1valid[iij1]=false; _ij1[1] = iij1; break;
614 }
615 }
616 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
617 {
618 IkReal evalcond[5];
619 IkReal x89=IKcos(j1);
620 IkReal x90=IKsin(j1);
621 IkReal x91=((0.035)*sj2);
622 IkReal x92=(py*sj0);
623 IkReal x93=((0.42)*sj2);
624 IkReal x94=((0.035)*cj2);
625 IkReal x95=(cj0*px);
626 IkReal x96=((0.42)*x90);
627 IkReal x97=((1.0)*x95);
628 IkReal x98=((1.0)*x89);
629 IkReal x99=(cj2*x89);
630 IkReal x100=((0.88)*x90);
631 evalcond[0]=((((0.05)*x89))+((pz*x90))+(((-1.0)*x92*x98))+(((0.42)*cj2))+(((-1.0)*x91))+(((-1.0)*x89*x97)));
632 evalcond[1]=((((-1.0)*x90*x91))+((cj2*x96))+pz+(((-0.44)*x89))+(((-1.0)*x89*x94))+(((-1.0)*x89*x93)));
633 evalcond[2]=((0.44)+(((-1.0)*pz*x98))+(((0.05)*x90))+(((-1.0)*x90*x97))+(((-1.0)*x90*x92))+x93+x94);
634 evalcond[3]=((-0.018475)+((x100*x95))+((x100*x92))+(((0.1)*x92))+(((0.1)*x95))+(((-0.044)*x90))+(((-1.0)*pp))+(((0.88)*pz*x89)));
635 evalcond[4]=((0.05)+(((0.42)*x99))+((x90*x93))+((x90*x94))+(((-1.0)*x92))+(((-1.0)*x97))+(((0.44)*x90))+(((-1.0)*x89*x91)));
636 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
637 {
638 continue;
639 }
640 }
641 
642 rotationfunction0(solutions);
643 }
644 }
645 
646 }
647 
648 }
649 
650 } else
651 {
652 {
653 IkReal j1array[1], cj1array[1], sj1array[1];
654 bool j1valid[1]={false};
655 _nj1 = 1;
656 IkReal x1520=cj2*cj2;
657 IkReal x1521=(cj2*sj2);
658 IkReal x1522=(cj0*px);
659 IkReal x1523=((0.42)*sj2);
660 IkReal x1524=((0.035)*cj2);
661 IkReal x1525=((1.0)*pz);
662 IkReal x1526=(py*sj0);
663 CheckValue<IkReal> x1527 = IKatan2WithCheck(IkReal(((-0.37)+(((-0.0294)*x1521))+(((-0.3696)*sj2))+(((-0.0308)*cj2))+(pz*pz)+(((0.175175)*x1520)))),IkReal(((0.0147)+(((-0.0294)*x1520))+(((-0.175175)*x1521))+(((-1.0)*x1525*x1526))+(((0.0154)*sj2))+(((-1.0)*x1522*x1525))+(((-0.1848)*cj2))+(((0.05)*pz)))),IKFAST_ATAN2_MAGTHRESH);
664 if(!x1527.valid){
665 continue;
666 }
667 CheckValue<IkReal> x1528=IKPowWithIntegerCheck(IKsign(((0.022)+(((-1.0)*x1524*x1526))+(((-0.44)*x1522))+(((-0.44)*x1526))+(((-0.42)*cj2*pz))+(((-1.0)*x1522*x1524))+(((-1.0)*x1522*x1523))+(((0.00175)*cj2))+(((-1.0)*x1523*x1526))+(((0.035)*pz*sj2))+(((0.021)*sj2)))),-1);
668 if(!x1528.valid){
669 continue;
670 }
671 j1array[0]=((-1.5707963267949)+(x1527.value)+(((1.5707963267949)*(x1528.value))));
672 sj1array[0]=IKsin(j1array[0]);
673 cj1array[0]=IKcos(j1array[0]);
674 if( j1array[0] > IKPI )
675 {
676  j1array[0]-=IK2PI;
677 }
678 else if( j1array[0] < -IKPI )
679 { j1array[0]+=IK2PI;
680 }
681 j1valid[0] = true;
682 for(int ij1 = 0; ij1 < 1; ++ij1)
683 {
684 if( !j1valid[ij1] )
685 {
686  continue;
687 }
688 _ij1[0] = ij1; _ij1[1] = -1;
689 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
690 {
691 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
692 {
693  j1valid[iij1]=false; _ij1[1] = iij1; break;
694 }
695 }
696 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
697 {
698 IkReal evalcond[5];
699 IkReal x1529=IKcos(j1);
700 IkReal x1530=IKsin(j1);
701 IkReal x1531=((0.035)*sj2);
702 IkReal x1532=(py*sj0);
703 IkReal x1533=((0.42)*sj2);
704 IkReal x1534=((0.035)*cj2);
705 IkReal x1535=(cj0*px);
706 IkReal x1536=((0.42)*x1530);
707 IkReal x1537=((1.0)*x1535);
708 IkReal x1538=((1.0)*x1529);
709 IkReal x1539=(cj2*x1529);
710 IkReal x1540=((0.88)*x1530);
711 evalcond[0]=((((-1.0)*x1529*x1537))+(((0.05)*x1529))+(((-1.0)*x1531))+(((-1.0)*x1532*x1538))+(((0.42)*cj2))+((pz*x1530)));
712 evalcond[1]=((((-1.0)*x1529*x1533))+(((-1.0)*x1529*x1534))+(((-1.0)*x1530*x1531))+((cj2*x1536))+(((-0.44)*x1529))+pz);
713 evalcond[2]=((0.44)+(((-1.0)*x1530*x1532))+(((0.05)*x1530))+(((-1.0)*x1530*x1537))+x1534+x1533+(((-1.0)*pz*x1538)));
714 evalcond[3]=((-0.018475)+((x1532*x1540))+(((-1.0)*pp))+(((0.1)*x1535))+(((0.1)*x1532))+(((0.88)*pz*x1529))+(((-0.044)*x1530))+((x1535*x1540)));
715 evalcond[4]=((0.05)+(((-1.0)*x1529*x1531))+(((0.44)*x1530))+(((-1.0)*x1532))+(((-1.0)*x1537))+((x1530*x1534))+((x1530*x1533))+(((0.42)*x1539)));
716 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
717 {
718 continue;
719 }
720 }
721 
722 rotationfunction0(solutions);
723 }
724 }
725 
726 }
727 
728 }
729 
730 } else
731 {
732 {
733 IkReal j1array[1], cj1array[1], sj1array[1];
734 bool j1valid[1]={false};
735 _nj1 = 1;
736 IkReal x1541=cj2*cj2;
737 IkReal x1542=(py*sj0);
738 IkReal x1543=((0.035)*sj2);
739 IkReal x1544=((0.42)*cj2);
740 IkReal x1545=(cj0*px);
741 IkReal x1546=(cj2*sj2);
742 CheckValue<IkReal> x1547=IKPowWithIntegerCheck(IKsign(((((0.021)*cj2))+(((0.42)*pz*sj2))+(((-0.00175)*sj2))+(((0.035)*cj2*pz))+((x1543*x1545))+(((-1.0)*x1542*x1544))+(((-1.0)*x1544*x1545))+((x1542*x1543))+(((0.44)*pz)))),-1);
743 if(!x1547.valid){
744 continue;
745 }
746 CheckValue<IkReal> x1548 = IKatan2WithCheck(IkReal(((0.0147)+(((-0.0294)*x1541))+(((-0.05)*pz))+(((-0.175175)*x1546))+(((0.0154)*sj2))+(((-0.1848)*cj2))+((pz*x1542))+((pz*x1545)))),IkReal(((-0.001225)+(((-0.175175)*x1541))+(pz*pz)+(((0.0294)*x1546)))),IKFAST_ATAN2_MAGTHRESH);
747 if(!x1548.valid){
748 continue;
749 }
750 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1547.value)))+(x1548.value));
751 sj1array[0]=IKsin(j1array[0]);
752 cj1array[0]=IKcos(j1array[0]);
753 if( j1array[0] > IKPI )
754 {
755  j1array[0]-=IK2PI;
756 }
757 else if( j1array[0] < -IKPI )
758 { j1array[0]+=IK2PI;
759 }
760 j1valid[0] = true;
761 for(int ij1 = 0; ij1 < 1; ++ij1)
762 {
763 if( !j1valid[ij1] )
764 {
765  continue;
766 }
767 _ij1[0] = ij1; _ij1[1] = -1;
768 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
769 {
770 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
771 {
772  j1valid[iij1]=false; _ij1[1] = iij1; break;
773 }
774 }
775 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
776 {
777 IkReal evalcond[5];
778 IkReal x1549=IKcos(j1);
779 IkReal x1550=IKsin(j1);
780 IkReal x1551=((0.035)*sj2);
781 IkReal x1552=(py*sj0);
782 IkReal x1553=((0.42)*sj2);
783 IkReal x1554=((0.035)*cj2);
784 IkReal x1555=(cj0*px);
785 IkReal x1556=((0.42)*x1550);
786 IkReal x1557=((1.0)*x1555);
787 IkReal x1558=((1.0)*x1549);
788 IkReal x1559=(cj2*x1549);
789 IkReal x1560=((0.88)*x1550);
790 evalcond[0]=((((-1.0)*x1549*x1557))+(((-1.0)*x1552*x1558))+(((0.05)*x1549))+((pz*x1550))+(((0.42)*cj2))+(((-1.0)*x1551)));
791 evalcond[1]=((((-1.0)*x1549*x1554))+(((-1.0)*x1549*x1553))+((cj2*x1556))+(((-0.44)*x1549))+pz+(((-1.0)*x1550*x1551)));
792 evalcond[2]=((0.44)+(((-1.0)*x1550*x1552))+(((-1.0)*pz*x1558))+(((0.05)*x1550))+x1553+x1554+(((-1.0)*x1550*x1557)));
793 evalcond[3]=((-0.018475)+((x1555*x1560))+(((0.1)*x1552))+(((0.1)*x1555))+((x1552*x1560))+(((0.88)*pz*x1549))+(((-1.0)*pp))+(((-0.044)*x1550)));
794 evalcond[4]=((0.05)+(((-1.0)*x1552))+(((-1.0)*x1549*x1551))+((x1550*x1554))+((x1550*x1553))+(((0.42)*x1559))+(((-1.0)*x1557))+(((0.44)*x1550)));
795 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
796 {
797 continue;
798 }
799 }
800 
801 rotationfunction0(solutions);
802 }
803 }
804 
805 }
806 
807 }
808 }
809 }
810 }
811 }
812 
813 }
814 
815 }
816 }
817 return solutions.GetNumSolutions()>0;
818 }
819 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
820 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
821 IkReal x101=(r11*sj0);
822 IkReal x102=(cj0*r02);
823 IkReal x103=(sj1*sj2);
824 IkReal x104=(r10*sj0);
825 IkReal x105=((1.0)*cj1);
826 IkReal x106=((1.0)*sj0);
827 IkReal x107=(cj0*r00);
828 IkReal x108=(r12*sj0);
829 IkReal x109=(cj0*r01);
830 IkReal x110=((((-1.0)*sj2*x105))+((cj2*sj1)));
831 IkReal x111=(x103+((cj1*cj2)));
832 IkReal x112=(sj0*x110);
833 IkReal x113=(cj0*x110);
834 IkReal x114=((((-1.0)*cj2*x105))+(((-1.0)*x103)));
835 new_r00=(((r20*x111))+((x104*x110))+((x107*x110)));
836 new_r01=(((x109*x110))+((r21*x111))+((x101*x110)));
837 new_r02=(((r22*x111))+((x102*x110))+((x108*x110)));
838 new_r10=((((-1.0)*r00*x106))+((cj0*r10)));
839 new_r11=((((-1.0)*r01*x106))+((cj0*r11)));
840 new_r12=((((-1.0)*r02*x106))+((cj0*r12)));
841 new_r20=(((r20*x110))+((x104*x114))+((x107*x114)));
842 new_r21=(((x109*x114))+((r21*x110))+((x101*x114)));
843 new_r22=(((r22*x110))+((x102*x114))+((x108*x114)));
844 {
845 IkReal j4array[2], cj4array[2], sj4array[2];
846 bool j4valid[2]={false};
847 _nj4 = 2;
848 cj4array[0]=new_r22;
849 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
850 {
851  j4valid[0] = j4valid[1] = true;
852  j4array[0] = IKacos(cj4array[0]);
853  sj4array[0] = IKsin(j4array[0]);
854  cj4array[1] = cj4array[0];
855  j4array[1] = -j4array[0];
856  sj4array[1] = -sj4array[0];
857 }
858 else if( isnan(cj4array[0]) )
859 {
860  // probably any value will work
861  j4valid[0] = true;
862  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
863 }
864 for(int ij4 = 0; ij4 < 2; ++ij4)
865 {
866 if( !j4valid[ij4] )
867 {
868  continue;
869 }
870 _ij4[0] = ij4; _ij4[1] = -1;
871 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
872 {
873 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
874 {
875  j4valid[iij4]=false; _ij4[1] = iij4; break;
876 }
877 }
878 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
879 
880 {
881 IkReal j5eval[3];
882 j5eval[0]=sj4;
883 j5eval[1]=IKsign(sj4);
884 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
885 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
886 {
887 {
888 IkReal j3eval[3];
889 j3eval[0]=sj4;
890 j3eval[1]=IKsign(sj4);
891 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
892 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
893 {
894 {
895 IkReal j3eval[2];
896 j3eval[0]=new_r12;
897 j3eval[1]=sj4;
898 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
899 {
900 {
901 IkReal evalcond[5];
902 bool bgotonextstatement = true;
903 do
904 {
905 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
906 evalcond[1]=new_r02;
907 evalcond[2]=new_r12;
908 evalcond[3]=new_r21;
909 evalcond[4]=new_r20;
910 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 )
911 {
912 bgotonextstatement=false;
913 IkReal j5mul = 1;
914 j5=0;
915 j3mul=-1.0;
916 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 )
917  continue;
918 j3=IKatan2(((-1.0)*new_r01), new_r00);
919 {
920 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
921 vinfos[0].jointtype = 1;
922 vinfos[0].foffset = j0;
923 vinfos[0].indices[0] = _ij0[0];
924 vinfos[0].indices[1] = _ij0[1];
925 vinfos[0].maxsolutions = _nj0;
926 vinfos[1].jointtype = 1;
927 vinfos[1].foffset = j1;
928 vinfos[1].indices[0] = _ij1[0];
929 vinfos[1].indices[1] = _ij1[1];
930 vinfos[1].maxsolutions = _nj1;
931 vinfos[2].jointtype = 1;
932 vinfos[2].foffset = j2;
933 vinfos[2].indices[0] = _ij2[0];
934 vinfos[2].indices[1] = _ij2[1];
935 vinfos[2].maxsolutions = _nj2;
936 vinfos[3].jointtype = 1;
937 vinfos[3].foffset = j3;
938 vinfos[3].fmul = j3mul;
939 vinfos[3].freeind = 0;
940 vinfos[3].maxsolutions = 0;
941 vinfos[4].jointtype = 1;
942 vinfos[4].foffset = j4;
943 vinfos[4].indices[0] = _ij4[0];
944 vinfos[4].indices[1] = _ij4[1];
945 vinfos[4].maxsolutions = _nj4;
946 vinfos[5].jointtype = 1;
947 vinfos[5].foffset = j5;
948 vinfos[5].fmul = j5mul;
949 vinfos[5].freeind = 0;
950 vinfos[5].maxsolutions = 0;
951 std::vector<int> vfree(1);
952 vfree[0] = 5;
953 solutions.AddSolution(vinfos,vfree);
954 }
955 
956 }
957 } while(0);
958 if( bgotonextstatement )
959 {
960 bool bgotonextstatement = true;
961 do
962 {
963 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
964 evalcond[1]=new_r02;
965 evalcond[2]=new_r12;
966 evalcond[3]=new_r21;
967 evalcond[4]=new_r20;
968 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 )
969 {
970 bgotonextstatement=false;
971 IkReal j5mul = 1;
972 j5=0;
973 j3mul=1.0;
974 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 )
975  continue;
976 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
977 {
978 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
979 vinfos[0].jointtype = 1;
980 vinfos[0].foffset = j0;
981 vinfos[0].indices[0] = _ij0[0];
982 vinfos[0].indices[1] = _ij0[1];
983 vinfos[0].maxsolutions = _nj0;
984 vinfos[1].jointtype = 1;
985 vinfos[1].foffset = j1;
986 vinfos[1].indices[0] = _ij1[0];
987 vinfos[1].indices[1] = _ij1[1];
988 vinfos[1].maxsolutions = _nj1;
989 vinfos[2].jointtype = 1;
990 vinfos[2].foffset = j2;
991 vinfos[2].indices[0] = _ij2[0];
992 vinfos[2].indices[1] = _ij2[1];
993 vinfos[2].maxsolutions = _nj2;
994 vinfos[3].jointtype = 1;
995 vinfos[3].foffset = j3;
996 vinfos[3].fmul = j3mul;
997 vinfos[3].freeind = 0;
998 vinfos[3].maxsolutions = 0;
999 vinfos[4].jointtype = 1;
1000 vinfos[4].foffset = j4;
1001 vinfos[4].indices[0] = _ij4[0];
1002 vinfos[4].indices[1] = _ij4[1];
1003 vinfos[4].maxsolutions = _nj4;
1004 vinfos[5].jointtype = 1;
1005 vinfos[5].foffset = j5;
1006 vinfos[5].fmul = j5mul;
1007 vinfos[5].freeind = 0;
1008 vinfos[5].maxsolutions = 0;
1009 std::vector<int> vfree(1);
1010 vfree[0] = 5;
1011 solutions.AddSolution(vinfos,vfree);
1012 }
1013 
1014 }
1015 } while(0);
1016 if( bgotonextstatement )
1017 {
1018 bool bgotonextstatement = true;
1019 do
1020 {
1021 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1022 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1023 {
1024 bgotonextstatement=false;
1025 {
1026 IkReal j3eval[1];
1027 new_r21=0;
1028 new_r20=0;
1029 new_r02=0;
1030 new_r12=0;
1031 IkReal x115=new_r22*new_r22;
1032 IkReal x116=((16.0)*new_r10);
1033 IkReal x117=((16.0)*new_r01);
1034 IkReal x118=((16.0)*new_r22);
1035 IkReal x119=((8.0)*new_r11);
1036 IkReal x120=((8.0)*new_r00);
1037 IkReal x121=(x115*x116);
1038 IkReal x122=(x115*x117);
1039 j3eval[0]=((IKabs((((new_r11*x118))+(((16.0)*new_r00))+(((-32.0)*new_r00*x115)))))+(IKabs((((new_r22*x120))+(((-1.0)*x115*x119)))))+(IKabs(((((-32.0)*new_r11))+((new_r00*x118))+(((16.0)*new_r11*x115)))))+(IKabs(((((-1.0)*x121))+x116)))+(IKabs(((((-1.0)*x122))+x117)))+(IKabs(((((-1.0)*x116))+x121)))+(IKabs(((((-1.0)*x117))+x122)))+(IKabs((((new_r22*x119))+(((-1.0)*x120))))));
1040 if( IKabs(j3eval[0]) < 0.0000000010000000 )
1041 {
1042 continue; // no branches [j3, j5]
1043 
1044 } else
1045 {
1046 IkReal op[4+1], zeror[4];
1047 int numroots;
1048 IkReal j3evalpoly[1];
1049 IkReal x123=new_r22*new_r22;
1050 IkReal x124=((16.0)*new_r10);
1051 IkReal x125=(new_r11*new_r22);
1052 IkReal x126=(x123*x124);
1053 IkReal x127=((((-8.0)*new_r00))+(((8.0)*x125)));
1054 op[0]=x127;
1055 op[1]=((((-1.0)*x126))+x124);
1056 op[2]=((((16.0)*x125))+(((16.0)*new_r00))+(((-32.0)*new_r00*x123)));
1057 op[3]=((((-1.0)*x124))+x126);
1058 op[4]=x127;
1059 polyroots4(op,zeror,numroots);
1060 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1061 int numsolutions = 0;
1062 for(int ij3 = 0; ij3 < numroots; ++ij3)
1063 {
1064 IkReal htj3 = zeror[ij3];
1065 tempj3array[0]=((2.0)*(atan(htj3)));
1066 for(int kj3 = 0; kj3 < 1; ++kj3)
1067 {
1068 j3array[numsolutions] = tempj3array[kj3];
1069 if( j3array[numsolutions] > IKPI )
1070 {
1071  j3array[numsolutions]-=IK2PI;
1072 }
1073 else if( j3array[numsolutions] < -IKPI )
1074 {
1075  j3array[numsolutions]+=IK2PI;
1076 }
1077 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1078 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1079 numsolutions++;
1080 }
1081 }
1082 bool j3valid[4]={true,true,true,true};
1083 _nj3 = 4;
1084 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1085  {
1086 if( !j3valid[ij3] )
1087 {
1088  continue;
1089 }
1090  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1091 htj3 = IKtan(j3/2);
1092 
1093 IkReal x128=((16.0)*new_r01);
1094 IkReal x129=new_r22*new_r22;
1095 IkReal x130=(new_r00*new_r22);
1096 IkReal x131=((8.0)*x130);
1097 IkReal x132=(new_r11*x129);
1098 IkReal x133=(x128*x129);
1099 IkReal x134=((8.0)*x132);
1100 j3evalpoly[0]=(((htj3*(((((-1.0)*x128))+x133))))+(((htj3*htj3)*(((((16.0)*x132))+(((16.0)*x130))+(((-32.0)*new_r11))))))+x131+(((htj3*htj3*htj3)*((x128+(((-1.0)*x133))))))+(((-1.0)*x134))+(((htj3*htj3*htj3*htj3)*((x131+(((-1.0)*x134)))))));
1101 if( IKabs(j3evalpoly[0]) > 0.0000000010000000 )
1102 {
1103  continue;
1104 }
1105 _ij3[0] = ij3; _ij3[1] = -1;
1106 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1107 {
1108 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1109 {
1110  j3valid[iij3]=false; _ij3[1] = iij3; break;
1111 }
1112 }
1113 {
1114 IkReal j5eval[3];
1115 new_r21=0;
1116 new_r20=0;
1117 new_r02=0;
1118 new_r12=0;
1119 IkReal x135=cj3*cj3;
1120 IkReal x136=(cj3*new_r22);
1121 IkReal x137=((-1.0)+(((-1.0)*x135*(new_r22*new_r22)))+x135);
1122 j5eval[0]=x137;
1123 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x136)))))+(IKabs((((new_r01*x136))+((new_r00*sj3))))));
1124 j5eval[2]=IKsign(x137);
1125 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1126 {
1127 {
1128 IkReal j5eval[1];
1129 new_r21=0;
1130 new_r20=0;
1131 new_r02=0;
1132 new_r12=0;
1133 j5eval[0]=new_r22;
1134 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1135 {
1136 {
1137 IkReal j5eval[1];
1138 new_r21=0;
1139 new_r20=0;
1140 new_r02=0;
1141 new_r12=0;
1142 j5eval[0]=sj3;
1143 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1144 {
1145 {
1146 IkReal evalcond[1];
1147 bool bgotonextstatement = true;
1148 do
1149 {
1150 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1151 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1152 {
1153 bgotonextstatement=false;
1154 {
1155 IkReal j5array[1], cj5array[1], sj5array[1];
1156 bool j5valid[1]={false};
1157 _nj5 = 1;
1158 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1159  continue;
1160 j5array[0]=IKatan2(new_r10, new_r11);
1161 sj5array[0]=IKsin(j5array[0]);
1162 cj5array[0]=IKcos(j5array[0]);
1163 if( j5array[0] > IKPI )
1164 {
1165  j5array[0]-=IK2PI;
1166 }
1167 else if( j5array[0] < -IKPI )
1168 { j5array[0]+=IK2PI;
1169 }
1170 j5valid[0] = true;
1171 for(int ij5 = 0; ij5 < 1; ++ij5)
1172 {
1173 if( !j5valid[ij5] )
1174 {
1175  continue;
1176 }
1177 _ij5[0] = ij5; _ij5[1] = -1;
1178 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1179 {
1180 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1181 {
1182  j5valid[iij5]=false; _ij5[1] = iij5; break;
1183 }
1184 }
1185 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1186 {
1187 IkReal evalcond[4];
1188 IkReal x138=IKsin(j5);
1189 IkReal x139=IKcos(j5);
1190 evalcond[0]=x138;
1191 evalcond[1]=((-1.0)*x139);
1192 evalcond[2]=(x138+(((-1.0)*new_r10)));
1193 evalcond[3]=(x139+(((-1.0)*new_r11)));
1194 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1195 {
1196 continue;
1197 }
1198 }
1199 
1200 {
1201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1202 vinfos[0].jointtype = 1;
1203 vinfos[0].foffset = j0;
1204 vinfos[0].indices[0] = _ij0[0];
1205 vinfos[0].indices[1] = _ij0[1];
1206 vinfos[0].maxsolutions = _nj0;
1207 vinfos[1].jointtype = 1;
1208 vinfos[1].foffset = j1;
1209 vinfos[1].indices[0] = _ij1[0];
1210 vinfos[1].indices[1] = _ij1[1];
1211 vinfos[1].maxsolutions = _nj1;
1212 vinfos[2].jointtype = 1;
1213 vinfos[2].foffset = j2;
1214 vinfos[2].indices[0] = _ij2[0];
1215 vinfos[2].indices[1] = _ij2[1];
1216 vinfos[2].maxsolutions = _nj2;
1217 vinfos[3].jointtype = 1;
1218 vinfos[3].foffset = j3;
1219 vinfos[3].indices[0] = _ij3[0];
1220 vinfos[3].indices[1] = _ij3[1];
1221 vinfos[3].maxsolutions = _nj3;
1222 vinfos[4].jointtype = 1;
1223 vinfos[4].foffset = j4;
1224 vinfos[4].indices[0] = _ij4[0];
1225 vinfos[4].indices[1] = _ij4[1];
1226 vinfos[4].maxsolutions = _nj4;
1227 vinfos[5].jointtype = 1;
1228 vinfos[5].foffset = j5;
1229 vinfos[5].indices[0] = _ij5[0];
1230 vinfos[5].indices[1] = _ij5[1];
1231 vinfos[5].maxsolutions = _nj5;
1232 std::vector<int> vfree(0);
1233 solutions.AddSolution(vinfos,vfree);
1234 }
1235 }
1236 }
1237 
1238 }
1239 } while(0);
1240 if( bgotonextstatement )
1241 {
1242 bool bgotonextstatement = true;
1243 do
1244 {
1245 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1246 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1247 {
1248 bgotonextstatement=false;
1249 {
1250 IkReal j5array[1], cj5array[1], sj5array[1];
1251 bool j5valid[1]={false};
1252 _nj5 = 1;
1253 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 )
1254  continue;
1255 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1256 sj5array[0]=IKsin(j5array[0]);
1257 cj5array[0]=IKcos(j5array[0]);
1258 if( j5array[0] > IKPI )
1259 {
1260  j5array[0]-=IK2PI;
1261 }
1262 else if( j5array[0] < -IKPI )
1263 { j5array[0]+=IK2PI;
1264 }
1265 j5valid[0] = true;
1266 for(int ij5 = 0; ij5 < 1; ++ij5)
1267 {
1268 if( !j5valid[ij5] )
1269 {
1270  continue;
1271 }
1272 _ij5[0] = ij5; _ij5[1] = -1;
1273 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1274 {
1275 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1276 {
1277  j5valid[iij5]=false; _ij5[1] = iij5; break;
1278 }
1279 }
1280 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1281 {
1282 IkReal evalcond[4];
1283 IkReal x140=IKsin(j5);
1284 IkReal x141=IKcos(j5);
1285 evalcond[0]=x140;
1286 evalcond[1]=(x140+new_r10);
1287 evalcond[2]=(x141+new_r11);
1288 evalcond[3]=((-1.0)*x141);
1289 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1290 {
1291 continue;
1292 }
1293 }
1294 
1295 {
1296 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1297 vinfos[0].jointtype = 1;
1298 vinfos[0].foffset = j0;
1299 vinfos[0].indices[0] = _ij0[0];
1300 vinfos[0].indices[1] = _ij0[1];
1301 vinfos[0].maxsolutions = _nj0;
1302 vinfos[1].jointtype = 1;
1303 vinfos[1].foffset = j1;
1304 vinfos[1].indices[0] = _ij1[0];
1305 vinfos[1].indices[1] = _ij1[1];
1306 vinfos[1].maxsolutions = _nj1;
1307 vinfos[2].jointtype = 1;
1308 vinfos[2].foffset = j2;
1309 vinfos[2].indices[0] = _ij2[0];
1310 vinfos[2].indices[1] = _ij2[1];
1311 vinfos[2].maxsolutions = _nj2;
1312 vinfos[3].jointtype = 1;
1313 vinfos[3].foffset = j3;
1314 vinfos[3].indices[0] = _ij3[0];
1315 vinfos[3].indices[1] = _ij3[1];
1316 vinfos[3].maxsolutions = _nj3;
1317 vinfos[4].jointtype = 1;
1318 vinfos[4].foffset = j4;
1319 vinfos[4].indices[0] = _ij4[0];
1320 vinfos[4].indices[1] = _ij4[1];
1321 vinfos[4].maxsolutions = _nj4;
1322 vinfos[5].jointtype = 1;
1323 vinfos[5].foffset = j5;
1324 vinfos[5].indices[0] = _ij5[0];
1325 vinfos[5].indices[1] = _ij5[1];
1326 vinfos[5].maxsolutions = _nj5;
1327 std::vector<int> vfree(0);
1328 solutions.AddSolution(vinfos,vfree);
1329 }
1330 }
1331 }
1332 
1333 }
1334 } while(0);
1335 if( bgotonextstatement )
1336 {
1337 bool bgotonextstatement = true;
1338 do
1339 {
1340 CheckValue<IkReal> x142=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1341 if(!x142.valid){
1342 continue;
1343 }
1344 if((x142.value) < -0.00001)
1345 continue;
1346 IkReal gconst24=((-1.0)*(IKsqrt(x142.value)));
1347 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1348 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1349 {
1350 bgotonextstatement=false;
1351 {
1352 IkReal j5eval[1];
1353 new_r21=0;
1354 new_r20=0;
1355 new_r02=0;
1356 new_r12=0;
1357 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1358 continue;
1359 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
1360 cj3=gconst24;
1361 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1362  continue;
1363 j3=IKacos(gconst24);
1364 CheckValue<IkReal> x143=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1365 if(!x143.valid){
1366 continue;
1367 }
1368 if((x143.value) < -0.00001)
1369 continue;
1370 IkReal gconst24=((-1.0)*(IKsqrt(x143.value)));
1371 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1372 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1373 {
1374 {
1375 IkReal j5array[1], cj5array[1], sj5array[1];
1376 bool j5valid[1]={false};
1377 _nj5 = 1;
1378 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1379 continue;
1380 CheckValue<IkReal> x144=IKPowWithIntegerCheck(gconst24,-1);
1381 if(!x144.valid){
1382 continue;
1383 }
1384 if( IKabs(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x144.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x144.value)))-1) <= IKFAST_SINCOS_THRESH )
1385  continue;
1386 j5array[0]=IKatan2(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x144.value)));
1387 sj5array[0]=IKsin(j5array[0]);
1388 cj5array[0]=IKcos(j5array[0]);
1389 if( j5array[0] > IKPI )
1390 {
1391  j5array[0]-=IK2PI;
1392 }
1393 else if( j5array[0] < -IKPI )
1394 { j5array[0]+=IK2PI;
1395 }
1396 j5valid[0] = true;
1397 for(int ij5 = 0; ij5 < 1; ++ij5)
1398 {
1399 if( !j5valid[ij5] )
1400 {
1401  continue;
1402 }
1403 _ij5[0] = ij5; _ij5[1] = -1;
1404 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1405 {
1406 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1407 {
1408  j5valid[iij5]=false; _ij5[1] = iij5; break;
1409 }
1410 }
1411 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1412 {
1413 IkReal evalcond[8];
1414 IkReal x145=IKcos(j5);
1415 IkReal x146=IKsin(j5);
1416 IkReal x147=((1.0)*gconst24);
1417 if((((1.0)+(((-1.0)*gconst24*x147)))) < -0.00001)
1418 continue;
1419 IkReal x148=IKsqrt(((1.0)+(((-1.0)*gconst24*x147))));
1420 evalcond[0]=x146;
1421 evalcond[1]=((-1.0)*x145);
1422 evalcond[2]=((((-1.0)*x145*x147))+new_r11);
1423 evalcond[3]=((((-1.0)*x146*x147))+new_r10);
1424 evalcond[4]=(((x145*x148))+new_r01);
1425 evalcond[5]=(((x146*x148))+new_r00);
1426 evalcond[6]=(((new_r00*x148))+x146+(((-1.0)*new_r10*x147)));
1427 evalcond[7]=(((new_r01*x148))+(((-1.0)*new_r11*x147))+x145);
1428 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1429 {
1430 continue;
1431 }
1432 }
1433 
1434 {
1435 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1436 vinfos[0].jointtype = 1;
1437 vinfos[0].foffset = j0;
1438 vinfos[0].indices[0] = _ij0[0];
1439 vinfos[0].indices[1] = _ij0[1];
1440 vinfos[0].maxsolutions = _nj0;
1441 vinfos[1].jointtype = 1;
1442 vinfos[1].foffset = j1;
1443 vinfos[1].indices[0] = _ij1[0];
1444 vinfos[1].indices[1] = _ij1[1];
1445 vinfos[1].maxsolutions = _nj1;
1446 vinfos[2].jointtype = 1;
1447 vinfos[2].foffset = j2;
1448 vinfos[2].indices[0] = _ij2[0];
1449 vinfos[2].indices[1] = _ij2[1];
1450 vinfos[2].maxsolutions = _nj2;
1451 vinfos[3].jointtype = 1;
1452 vinfos[3].foffset = j3;
1453 vinfos[3].indices[0] = _ij3[0];
1454 vinfos[3].indices[1] = _ij3[1];
1455 vinfos[3].maxsolutions = _nj3;
1456 vinfos[4].jointtype = 1;
1457 vinfos[4].foffset = j4;
1458 vinfos[4].indices[0] = _ij4[0];
1459 vinfos[4].indices[1] = _ij4[1];
1460 vinfos[4].maxsolutions = _nj4;
1461 vinfos[5].jointtype = 1;
1462 vinfos[5].foffset = j5;
1463 vinfos[5].indices[0] = _ij5[0];
1464 vinfos[5].indices[1] = _ij5[1];
1465 vinfos[5].maxsolutions = _nj5;
1466 std::vector<int> vfree(0);
1467 solutions.AddSolution(vinfos,vfree);
1468 }
1469 }
1470 }
1471 
1472 } else
1473 {
1474 {
1475 IkReal j5array[1], cj5array[1], sj5array[1];
1476 bool j5valid[1]={false};
1477 _nj5 = 1;
1478 CheckValue<IkReal> x149=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1479 if(!x149.valid){
1480 continue;
1481 }
1482 CheckValue<IkReal> x150 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1483 if(!x150.valid){
1484 continue;
1485 }
1486 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x149.value)))+(x150.value));
1487 sj5array[0]=IKsin(j5array[0]);
1488 cj5array[0]=IKcos(j5array[0]);
1489 if( j5array[0] > IKPI )
1490 {
1491  j5array[0]-=IK2PI;
1492 }
1493 else if( j5array[0] < -IKPI )
1494 { j5array[0]+=IK2PI;
1495 }
1496 j5valid[0] = true;
1497 for(int ij5 = 0; ij5 < 1; ++ij5)
1498 {
1499 if( !j5valid[ij5] )
1500 {
1501  continue;
1502 }
1503 _ij5[0] = ij5; _ij5[1] = -1;
1504 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1505 {
1506 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1507 {
1508  j5valid[iij5]=false; _ij5[1] = iij5; break;
1509 }
1510 }
1511 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1512 {
1513 IkReal evalcond[8];
1514 IkReal x151=IKcos(j5);
1515 IkReal x152=IKsin(j5);
1516 IkReal x153=((1.0)*gconst24);
1517 if((((1.0)+(((-1.0)*gconst24*x153)))) < -0.00001)
1518 continue;
1519 IkReal x154=IKsqrt(((1.0)+(((-1.0)*gconst24*x153))));
1520 evalcond[0]=x152;
1521 evalcond[1]=((-1.0)*x151);
1522 evalcond[2]=((((-1.0)*x151*x153))+new_r11);
1523 evalcond[3]=((((-1.0)*x152*x153))+new_r10);
1524 evalcond[4]=(((x151*x154))+new_r01);
1525 evalcond[5]=(((x152*x154))+new_r00);
1526 evalcond[6]=(((new_r00*x154))+x152+(((-1.0)*new_r10*x153)));
1527 evalcond[7]=(((new_r01*x154))+x151+(((-1.0)*new_r11*x153)));
1528 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1529 {
1530 continue;
1531 }
1532 }
1533 
1534 {
1535 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1536 vinfos[0].jointtype = 1;
1537 vinfos[0].foffset = j0;
1538 vinfos[0].indices[0] = _ij0[0];
1539 vinfos[0].indices[1] = _ij0[1];
1540 vinfos[0].maxsolutions = _nj0;
1541 vinfos[1].jointtype = 1;
1542 vinfos[1].foffset = j1;
1543 vinfos[1].indices[0] = _ij1[0];
1544 vinfos[1].indices[1] = _ij1[1];
1545 vinfos[1].maxsolutions = _nj1;
1546 vinfos[2].jointtype = 1;
1547 vinfos[2].foffset = j2;
1548 vinfos[2].indices[0] = _ij2[0];
1549 vinfos[2].indices[1] = _ij2[1];
1550 vinfos[2].maxsolutions = _nj2;
1551 vinfos[3].jointtype = 1;
1552 vinfos[3].foffset = j3;
1553 vinfos[3].indices[0] = _ij3[0];
1554 vinfos[3].indices[1] = _ij3[1];
1555 vinfos[3].maxsolutions = _nj3;
1556 vinfos[4].jointtype = 1;
1557 vinfos[4].foffset = j4;
1558 vinfos[4].indices[0] = _ij4[0];
1559 vinfos[4].indices[1] = _ij4[1];
1560 vinfos[4].maxsolutions = _nj4;
1561 vinfos[5].jointtype = 1;
1562 vinfos[5].foffset = j5;
1563 vinfos[5].indices[0] = _ij5[0];
1564 vinfos[5].indices[1] = _ij5[1];
1565 vinfos[5].maxsolutions = _nj5;
1566 std::vector<int> vfree(0);
1567 solutions.AddSolution(vinfos,vfree);
1568 }
1569 }
1570 }
1571 
1572 }
1573 
1574 }
1575 
1576 }
1577 } while(0);
1578 if( bgotonextstatement )
1579 {
1580 bool bgotonextstatement = true;
1581 do
1582 {
1583 CheckValue<IkReal> x155=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1584 if(!x155.valid){
1585 continue;
1586 }
1587 if((x155.value) < -0.00001)
1588 continue;
1589 IkReal gconst24=((-1.0)*(IKsqrt(x155.value)));
1590 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1591 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1592 {
1593 bgotonextstatement=false;
1594 {
1595 IkReal j5eval[1];
1596 new_r21=0;
1597 new_r20=0;
1598 new_r02=0;
1599 new_r12=0;
1600 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1601 continue;
1602 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
1603 cj3=gconst24;
1604 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1605  continue;
1606 j3=((-1.0)*(IKacos(gconst24)));
1607 CheckValue<IkReal> x156=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1608 if(!x156.valid){
1609 continue;
1610 }
1611 if((x156.value) < -0.00001)
1612 continue;
1613 IkReal gconst24=((-1.0)*(IKsqrt(x156.value)));
1614 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1615 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1616 {
1617 {
1618 IkReal j5array[1], cj5array[1], sj5array[1];
1619 bool j5valid[1]={false};
1620 _nj5 = 1;
1621 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1622 continue;
1623 CheckValue<IkReal> x157=IKPowWithIntegerCheck(gconst24,-1);
1624 if(!x157.valid){
1625 continue;
1626 }
1627 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x157.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x157.value)))-1) <= IKFAST_SINCOS_THRESH )
1628  continue;
1629 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x157.value)));
1630 sj5array[0]=IKsin(j5array[0]);
1631 cj5array[0]=IKcos(j5array[0]);
1632 if( j5array[0] > IKPI )
1633 {
1634  j5array[0]-=IK2PI;
1635 }
1636 else if( j5array[0] < -IKPI )
1637 { j5array[0]+=IK2PI;
1638 }
1639 j5valid[0] = true;
1640 for(int ij5 = 0; ij5 < 1; ++ij5)
1641 {
1642 if( !j5valid[ij5] )
1643 {
1644  continue;
1645 }
1646 _ij5[0] = ij5; _ij5[1] = -1;
1647 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1648 {
1649 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1650 {
1651  j5valid[iij5]=false; _ij5[1] = iij5; break;
1652 }
1653 }
1654 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1655 {
1656 IkReal evalcond[8];
1657 IkReal x158=IKcos(j5);
1658 IkReal x159=IKsin(j5);
1659 IkReal x160=((1.0)*gconst24);
1660 IkReal x161=((1.0)*x159);
1661 if((((1.0)+(((-1.0)*gconst24*x160)))) < -0.00001)
1662 continue;
1663 IkReal x162=IKsqrt(((1.0)+(((-1.0)*gconst24*x160))));
1664 IkReal x163=((1.0)*x162);
1665 evalcond[0]=x159;
1666 evalcond[1]=((-1.0)*x158);
1667 evalcond[2]=((((-1.0)*x158*x160))+new_r11);
1668 evalcond[3]=((((-1.0)*x159*x160))+new_r10);
1669 evalcond[4]=((((-1.0)*x158*x163))+new_r01);
1670 evalcond[5]=((((-1.0)*x161*x162))+new_r00);
1671 evalcond[6]=((((-1.0)*new_r00*x163))+(((-1.0)*new_r10*x160))+x159);
1672 evalcond[7]=((((-1.0)*new_r11*x160))+x158+(((-1.0)*new_r01*x163)));
1673 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1674 {
1675 continue;
1676 }
1677 }
1678 
1679 {
1680 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1681 vinfos[0].jointtype = 1;
1682 vinfos[0].foffset = j0;
1683 vinfos[0].indices[0] = _ij0[0];
1684 vinfos[0].indices[1] = _ij0[1];
1685 vinfos[0].maxsolutions = _nj0;
1686 vinfos[1].jointtype = 1;
1687 vinfos[1].foffset = j1;
1688 vinfos[1].indices[0] = _ij1[0];
1689 vinfos[1].indices[1] = _ij1[1];
1690 vinfos[1].maxsolutions = _nj1;
1691 vinfos[2].jointtype = 1;
1692 vinfos[2].foffset = j2;
1693 vinfos[2].indices[0] = _ij2[0];
1694 vinfos[2].indices[1] = _ij2[1];
1695 vinfos[2].maxsolutions = _nj2;
1696 vinfos[3].jointtype = 1;
1697 vinfos[3].foffset = j3;
1698 vinfos[3].indices[0] = _ij3[0];
1699 vinfos[3].indices[1] = _ij3[1];
1700 vinfos[3].maxsolutions = _nj3;
1701 vinfos[4].jointtype = 1;
1702 vinfos[4].foffset = j4;
1703 vinfos[4].indices[0] = _ij4[0];
1704 vinfos[4].indices[1] = _ij4[1];
1705 vinfos[4].maxsolutions = _nj4;
1706 vinfos[5].jointtype = 1;
1707 vinfos[5].foffset = j5;
1708 vinfos[5].indices[0] = _ij5[0];
1709 vinfos[5].indices[1] = _ij5[1];
1710 vinfos[5].maxsolutions = _nj5;
1711 std::vector<int> vfree(0);
1712 solutions.AddSolution(vinfos,vfree);
1713 }
1714 }
1715 }
1716 
1717 } else
1718 {
1719 {
1720 IkReal j5array[1], cj5array[1], sj5array[1];
1721 bool j5valid[1]={false};
1722 _nj5 = 1;
1723 CheckValue<IkReal> x164=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1724 if(!x164.valid){
1725 continue;
1726 }
1727 CheckValue<IkReal> x165 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1728 if(!x165.valid){
1729 continue;
1730 }
1731 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x164.value)))+(x165.value));
1732 sj5array[0]=IKsin(j5array[0]);
1733 cj5array[0]=IKcos(j5array[0]);
1734 if( j5array[0] > IKPI )
1735 {
1736  j5array[0]-=IK2PI;
1737 }
1738 else if( j5array[0] < -IKPI )
1739 { j5array[0]+=IK2PI;
1740 }
1741 j5valid[0] = true;
1742 for(int ij5 = 0; ij5 < 1; ++ij5)
1743 {
1744 if( !j5valid[ij5] )
1745 {
1746  continue;
1747 }
1748 _ij5[0] = ij5; _ij5[1] = -1;
1749 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1750 {
1751 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1752 {
1753  j5valid[iij5]=false; _ij5[1] = iij5; break;
1754 }
1755 }
1756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1757 {
1758 IkReal evalcond[8];
1759 IkReal x166=IKcos(j5);
1760 IkReal x167=IKsin(j5);
1761 IkReal x168=((1.0)*gconst24);
1762 IkReal x169=((1.0)*x167);
1763 if((((1.0)+(((-1.0)*gconst24*x168)))) < -0.00001)
1764 continue;
1765 IkReal x170=IKsqrt(((1.0)+(((-1.0)*gconst24*x168))));
1766 IkReal x171=((1.0)*x170);
1767 evalcond[0]=x167;
1768 evalcond[1]=((-1.0)*x166);
1769 evalcond[2]=((((-1.0)*x166*x168))+new_r11);
1770 evalcond[3]=(new_r10+(((-1.0)*x167*x168)));
1771 evalcond[4]=(new_r01+(((-1.0)*x166*x171)));
1772 evalcond[5]=(new_r00+(((-1.0)*x169*x170)));
1773 evalcond[6]=((((-1.0)*new_r10*x168))+x167+(((-1.0)*new_r00*x171)));
1774 evalcond[7]=((((-1.0)*new_r11*x168))+x166+(((-1.0)*new_r01*x171)));
1775 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1776 {
1777 continue;
1778 }
1779 }
1780 
1781 {
1782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1783 vinfos[0].jointtype = 1;
1784 vinfos[0].foffset = j0;
1785 vinfos[0].indices[0] = _ij0[0];
1786 vinfos[0].indices[1] = _ij0[1];
1787 vinfos[0].maxsolutions = _nj0;
1788 vinfos[1].jointtype = 1;
1789 vinfos[1].foffset = j1;
1790 vinfos[1].indices[0] = _ij1[0];
1791 vinfos[1].indices[1] = _ij1[1];
1792 vinfos[1].maxsolutions = _nj1;
1793 vinfos[2].jointtype = 1;
1794 vinfos[2].foffset = j2;
1795 vinfos[2].indices[0] = _ij2[0];
1796 vinfos[2].indices[1] = _ij2[1];
1797 vinfos[2].maxsolutions = _nj2;
1798 vinfos[3].jointtype = 1;
1799 vinfos[3].foffset = j3;
1800 vinfos[3].indices[0] = _ij3[0];
1801 vinfos[3].indices[1] = _ij3[1];
1802 vinfos[3].maxsolutions = _nj3;
1803 vinfos[4].jointtype = 1;
1804 vinfos[4].foffset = j4;
1805 vinfos[4].indices[0] = _ij4[0];
1806 vinfos[4].indices[1] = _ij4[1];
1807 vinfos[4].maxsolutions = _nj4;
1808 vinfos[5].jointtype = 1;
1809 vinfos[5].foffset = j5;
1810 vinfos[5].indices[0] = _ij5[0];
1811 vinfos[5].indices[1] = _ij5[1];
1812 vinfos[5].maxsolutions = _nj5;
1813 std::vector<int> vfree(0);
1814 solutions.AddSolution(vinfos,vfree);
1815 }
1816 }
1817 }
1818 
1819 }
1820 
1821 }
1822 
1823 }
1824 } while(0);
1825 if( bgotonextstatement )
1826 {
1827 bool bgotonextstatement = true;
1828 do
1829 {
1830 CheckValue<IkReal> x172=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1831 if(!x172.valid){
1832 continue;
1833 }
1834 if((x172.value) < -0.00001)
1835 continue;
1836 IkReal gconst25=IKsqrt(x172.value);
1837 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1838 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1839 {
1840 bgotonextstatement=false;
1841 {
1842 IkReal j5eval[1];
1843 new_r21=0;
1844 new_r20=0;
1845 new_r02=0;
1846 new_r12=0;
1847 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1848 continue;
1849 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
1850 cj3=gconst25;
1851 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
1852  continue;
1853 j3=IKacos(gconst25);
1854 CheckValue<IkReal> x173=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1855 if(!x173.valid){
1856 continue;
1857 }
1858 if((x173.value) < -0.00001)
1859 continue;
1860 IkReal gconst25=IKsqrt(x173.value);
1861 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1862 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1863 {
1864 {
1865 IkReal j5array[1], cj5array[1], sj5array[1];
1866 bool j5valid[1]={false};
1867 _nj5 = 1;
1868 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1869 continue;
1870 CheckValue<IkReal> x174=IKPowWithIntegerCheck(gconst25,-1);
1871 if(!x174.valid){
1872 continue;
1873 }
1874 if( IKabs((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x174.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x174.value)))-1) <= IKFAST_SINCOS_THRESH )
1875  continue;
1876 j5array[0]=IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x174.value)));
1877 sj5array[0]=IKsin(j5array[0]);
1878 cj5array[0]=IKcos(j5array[0]);
1879 if( j5array[0] > IKPI )
1880 {
1881  j5array[0]-=IK2PI;
1882 }
1883 else if( j5array[0] < -IKPI )
1884 { j5array[0]+=IK2PI;
1885 }
1886 j5valid[0] = true;
1887 for(int ij5 = 0; ij5 < 1; ++ij5)
1888 {
1889 if( !j5valid[ij5] )
1890 {
1891  continue;
1892 }
1893 _ij5[0] = ij5; _ij5[1] = -1;
1894 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1895 {
1896 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1897 {
1898  j5valid[iij5]=false; _ij5[1] = iij5; break;
1899 }
1900 }
1901 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1902 {
1903 IkReal evalcond[8];
1904 IkReal x175=IKcos(j5);
1905 IkReal x176=IKsin(j5);
1906 IkReal x177=((1.0)*gconst25);
1907 if((((1.0)+(((-1.0)*gconst25*x177)))) < -0.00001)
1908 continue;
1909 IkReal x178=IKsqrt(((1.0)+(((-1.0)*gconst25*x177))));
1910 evalcond[0]=x176;
1911 evalcond[1]=((-1.0)*x175);
1912 evalcond[2]=((((-1.0)*x175*x177))+new_r11);
1913 evalcond[3]=((((-1.0)*x176*x177))+new_r10);
1914 evalcond[4]=(((x175*x178))+new_r01);
1915 evalcond[5]=(((x176*x178))+new_r00);
1916 evalcond[6]=(((new_r00*x178))+(((-1.0)*new_r10*x177))+x176);
1917 evalcond[7]=(((new_r01*x178))+(((-1.0)*new_r11*x177))+x175);
1918 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
1919 {
1920 continue;
1921 }
1922 }
1923 
1924 {
1925 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1926 vinfos[0].jointtype = 1;
1927 vinfos[0].foffset = j0;
1928 vinfos[0].indices[0] = _ij0[0];
1929 vinfos[0].indices[1] = _ij0[1];
1930 vinfos[0].maxsolutions = _nj0;
1931 vinfos[1].jointtype = 1;
1932 vinfos[1].foffset = j1;
1933 vinfos[1].indices[0] = _ij1[0];
1934 vinfos[1].indices[1] = _ij1[1];
1935 vinfos[1].maxsolutions = _nj1;
1936 vinfos[2].jointtype = 1;
1937 vinfos[2].foffset = j2;
1938 vinfos[2].indices[0] = _ij2[0];
1939 vinfos[2].indices[1] = _ij2[1];
1940 vinfos[2].maxsolutions = _nj2;
1941 vinfos[3].jointtype = 1;
1942 vinfos[3].foffset = j3;
1943 vinfos[3].indices[0] = _ij3[0];
1944 vinfos[3].indices[1] = _ij3[1];
1945 vinfos[3].maxsolutions = _nj3;
1946 vinfos[4].jointtype = 1;
1947 vinfos[4].foffset = j4;
1948 vinfos[4].indices[0] = _ij4[0];
1949 vinfos[4].indices[1] = _ij4[1];
1950 vinfos[4].maxsolutions = _nj4;
1951 vinfos[5].jointtype = 1;
1952 vinfos[5].foffset = j5;
1953 vinfos[5].indices[0] = _ij5[0];
1954 vinfos[5].indices[1] = _ij5[1];
1955 vinfos[5].maxsolutions = _nj5;
1956 std::vector<int> vfree(0);
1957 solutions.AddSolution(vinfos,vfree);
1958 }
1959 }
1960 }
1961 
1962 } else
1963 {
1964 {
1965 IkReal j5array[1], cj5array[1], sj5array[1];
1966 bool j5valid[1]={false};
1967 _nj5 = 1;
1968 CheckValue<IkReal> x179=IKPowWithIntegerCheck(IKsign(gconst25),-1);
1969 if(!x179.valid){
1970 continue;
1971 }
1972 CheckValue<IkReal> x180 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1973 if(!x180.valid){
1974 continue;
1975 }
1976 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x179.value)))+(x180.value));
1977 sj5array[0]=IKsin(j5array[0]);
1978 cj5array[0]=IKcos(j5array[0]);
1979 if( j5array[0] > IKPI )
1980 {
1981  j5array[0]-=IK2PI;
1982 }
1983 else if( j5array[0] < -IKPI )
1984 { j5array[0]+=IK2PI;
1985 }
1986 j5valid[0] = true;
1987 for(int ij5 = 0; ij5 < 1; ++ij5)
1988 {
1989 if( !j5valid[ij5] )
1990 {
1991  continue;
1992 }
1993 _ij5[0] = ij5; _ij5[1] = -1;
1994 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1995 {
1996 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1997 {
1998  j5valid[iij5]=false; _ij5[1] = iij5; break;
1999 }
2000 }
2001 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2002 {
2003 IkReal evalcond[8];
2004 IkReal x181=IKcos(j5);
2005 IkReal x182=IKsin(j5);
2006 IkReal x183=((1.0)*gconst25);
2007 if((((1.0)+(((-1.0)*gconst25*x183)))) < -0.00001)
2008 continue;
2009 IkReal x184=IKsqrt(((1.0)+(((-1.0)*gconst25*x183))));
2010 evalcond[0]=x182;
2011 evalcond[1]=((-1.0)*x181);
2012 evalcond[2]=(new_r11+(((-1.0)*x181*x183)));
2013 evalcond[3]=(new_r10+(((-1.0)*x182*x183)));
2014 evalcond[4]=(new_r01+((x181*x184)));
2015 evalcond[5]=(new_r00+((x182*x184)));
2016 evalcond[6]=(((new_r00*x184))+x182+(((-1.0)*new_r10*x183)));
2017 evalcond[7]=(((new_r01*x184))+x181+(((-1.0)*new_r11*x183)));
2018 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2019 {
2020 continue;
2021 }
2022 }
2023 
2024 {
2025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2026 vinfos[0].jointtype = 1;
2027 vinfos[0].foffset = j0;
2028 vinfos[0].indices[0] = _ij0[0];
2029 vinfos[0].indices[1] = _ij0[1];
2030 vinfos[0].maxsolutions = _nj0;
2031 vinfos[1].jointtype = 1;
2032 vinfos[1].foffset = j1;
2033 vinfos[1].indices[0] = _ij1[0];
2034 vinfos[1].indices[1] = _ij1[1];
2035 vinfos[1].maxsolutions = _nj1;
2036 vinfos[2].jointtype = 1;
2037 vinfos[2].foffset = j2;
2038 vinfos[2].indices[0] = _ij2[0];
2039 vinfos[2].indices[1] = _ij2[1];
2040 vinfos[2].maxsolutions = _nj2;
2041 vinfos[3].jointtype = 1;
2042 vinfos[3].foffset = j3;
2043 vinfos[3].indices[0] = _ij3[0];
2044 vinfos[3].indices[1] = _ij3[1];
2045 vinfos[3].maxsolutions = _nj3;
2046 vinfos[4].jointtype = 1;
2047 vinfos[4].foffset = j4;
2048 vinfos[4].indices[0] = _ij4[0];
2049 vinfos[4].indices[1] = _ij4[1];
2050 vinfos[4].maxsolutions = _nj4;
2051 vinfos[5].jointtype = 1;
2052 vinfos[5].foffset = j5;
2053 vinfos[5].indices[0] = _ij5[0];
2054 vinfos[5].indices[1] = _ij5[1];
2055 vinfos[5].maxsolutions = _nj5;
2056 std::vector<int> vfree(0);
2057 solutions.AddSolution(vinfos,vfree);
2058 }
2059 }
2060 }
2061 
2062 }
2063 
2064 }
2065 
2066 }
2067 } while(0);
2068 if( bgotonextstatement )
2069 {
2070 bool bgotonextstatement = true;
2071 do
2072 {
2073 CheckValue<IkReal> x185=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2074 if(!x185.valid){
2075 continue;
2076 }
2077 if((x185.value) < -0.00001)
2078 continue;
2079 IkReal gconst25=IKsqrt(x185.value);
2080 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
2081 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2082 {
2083 bgotonextstatement=false;
2084 {
2085 IkReal j5eval[1];
2086 new_r21=0;
2087 new_r20=0;
2088 new_r02=0;
2089 new_r12=0;
2090 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2091 continue;
2092 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
2093 cj3=gconst25;
2094 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
2095  continue;
2096 j3=((-1.0)*(IKacos(gconst25)));
2097 CheckValue<IkReal> x186=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2098 if(!x186.valid){
2099 continue;
2100 }
2101 if((x186.value) < -0.00001)
2102 continue;
2103 IkReal gconst25=IKsqrt(x186.value);
2104 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2105 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2106 {
2107 {
2108 IkReal j5array[1], cj5array[1], sj5array[1];
2109 bool j5valid[1]={false};
2110 _nj5 = 1;
2111 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2112 continue;
2113 CheckValue<IkReal> x187=IKPowWithIntegerCheck(gconst25,-1);
2114 if(!x187.valid){
2115 continue;
2116 }
2117 if( IKabs((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x187.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x187.value)))-1) <= IKFAST_SINCOS_THRESH )
2118  continue;
2119 j5array[0]=IKatan2((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x187.value)));
2120 sj5array[0]=IKsin(j5array[0]);
2121 cj5array[0]=IKcos(j5array[0]);
2122 if( j5array[0] > IKPI )
2123 {
2124  j5array[0]-=IK2PI;
2125 }
2126 else if( j5array[0] < -IKPI )
2127 { j5array[0]+=IK2PI;
2128 }
2129 j5valid[0] = true;
2130 for(int ij5 = 0; ij5 < 1; ++ij5)
2131 {
2132 if( !j5valid[ij5] )
2133 {
2134  continue;
2135 }
2136 _ij5[0] = ij5; _ij5[1] = -1;
2137 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2138 {
2139 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2140 {
2141  j5valid[iij5]=false; _ij5[1] = iij5; break;
2142 }
2143 }
2144 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2145 {
2146 IkReal evalcond[8];
2147 IkReal x188=IKcos(j5);
2148 IkReal x189=IKsin(j5);
2149 IkReal x190=((1.0)*gconst25);
2150 if((((1.0)+(((-1.0)*gconst25*x190)))) < -0.00001)
2151 continue;
2152 IkReal x191=IKsqrt(((1.0)+(((-1.0)*gconst25*x190))));
2153 IkReal x192=((1.0)*x191);
2154 evalcond[0]=x189;
2155 evalcond[1]=((-1.0)*x188);
2156 evalcond[2]=((((-1.0)*x188*x190))+new_r11);
2157 evalcond[3]=((((-1.0)*x189*x190))+new_r10);
2158 evalcond[4]=((((-1.0)*x188*x192))+new_r01);
2159 evalcond[5]=((((-1.0)*x189*x192))+new_r00);
2160 evalcond[6]=(x189+(((-1.0)*new_r10*x190))+(((-1.0)*new_r00*x192)));
2161 evalcond[7]=(x188+(((-1.0)*new_r11*x190))+(((-1.0)*new_r01*x192)));
2162 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2163 {
2164 continue;
2165 }
2166 }
2167 
2168 {
2169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2170 vinfos[0].jointtype = 1;
2171 vinfos[0].foffset = j0;
2172 vinfos[0].indices[0] = _ij0[0];
2173 vinfos[0].indices[1] = _ij0[1];
2174 vinfos[0].maxsolutions = _nj0;
2175 vinfos[1].jointtype = 1;
2176 vinfos[1].foffset = j1;
2177 vinfos[1].indices[0] = _ij1[0];
2178 vinfos[1].indices[1] = _ij1[1];
2179 vinfos[1].maxsolutions = _nj1;
2180 vinfos[2].jointtype = 1;
2181 vinfos[2].foffset = j2;
2182 vinfos[2].indices[0] = _ij2[0];
2183 vinfos[2].indices[1] = _ij2[1];
2184 vinfos[2].maxsolutions = _nj2;
2185 vinfos[3].jointtype = 1;
2186 vinfos[3].foffset = j3;
2187 vinfos[3].indices[0] = _ij3[0];
2188 vinfos[3].indices[1] = _ij3[1];
2189 vinfos[3].maxsolutions = _nj3;
2190 vinfos[4].jointtype = 1;
2191 vinfos[4].foffset = j4;
2192 vinfos[4].indices[0] = _ij4[0];
2193 vinfos[4].indices[1] = _ij4[1];
2194 vinfos[4].maxsolutions = _nj4;
2195 vinfos[5].jointtype = 1;
2196 vinfos[5].foffset = j5;
2197 vinfos[5].indices[0] = _ij5[0];
2198 vinfos[5].indices[1] = _ij5[1];
2199 vinfos[5].maxsolutions = _nj5;
2200 std::vector<int> vfree(0);
2201 solutions.AddSolution(vinfos,vfree);
2202 }
2203 }
2204 }
2205 
2206 } else
2207 {
2208 {
2209 IkReal j5array[1], cj5array[1], sj5array[1];
2210 bool j5valid[1]={false};
2211 _nj5 = 1;
2212 CheckValue<IkReal> x193=IKPowWithIntegerCheck(IKsign(gconst25),-1);
2213 if(!x193.valid){
2214 continue;
2215 }
2216 CheckValue<IkReal> x194 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2217 if(!x194.valid){
2218 continue;
2219 }
2220 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x193.value)))+(x194.value));
2221 sj5array[0]=IKsin(j5array[0]);
2222 cj5array[0]=IKcos(j5array[0]);
2223 if( j5array[0] > IKPI )
2224 {
2225  j5array[0]-=IK2PI;
2226 }
2227 else if( j5array[0] < -IKPI )
2228 { j5array[0]+=IK2PI;
2229 }
2230 j5valid[0] = true;
2231 for(int ij5 = 0; ij5 < 1; ++ij5)
2232 {
2233 if( !j5valid[ij5] )
2234 {
2235  continue;
2236 }
2237 _ij5[0] = ij5; _ij5[1] = -1;
2238 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2239 {
2240 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2241 {
2242  j5valid[iij5]=false; _ij5[1] = iij5; break;
2243 }
2244 }
2245 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2246 {
2247 IkReal evalcond[8];
2248 IkReal x195=IKcos(j5);
2249 IkReal x196=IKsin(j5);
2250 IkReal x197=((1.0)*gconst25);
2251 if((((1.0)+(((-1.0)*gconst25*x197)))) < -0.00001)
2252 continue;
2253 IkReal x198=IKsqrt(((1.0)+(((-1.0)*gconst25*x197))));
2254 IkReal x199=((1.0)*x198);
2255 evalcond[0]=x196;
2256 evalcond[1]=((-1.0)*x195);
2257 evalcond[2]=((((-1.0)*x195*x197))+new_r11);
2258 evalcond[3]=((((-1.0)*x196*x197))+new_r10);
2259 evalcond[4]=((((-1.0)*x195*x199))+new_r01);
2260 evalcond[5]=((((-1.0)*x196*x199))+new_r00);
2261 evalcond[6]=(x196+(((-1.0)*new_r10*x197))+(((-1.0)*new_r00*x199)));
2262 evalcond[7]=(x195+(((-1.0)*new_r11*x197))+(((-1.0)*new_r01*x199)));
2263 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2264 {
2265 continue;
2266 }
2267 }
2268 
2269 {
2270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2271 vinfos[0].jointtype = 1;
2272 vinfos[0].foffset = j0;
2273 vinfos[0].indices[0] = _ij0[0];
2274 vinfos[0].indices[1] = _ij0[1];
2275 vinfos[0].maxsolutions = _nj0;
2276 vinfos[1].jointtype = 1;
2277 vinfos[1].foffset = j1;
2278 vinfos[1].indices[0] = _ij1[0];
2279 vinfos[1].indices[1] = _ij1[1];
2280 vinfos[1].maxsolutions = _nj1;
2281 vinfos[2].jointtype = 1;
2282 vinfos[2].foffset = j2;
2283 vinfos[2].indices[0] = _ij2[0];
2284 vinfos[2].indices[1] = _ij2[1];
2285 vinfos[2].maxsolutions = _nj2;
2286 vinfos[3].jointtype = 1;
2287 vinfos[3].foffset = j3;
2288 vinfos[3].indices[0] = _ij3[0];
2289 vinfos[3].indices[1] = _ij3[1];
2290 vinfos[3].maxsolutions = _nj3;
2291 vinfos[4].jointtype = 1;
2292 vinfos[4].foffset = j4;
2293 vinfos[4].indices[0] = _ij4[0];
2294 vinfos[4].indices[1] = _ij4[1];
2295 vinfos[4].maxsolutions = _nj4;
2296 vinfos[5].jointtype = 1;
2297 vinfos[5].foffset = j5;
2298 vinfos[5].indices[0] = _ij5[0];
2299 vinfos[5].indices[1] = _ij5[1];
2300 vinfos[5].maxsolutions = _nj5;
2301 std::vector<int> vfree(0);
2302 solutions.AddSolution(vinfos,vfree);
2303 }
2304 }
2305 }
2306 
2307 }
2308 
2309 }
2310 
2311 }
2312 } while(0);
2313 if( bgotonextstatement )
2314 {
2315 bool bgotonextstatement = true;
2316 do
2317 {
2318 if( 1 )
2319 {
2320 bgotonextstatement=false;
2321 continue; // branch miss [j5]
2322 
2323 }
2324 } while(0);
2325 if( bgotonextstatement )
2326 {
2327 }
2328 }
2329 }
2330 }
2331 }
2332 }
2333 }
2334 }
2335 
2336 } else
2337 {
2338 {
2339 IkReal j5array[1], cj5array[1], sj5array[1];
2340 bool j5valid[1]={false};
2341 _nj5 = 1;
2342 IkReal x200=(new_r00*sj3);
2343 CheckValue<IkReal> x201=IKPowWithIntegerCheck(sj3,-1);
2344 if(!x201.valid){
2345 continue;
2346 }
2347 if( IKabs((((cj3*new_r10))+(((-1.0)*x200)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r10))+(((-1.0)*x200))))+IKsqr(((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2348  continue;
2349 j5array[0]=IKatan2((((cj3*new_r10))+(((-1.0)*x200))), ((x201.value)*((((cj3*new_r22*x200))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2350 sj5array[0]=IKsin(j5array[0]);
2351 cj5array[0]=IKcos(j5array[0]);
2352 if( j5array[0] > IKPI )
2353 {
2354  j5array[0]-=IK2PI;
2355 }
2356 else if( j5array[0] < -IKPI )
2357 { j5array[0]+=IK2PI;
2358 }
2359 j5valid[0] = true;
2360 for(int ij5 = 0; ij5 < 1; ++ij5)
2361 {
2362 if( !j5valid[ij5] )
2363 {
2364  continue;
2365 }
2366 _ij5[0] = ij5; _ij5[1] = -1;
2367 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2368 {
2369 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2370 {
2371  j5valid[iij5]=false; _ij5[1] = iij5; break;
2372 }
2373 }
2374 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2375 {
2376 IkReal evalcond[10];
2377 IkReal x202=IKsin(j5);
2378 IkReal x203=IKcos(j5);
2379 IkReal x204=((1.0)*cj3);
2380 IkReal x205=(cj3*new_r00);
2381 IkReal x206=(cj3*new_r22);
2382 IkReal x207=(new_r22*sj3);
2383 IkReal x208=(new_r22*x203);
2384 IkReal x209=(sj3*x202);
2385 evalcond[0]=(((new_r00*sj3))+x202+(((-1.0)*new_r10*x204)));
2386 evalcond[1]=(((new_r01*sj3))+x203+(((-1.0)*new_r11*x204)));
2387 evalcond[2]=(((new_r11*sj3))+((new_r22*x202))+((cj3*new_r01)));
2388 evalcond[3]=(((new_r11*x207))+((new_r01*x206))+x202);
2389 evalcond[4]=(((sj3*x203))+((x202*x206))+new_r01);
2390 evalcond[5]=(((new_r10*sj3))+x205+(((-1.0)*x208)));
2391 evalcond[6]=((((-1.0)*x204*x208))+x209+new_r00);
2392 evalcond[7]=((((-1.0)*x203*x204))+((x202*x207))+new_r11);
2393 evalcond[8]=(((new_r10*x207))+((new_r22*x205))+(((-1.0)*x203)));
2394 evalcond[9]=((((-1.0)*x202*x204))+(((-1.0)*x203*x207))+new_r10);
2395 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2396 {
2397 continue;
2398 }
2399 }
2400 
2401 {
2402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2403 vinfos[0].jointtype = 1;
2404 vinfos[0].foffset = j0;
2405 vinfos[0].indices[0] = _ij0[0];
2406 vinfos[0].indices[1] = _ij0[1];
2407 vinfos[0].maxsolutions = _nj0;
2408 vinfos[1].jointtype = 1;
2409 vinfos[1].foffset = j1;
2410 vinfos[1].indices[0] = _ij1[0];
2411 vinfos[1].indices[1] = _ij1[1];
2412 vinfos[1].maxsolutions = _nj1;
2413 vinfos[2].jointtype = 1;
2414 vinfos[2].foffset = j2;
2415 vinfos[2].indices[0] = _ij2[0];
2416 vinfos[2].indices[1] = _ij2[1];
2417 vinfos[2].maxsolutions = _nj2;
2418 vinfos[3].jointtype = 1;
2419 vinfos[3].foffset = j3;
2420 vinfos[3].indices[0] = _ij3[0];
2421 vinfos[3].indices[1] = _ij3[1];
2422 vinfos[3].maxsolutions = _nj3;
2423 vinfos[4].jointtype = 1;
2424 vinfos[4].foffset = j4;
2425 vinfos[4].indices[0] = _ij4[0];
2426 vinfos[4].indices[1] = _ij4[1];
2427 vinfos[4].maxsolutions = _nj4;
2428 vinfos[5].jointtype = 1;
2429 vinfos[5].foffset = j5;
2430 vinfos[5].indices[0] = _ij5[0];
2431 vinfos[5].indices[1] = _ij5[1];
2432 vinfos[5].maxsolutions = _nj5;
2433 std::vector<int> vfree(0);
2434 solutions.AddSolution(vinfos,vfree);
2435 }
2436 }
2437 }
2438 
2439 }
2440 
2441 }
2442 
2443 } else
2444 {
2445 {
2446 IkReal j5array[1], cj5array[1], sj5array[1];
2447 bool j5valid[1]={false};
2448 _nj5 = 1;
2449 IkReal x210=((1.0)*new_r01);
2450 CheckValue<IkReal> x211=IKPowWithIntegerCheck(new_r22,-1);
2451 if(!x211.valid){
2452 continue;
2453 }
2454 if( IKabs(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x210))+((cj3*new_r11)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3))))))+IKsqr(((((-1.0)*sj3*x210))+((cj3*new_r11))))-1) <= IKFAST_SINCOS_THRESH )
2455  continue;
2456 j5array[0]=IKatan2(((x211.value)*(((((-1.0)*cj3*x210))+(((-1.0)*new_r11*sj3))))), ((((-1.0)*sj3*x210))+((cj3*new_r11))));
2457 sj5array[0]=IKsin(j5array[0]);
2458 cj5array[0]=IKcos(j5array[0]);
2459 if( j5array[0] > IKPI )
2460 {
2461  j5array[0]-=IK2PI;
2462 }
2463 else if( j5array[0] < -IKPI )
2464 { j5array[0]+=IK2PI;
2465 }
2466 j5valid[0] = true;
2467 for(int ij5 = 0; ij5 < 1; ++ij5)
2468 {
2469 if( !j5valid[ij5] )
2470 {
2471  continue;
2472 }
2473 _ij5[0] = ij5; _ij5[1] = -1;
2474 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2475 {
2476 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2477 {
2478  j5valid[iij5]=false; _ij5[1] = iij5; break;
2479 }
2480 }
2481 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2482 {
2483 IkReal evalcond[10];
2484 IkReal x212=IKsin(j5);
2485 IkReal x213=IKcos(j5);
2486 IkReal x214=((1.0)*cj3);
2487 IkReal x215=(cj3*new_r00);
2488 IkReal x216=(cj3*new_r22);
2489 IkReal x217=(new_r22*sj3);
2490 IkReal x218=(new_r22*x213);
2491 IkReal x219=(sj3*x212);
2492 evalcond[0]=(((new_r00*sj3))+(((-1.0)*new_r10*x214))+x212);
2493 evalcond[1]=((((-1.0)*new_r11*x214))+((new_r01*sj3))+x213);
2494 evalcond[2]=(((new_r22*x212))+((new_r11*sj3))+((cj3*new_r01)));
2495 evalcond[3]=(x212+((new_r01*x216))+((new_r11*x217)));
2496 evalcond[4]=(((sj3*x213))+((x212*x216))+new_r01);
2497 evalcond[5]=(((new_r10*sj3))+x215+(((-1.0)*x218)));
2498 evalcond[6]=(x219+(((-1.0)*x214*x218))+new_r00);
2499 evalcond[7]=(((x212*x217))+(((-1.0)*x213*x214))+new_r11);
2500 evalcond[8]=(((new_r22*x215))+(((-1.0)*x213))+((new_r10*x217)));
2501 evalcond[9]=((((-1.0)*x213*x217))+new_r10+(((-1.0)*x212*x214)));
2502 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2503 {
2504 continue;
2505 }
2506 }
2507 
2508 {
2509 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2510 vinfos[0].jointtype = 1;
2511 vinfos[0].foffset = j0;
2512 vinfos[0].indices[0] = _ij0[0];
2513 vinfos[0].indices[1] = _ij0[1];
2514 vinfos[0].maxsolutions = _nj0;
2515 vinfos[1].jointtype = 1;
2516 vinfos[1].foffset = j1;
2517 vinfos[1].indices[0] = _ij1[0];
2518 vinfos[1].indices[1] = _ij1[1];
2519 vinfos[1].maxsolutions = _nj1;
2520 vinfos[2].jointtype = 1;
2521 vinfos[2].foffset = j2;
2522 vinfos[2].indices[0] = _ij2[0];
2523 vinfos[2].indices[1] = _ij2[1];
2524 vinfos[2].maxsolutions = _nj2;
2525 vinfos[3].jointtype = 1;
2526 vinfos[3].foffset = j3;
2527 vinfos[3].indices[0] = _ij3[0];
2528 vinfos[3].indices[1] = _ij3[1];
2529 vinfos[3].maxsolutions = _nj3;
2530 vinfos[4].jointtype = 1;
2531 vinfos[4].foffset = j4;
2532 vinfos[4].indices[0] = _ij4[0];
2533 vinfos[4].indices[1] = _ij4[1];
2534 vinfos[4].maxsolutions = _nj4;
2535 vinfos[5].jointtype = 1;
2536 vinfos[5].foffset = j5;
2537 vinfos[5].indices[0] = _ij5[0];
2538 vinfos[5].indices[1] = _ij5[1];
2539 vinfos[5].maxsolutions = _nj5;
2540 std::vector<int> vfree(0);
2541 solutions.AddSolution(vinfos,vfree);
2542 }
2543 }
2544 }
2545 
2546 }
2547 
2548 }
2549 
2550 } else
2551 {
2552 {
2553 IkReal j5array[1], cj5array[1], sj5array[1];
2554 bool j5valid[1]={false};
2555 _nj5 = 1;
2556 IkReal x220=cj3*cj3;
2557 IkReal x221=(cj3*new_r22);
2558 CheckValue<IkReal> x222=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x220*(new_r22*new_r22)))+x220)),-1);
2559 if(!x222.valid){
2560 continue;
2561 }
2562 CheckValue<IkReal> x223 = IKatan2WithCheck(IkReal((((new_r01*x221))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x221)))),IKFAST_ATAN2_MAGTHRESH);
2563 if(!x223.valid){
2564 continue;
2565 }
2566 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x222.value)))+(x223.value));
2567 sj5array[0]=IKsin(j5array[0]);
2568 cj5array[0]=IKcos(j5array[0]);
2569 if( j5array[0] > IKPI )
2570 {
2571  j5array[0]-=IK2PI;
2572 }
2573 else if( j5array[0] < -IKPI )
2574 { j5array[0]+=IK2PI;
2575 }
2576 j5valid[0] = true;
2577 for(int ij5 = 0; ij5 < 1; ++ij5)
2578 {
2579 if( !j5valid[ij5] )
2580 {
2581  continue;
2582 }
2583 _ij5[0] = ij5; _ij5[1] = -1;
2584 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2585 {
2586 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2587 {
2588  j5valid[iij5]=false; _ij5[1] = iij5; break;
2589 }
2590 }
2591 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2592 {
2593 IkReal evalcond[10];
2594 IkReal x224=IKsin(j5);
2595 IkReal x225=IKcos(j5);
2596 IkReal x226=((1.0)*cj3);
2597 IkReal x227=(cj3*new_r00);
2598 IkReal x228=(cj3*new_r22);
2599 IkReal x229=(new_r22*sj3);
2600 IkReal x230=(new_r22*x225);
2601 IkReal x231=(sj3*x224);
2602 evalcond[0]=((((-1.0)*new_r10*x226))+((new_r00*sj3))+x224);
2603 evalcond[1]=((((-1.0)*new_r11*x226))+((new_r01*sj3))+x225);
2604 evalcond[2]=(((new_r11*sj3))+((new_r22*x224))+((cj3*new_r01)));
2605 evalcond[3]=(((new_r01*x228))+x224+((new_r11*x229)));
2606 evalcond[4]=(((x224*x228))+((sj3*x225))+new_r01);
2607 evalcond[5]=(((new_r10*sj3))+x227+(((-1.0)*x230)));
2608 evalcond[6]=(x231+(((-1.0)*x226*x230))+new_r00);
2609 evalcond[7]=(((x224*x229))+(((-1.0)*x225*x226))+new_r11);
2610 evalcond[8]=(((new_r22*x227))+((new_r10*x229))+(((-1.0)*x225)));
2611 evalcond[9]=((((-1.0)*x224*x226))+(((-1.0)*x225*x229))+new_r10);
2612 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2613 {
2614 continue;
2615 }
2616 }
2617 
2618 {
2619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2620 vinfos[0].jointtype = 1;
2621 vinfos[0].foffset = j0;
2622 vinfos[0].indices[0] = _ij0[0];
2623 vinfos[0].indices[1] = _ij0[1];
2624 vinfos[0].maxsolutions = _nj0;
2625 vinfos[1].jointtype = 1;
2626 vinfos[1].foffset = j1;
2627 vinfos[1].indices[0] = _ij1[0];
2628 vinfos[1].indices[1] = _ij1[1];
2629 vinfos[1].maxsolutions = _nj1;
2630 vinfos[2].jointtype = 1;
2631 vinfos[2].foffset = j2;
2632 vinfos[2].indices[0] = _ij2[0];
2633 vinfos[2].indices[1] = _ij2[1];
2634 vinfos[2].maxsolutions = _nj2;
2635 vinfos[3].jointtype = 1;
2636 vinfos[3].foffset = j3;
2637 vinfos[3].indices[0] = _ij3[0];
2638 vinfos[3].indices[1] = _ij3[1];
2639 vinfos[3].maxsolutions = _nj3;
2640 vinfos[4].jointtype = 1;
2641 vinfos[4].foffset = j4;
2642 vinfos[4].indices[0] = _ij4[0];
2643 vinfos[4].indices[1] = _ij4[1];
2644 vinfos[4].maxsolutions = _nj4;
2645 vinfos[5].jointtype = 1;
2646 vinfos[5].foffset = j5;
2647 vinfos[5].indices[0] = _ij5[0];
2648 vinfos[5].indices[1] = _ij5[1];
2649 vinfos[5].maxsolutions = _nj5;
2650 std::vector<int> vfree(0);
2651 solutions.AddSolution(vinfos,vfree);
2652 }
2653 }
2654 }
2655 
2656 }
2657 
2658 }
2659  }
2660 
2661 }
2662 
2663 }
2664 
2665 }
2666 } while(0);
2667 if( bgotonextstatement )
2668 {
2669 bool bgotonextstatement = true;
2670 do
2671 {
2672 if( 1 )
2673 {
2674 bgotonextstatement=false;
2675 continue; // branch miss [j3, j5]
2676 
2677 }
2678 } while(0);
2679 if( bgotonextstatement )
2680 {
2681 }
2682 }
2683 }
2684 }
2685 }
2686 
2687 } else
2688 {
2689 {
2690 IkReal j3array[1], cj3array[1], sj3array[1];
2691 bool j3valid[1]={false};
2692 _nj3 = 1;
2693 CheckValue<IkReal> x233=IKPowWithIntegerCheck(sj4,-1);
2694 if(!x233.valid){
2695 continue;
2696 }
2697 IkReal x232=x233.value;
2698 CheckValue<IkReal> x234=IKPowWithIntegerCheck(new_r12,-1);
2699 if(!x234.valid){
2700 continue;
2701 }
2702 if( IKabs((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x232)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x232))-1) <= IKFAST_SINCOS_THRESH )
2703  continue;
2704 j3array[0]=IKatan2((x232*(x234.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x232));
2705 sj3array[0]=IKsin(j3array[0]);
2706 cj3array[0]=IKcos(j3array[0]);
2707 if( j3array[0] > IKPI )
2708 {
2709  j3array[0]-=IK2PI;
2710 }
2711 else if( j3array[0] < -IKPI )
2712 { j3array[0]+=IK2PI;
2713 }
2714 j3valid[0] = true;
2715 for(int ij3 = 0; ij3 < 1; ++ij3)
2716 {
2717 if( !j3valid[ij3] )
2718 {
2719  continue;
2720 }
2721 _ij3[0] = ij3; _ij3[1] = -1;
2722 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2723 {
2724 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2725 {
2726  j3valid[iij3]=false; _ij3[1] = iij3; break;
2727 }
2728 }
2729 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2730 {
2731 IkReal evalcond[8];
2732 IkReal x235=IKcos(j3);
2733 IkReal x236=IKsin(j3);
2734 IkReal x237=((1.0)*cj4);
2735 IkReal x238=(new_r02*x235);
2736 IkReal x239=(sj4*x236);
2737 IkReal x240=(sj4*x235);
2738 IkReal x241=(new_r12*x236);
2739 evalcond[0]=(x240+new_r02);
2740 evalcond[1]=(x239+new_r12);
2741 evalcond[2]=((((-1.0)*new_r02*x236))+((new_r12*x235)));
2742 evalcond[3]=(sj4+x238+x241);
2743 evalcond[4]=(((cj4*x241))+((new_r22*sj4))+((cj4*x238)));
2744 evalcond[5]=(((new_r10*x239))+(((-1.0)*new_r20*x237))+((new_r00*x240)));
2745 evalcond[6]=((((-1.0)*new_r21*x237))+((new_r11*x239))+((new_r01*x240)));
2746 evalcond[7]=((1.0)+(((-1.0)*new_r22*x237))+((new_r12*x239))+((sj4*x238)));
2747 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2748 {
2749 continue;
2750 }
2751 }
2752 
2753 {
2754 IkReal j5eval[3];
2755 j5eval[0]=sj4;
2756 j5eval[1]=IKsign(sj4);
2757 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2758 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2759 {
2760 {
2761 IkReal j5eval[2];
2762 j5eval[0]=sj4;
2763 j5eval[1]=sj3;
2764 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2765 {
2766 {
2767 IkReal j5eval[2];
2768 j5eval[0]=sj4;
2769 j5eval[1]=cj3;
2770 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2771 {
2772 {
2773 IkReal evalcond[5];
2774 bool bgotonextstatement = true;
2775 do
2776 {
2777 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2778 evalcond[1]=new_r02;
2779 evalcond[2]=new_r12;
2780 evalcond[3]=new_r21;
2781 evalcond[4]=new_r20;
2782 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 )
2783 {
2784 bgotonextstatement=false;
2785 {
2786 IkReal j5array[1], cj5array[1], sj5array[1];
2787 bool j5valid[1]={false};
2788 _nj5 = 1;
2789 IkReal x242=((1.0)*new_r01);
2790 if( IKabs(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x242))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x242))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2791  continue;
2792 j5array[0]=IKatan2(((((-1.0)*cj3*x242))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x242))+((cj3*new_r00))));
2793 sj5array[0]=IKsin(j5array[0]);
2794 cj5array[0]=IKcos(j5array[0]);
2795 if( j5array[0] > IKPI )
2796 {
2797  j5array[0]-=IK2PI;
2798 }
2799 else if( j5array[0] < -IKPI )
2800 { j5array[0]+=IK2PI;
2801 }
2802 j5valid[0] = true;
2803 for(int ij5 = 0; ij5 < 1; ++ij5)
2804 {
2805 if( !j5valid[ij5] )
2806 {
2807  continue;
2808 }
2809 _ij5[0] = ij5; _ij5[1] = -1;
2810 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2811 {
2812 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2813 {
2814  j5valid[iij5]=false; _ij5[1] = iij5; break;
2815 }
2816 }
2817 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2818 {
2819 IkReal evalcond[8];
2820 IkReal x243=IKsin(j5);
2821 IkReal x244=IKcos(j5);
2822 IkReal x245=((1.0)*cj3);
2823 IkReal x246=(sj3*x243);
2824 IkReal x247=((1.0)*x244);
2825 IkReal x248=(x244*x245);
2826 evalcond[0]=(((new_r11*sj3))+x243+((cj3*new_r01)));
2827 evalcond[1]=(((new_r00*sj3))+x243+(((-1.0)*new_r10*x245)));
2828 evalcond[2]=(((new_r01*sj3))+x244+(((-1.0)*new_r11*x245)));
2829 evalcond[3]=(((sj3*x244))+new_r01+((cj3*x243)));
2830 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x247)));
2831 evalcond[5]=(x246+(((-1.0)*x248))+new_r00);
2832 evalcond[6]=(x246+(((-1.0)*x248))+new_r11);
2833 evalcond[7]=((((-1.0)*sj3*x247))+(((-1.0)*x243*x245))+new_r10);
2834 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2835 {
2836 continue;
2837 }
2838 }
2839 
2840 {
2841 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2842 vinfos[0].jointtype = 1;
2843 vinfos[0].foffset = j0;
2844 vinfos[0].indices[0] = _ij0[0];
2845 vinfos[0].indices[1] = _ij0[1];
2846 vinfos[0].maxsolutions = _nj0;
2847 vinfos[1].jointtype = 1;
2848 vinfos[1].foffset = j1;
2849 vinfos[1].indices[0] = _ij1[0];
2850 vinfos[1].indices[1] = _ij1[1];
2851 vinfos[1].maxsolutions = _nj1;
2852 vinfos[2].jointtype = 1;
2853 vinfos[2].foffset = j2;
2854 vinfos[2].indices[0] = _ij2[0];
2855 vinfos[2].indices[1] = _ij2[1];
2856 vinfos[2].maxsolutions = _nj2;
2857 vinfos[3].jointtype = 1;
2858 vinfos[3].foffset = j3;
2859 vinfos[3].indices[0] = _ij3[0];
2860 vinfos[3].indices[1] = _ij3[1];
2861 vinfos[3].maxsolutions = _nj3;
2862 vinfos[4].jointtype = 1;
2863 vinfos[4].foffset = j4;
2864 vinfos[4].indices[0] = _ij4[0];
2865 vinfos[4].indices[1] = _ij4[1];
2866 vinfos[4].maxsolutions = _nj4;
2867 vinfos[5].jointtype = 1;
2868 vinfos[5].foffset = j5;
2869 vinfos[5].indices[0] = _ij5[0];
2870 vinfos[5].indices[1] = _ij5[1];
2871 vinfos[5].maxsolutions = _nj5;
2872 std::vector<int> vfree(0);
2873 solutions.AddSolution(vinfos,vfree);
2874 }
2875 }
2876 }
2877 
2878 }
2879 } while(0);
2880 if( bgotonextstatement )
2881 {
2882 bool bgotonextstatement = true;
2883 do
2884 {
2885 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2886 evalcond[1]=new_r02;
2887 evalcond[2]=new_r12;
2888 evalcond[3]=new_r21;
2889 evalcond[4]=new_r20;
2890 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 )
2891 {
2892 bgotonextstatement=false;
2893 {
2894 IkReal j5array[1], cj5array[1], sj5array[1];
2895 bool j5valid[1]={false};
2896 _nj5 = 1;
2897 IkReal x249=((1.0)*sj3);
2898 if( IKabs(((((-1.0)*new_r00*x249))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x249))+((cj3*new_r01))))+IKsqr(((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2899  continue;
2900 j5array[0]=IKatan2(((((-1.0)*new_r00*x249))+((cj3*new_r01))), ((((-1.0)*new_r01*x249))+(((-1.0)*cj3*new_r00))));
2901 sj5array[0]=IKsin(j5array[0]);
2902 cj5array[0]=IKcos(j5array[0]);
2903 if( j5array[0] > IKPI )
2904 {
2905  j5array[0]-=IK2PI;
2906 }
2907 else if( j5array[0] < -IKPI )
2908 { j5array[0]+=IK2PI;
2909 }
2910 j5valid[0] = true;
2911 for(int ij5 = 0; ij5 < 1; ++ij5)
2912 {
2913 if( !j5valid[ij5] )
2914 {
2915  continue;
2916 }
2917 _ij5[0] = ij5; _ij5[1] = -1;
2918 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2919 {
2920 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2921 {
2922  j5valid[iij5]=false; _ij5[1] = iij5; break;
2923 }
2924 }
2925 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2926 {
2927 IkReal evalcond[8];
2928 IkReal x250=IKcos(j5);
2929 IkReal x251=IKsin(j5);
2930 IkReal x252=((1.0)*cj3);
2931 IkReal x253=(sj3*x250);
2932 IkReal x254=((1.0)*x251);
2933 IkReal x255=(x251*x252);
2934 evalcond[0]=(((new_r10*sj3))+x250+((cj3*new_r00)));
2935 evalcond[1]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x251);
2936 evalcond[2]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x250);
2937 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x254)));
2938 evalcond[4]=(((cj3*x250))+((sj3*x251))+new_r00);
2939 evalcond[5]=(x253+new_r01+(((-1.0)*x255)));
2940 evalcond[6]=(x253+new_r10+(((-1.0)*x255)));
2941 evalcond[7]=((((-1.0)*x250*x252))+(((-1.0)*sj3*x254))+new_r11);
2942 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
2943 {
2944 continue;
2945 }
2946 }
2947 
2948 {
2949 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2950 vinfos[0].jointtype = 1;
2951 vinfos[0].foffset = j0;
2952 vinfos[0].indices[0] = _ij0[0];
2953 vinfos[0].indices[1] = _ij0[1];
2954 vinfos[0].maxsolutions = _nj0;
2955 vinfos[1].jointtype = 1;
2956 vinfos[1].foffset = j1;
2957 vinfos[1].indices[0] = _ij1[0];
2958 vinfos[1].indices[1] = _ij1[1];
2959 vinfos[1].maxsolutions = _nj1;
2960 vinfos[2].jointtype = 1;
2961 vinfos[2].foffset = j2;
2962 vinfos[2].indices[0] = _ij2[0];
2963 vinfos[2].indices[1] = _ij2[1];
2964 vinfos[2].maxsolutions = _nj2;
2965 vinfos[3].jointtype = 1;
2966 vinfos[3].foffset = j3;
2967 vinfos[3].indices[0] = _ij3[0];
2968 vinfos[3].indices[1] = _ij3[1];
2969 vinfos[3].maxsolutions = _nj3;
2970 vinfos[4].jointtype = 1;
2971 vinfos[4].foffset = j4;
2972 vinfos[4].indices[0] = _ij4[0];
2973 vinfos[4].indices[1] = _ij4[1];
2974 vinfos[4].maxsolutions = _nj4;
2975 vinfos[5].jointtype = 1;
2976 vinfos[5].foffset = j5;
2977 vinfos[5].indices[0] = _ij5[0];
2978 vinfos[5].indices[1] = _ij5[1];
2979 vinfos[5].maxsolutions = _nj5;
2980 std::vector<int> vfree(0);
2981 solutions.AddSolution(vinfos,vfree);
2982 }
2983 }
2984 }
2985 
2986 }
2987 } while(0);
2988 if( bgotonextstatement )
2989 {
2990 bool bgotonextstatement = true;
2991 do
2992 {
2993 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
2994 evalcond[1]=new_r02;
2995 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
2996 {
2997 bgotonextstatement=false;
2998 {
2999 IkReal j5array[1], cj5array[1], sj5array[1];
3000 bool j5valid[1]={false};
3001 _nj5 = 1;
3002 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 )
3003  continue;
3004 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3005 sj5array[0]=IKsin(j5array[0]);
3006 cj5array[0]=IKcos(j5array[0]);
3007 if( j5array[0] > IKPI )
3008 {
3009  j5array[0]-=IK2PI;
3010 }
3011 else if( j5array[0] < -IKPI )
3012 { j5array[0]+=IK2PI;
3013 }
3014 j5valid[0] = true;
3015 for(int ij5 = 0; ij5 < 1; ++ij5)
3016 {
3017 if( !j5valid[ij5] )
3018 {
3019  continue;
3020 }
3021 _ij5[0] = ij5; _ij5[1] = -1;
3022 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3023 {
3024 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3025 {
3026  j5valid[iij5]=false; _ij5[1] = iij5; break;
3027 }
3028 }
3029 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3030 {
3031 IkReal evalcond[8];
3032 IkReal x256=IKsin(j5);
3033 IkReal x257=IKcos(j5);
3034 IkReal x258=((1.0)*x257);
3035 evalcond[0]=(x256+new_r00);
3036 evalcond[1]=(x257+new_r01);
3037 evalcond[2]=(((sj4*x256))+new_r21);
3038 evalcond[3]=(new_r11+((cj4*x256)));
3039 evalcond[4]=((((-1.0)*sj4*x258))+new_r20);
3040 evalcond[5]=((((-1.0)*cj4*x258))+new_r10);
3041 evalcond[6]=(((cj4*new_r11))+x256+((new_r21*sj4)));
3042 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x258)));
3043 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3044 {
3045 continue;
3046 }
3047 }
3048 
3049 {
3050 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3051 vinfos[0].jointtype = 1;
3052 vinfos[0].foffset = j0;
3053 vinfos[0].indices[0] = _ij0[0];
3054 vinfos[0].indices[1] = _ij0[1];
3055 vinfos[0].maxsolutions = _nj0;
3056 vinfos[1].jointtype = 1;
3057 vinfos[1].foffset = j1;
3058 vinfos[1].indices[0] = _ij1[0];
3059 vinfos[1].indices[1] = _ij1[1];
3060 vinfos[1].maxsolutions = _nj1;
3061 vinfos[2].jointtype = 1;
3062 vinfos[2].foffset = j2;
3063 vinfos[2].indices[0] = _ij2[0];
3064 vinfos[2].indices[1] = _ij2[1];
3065 vinfos[2].maxsolutions = _nj2;
3066 vinfos[3].jointtype = 1;
3067 vinfos[3].foffset = j3;
3068 vinfos[3].indices[0] = _ij3[0];
3069 vinfos[3].indices[1] = _ij3[1];
3070 vinfos[3].maxsolutions = _nj3;
3071 vinfos[4].jointtype = 1;
3072 vinfos[4].foffset = j4;
3073 vinfos[4].indices[0] = _ij4[0];
3074 vinfos[4].indices[1] = _ij4[1];
3075 vinfos[4].maxsolutions = _nj4;
3076 vinfos[5].jointtype = 1;
3077 vinfos[5].foffset = j5;
3078 vinfos[5].indices[0] = _ij5[0];
3079 vinfos[5].indices[1] = _ij5[1];
3080 vinfos[5].maxsolutions = _nj5;
3081 std::vector<int> vfree(0);
3082 solutions.AddSolution(vinfos,vfree);
3083 }
3084 }
3085 }
3086 
3087 }
3088 } while(0);
3089 if( bgotonextstatement )
3090 {
3091 bool bgotonextstatement = true;
3092 do
3093 {
3094 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3095 evalcond[1]=new_r02;
3096 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3097 {
3098 bgotonextstatement=false;
3099 {
3100 IkReal j5array[1], cj5array[1], sj5array[1];
3101 bool j5valid[1]={false};
3102 _nj5 = 1;
3103 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3104  continue;
3105 j5array[0]=IKatan2(new_r00, new_r01);
3106 sj5array[0]=IKsin(j5array[0]);
3107 cj5array[0]=IKcos(j5array[0]);
3108 if( j5array[0] > IKPI )
3109 {
3110  j5array[0]-=IK2PI;
3111 }
3112 else if( j5array[0] < -IKPI )
3113 { j5array[0]+=IK2PI;
3114 }
3115 j5valid[0] = true;
3116 for(int ij5 = 0; ij5 < 1; ++ij5)
3117 {
3118 if( !j5valid[ij5] )
3119 {
3120  continue;
3121 }
3122 _ij5[0] = ij5; _ij5[1] = -1;
3123 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3124 {
3125 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3126 {
3127  j5valid[iij5]=false; _ij5[1] = iij5; break;
3128 }
3129 }
3130 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3131 {
3132 IkReal evalcond[8];
3133 IkReal x259=IKsin(j5);
3134 IkReal x260=IKcos(j5);
3135 IkReal x261=((1.0)*cj4);
3136 IkReal x262=((1.0)*x260);
3137 evalcond[0]=(((sj4*x259))+new_r21);
3138 evalcond[1]=(x259+(((-1.0)*new_r00)));
3139 evalcond[2]=(x260+(((-1.0)*new_r01)));
3140 evalcond[3]=((((-1.0)*sj4*x262))+new_r20);
3141 evalcond[4]=((((-1.0)*new_r11))+((cj4*x259)));
3142 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x260*x261)));
3143 evalcond[6]=(x259+(((-1.0)*new_r11*x261))+((new_r21*sj4)));
3144 evalcond[7]=(((new_r20*sj4))+(((-1.0)*new_r10*x261))+(((-1.0)*x262)));
3145 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3146 {
3147 continue;
3148 }
3149 }
3150 
3151 {
3152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3153 vinfos[0].jointtype = 1;
3154 vinfos[0].foffset = j0;
3155 vinfos[0].indices[0] = _ij0[0];
3156 vinfos[0].indices[1] = _ij0[1];
3157 vinfos[0].maxsolutions = _nj0;
3158 vinfos[1].jointtype = 1;
3159 vinfos[1].foffset = j1;
3160 vinfos[1].indices[0] = _ij1[0];
3161 vinfos[1].indices[1] = _ij1[1];
3162 vinfos[1].maxsolutions = _nj1;
3163 vinfos[2].jointtype = 1;
3164 vinfos[2].foffset = j2;
3165 vinfos[2].indices[0] = _ij2[0];
3166 vinfos[2].indices[1] = _ij2[1];
3167 vinfos[2].maxsolutions = _nj2;
3168 vinfos[3].jointtype = 1;
3169 vinfos[3].foffset = j3;
3170 vinfos[3].indices[0] = _ij3[0];
3171 vinfos[3].indices[1] = _ij3[1];
3172 vinfos[3].maxsolutions = _nj3;
3173 vinfos[4].jointtype = 1;
3174 vinfos[4].foffset = j4;
3175 vinfos[4].indices[0] = _ij4[0];
3176 vinfos[4].indices[1] = _ij4[1];
3177 vinfos[4].maxsolutions = _nj4;
3178 vinfos[5].jointtype = 1;
3179 vinfos[5].foffset = j5;
3180 vinfos[5].indices[0] = _ij5[0];
3181 vinfos[5].indices[1] = _ij5[1];
3182 vinfos[5].maxsolutions = _nj5;
3183 std::vector<int> vfree(0);
3184 solutions.AddSolution(vinfos,vfree);
3185 }
3186 }
3187 }
3188 
3189 }
3190 } while(0);
3191 if( bgotonextstatement )
3192 {
3193 bool bgotonextstatement = true;
3194 do
3195 {
3196 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3197 evalcond[1]=new_r12;
3198 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3199 {
3200 bgotonextstatement=false;
3201 {
3202 IkReal j5array[1], cj5array[1], sj5array[1];
3203 bool j5valid[1]={false};
3204 _nj5 = 1;
3205 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3206  continue;
3207 j5array[0]=IKatan2(new_r10, new_r11);
3208 sj5array[0]=IKsin(j5array[0]);
3209 cj5array[0]=IKcos(j5array[0]);
3210 if( j5array[0] > IKPI )
3211 {
3212  j5array[0]-=IK2PI;
3213 }
3214 else if( j5array[0] < -IKPI )
3215 { j5array[0]+=IK2PI;
3216 }
3217 j5valid[0] = true;
3218 for(int ij5 = 0; ij5 < 1; ++ij5)
3219 {
3220 if( !j5valid[ij5] )
3221 {
3222  continue;
3223 }
3224 _ij5[0] = ij5; _ij5[1] = -1;
3225 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3226 {
3227 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3228 {
3229  j5valid[iij5]=false; _ij5[1] = iij5; break;
3230 }
3231 }
3232 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3233 {
3234 IkReal evalcond[8];
3235 IkReal x263=IKcos(j5);
3236 IkReal x264=IKsin(j5);
3237 IkReal x265=((1.0)*x263);
3238 evalcond[0]=(new_r20+((new_r02*x263)));
3239 evalcond[1]=(x264+(((-1.0)*new_r10)));
3240 evalcond[2]=(x263+(((-1.0)*new_r11)));
3241 evalcond[3]=(((cj4*x264))+new_r01);
3242 evalcond[4]=((((-1.0)*new_r02*x264))+new_r21);
3243 evalcond[5]=((((-1.0)*cj4*x265))+new_r00);
3244 evalcond[6]=(((cj4*new_r01))+x264+((new_r21*sj4)));
3245 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x265)));
3246 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3247 {
3248 continue;
3249 }
3250 }
3251 
3252 {
3253 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3254 vinfos[0].jointtype = 1;
3255 vinfos[0].foffset = j0;
3256 vinfos[0].indices[0] = _ij0[0];
3257 vinfos[0].indices[1] = _ij0[1];
3258 vinfos[0].maxsolutions = _nj0;
3259 vinfos[1].jointtype = 1;
3260 vinfos[1].foffset = j1;
3261 vinfos[1].indices[0] = _ij1[0];
3262 vinfos[1].indices[1] = _ij1[1];
3263 vinfos[1].maxsolutions = _nj1;
3264 vinfos[2].jointtype = 1;
3265 vinfos[2].foffset = j2;
3266 vinfos[2].indices[0] = _ij2[0];
3267 vinfos[2].indices[1] = _ij2[1];
3268 vinfos[2].maxsolutions = _nj2;
3269 vinfos[3].jointtype = 1;
3270 vinfos[3].foffset = j3;
3271 vinfos[3].indices[0] = _ij3[0];
3272 vinfos[3].indices[1] = _ij3[1];
3273 vinfos[3].maxsolutions = _nj3;
3274 vinfos[4].jointtype = 1;
3275 vinfos[4].foffset = j4;
3276 vinfos[4].indices[0] = _ij4[0];
3277 vinfos[4].indices[1] = _ij4[1];
3278 vinfos[4].maxsolutions = _nj4;
3279 vinfos[5].jointtype = 1;
3280 vinfos[5].foffset = j5;
3281 vinfos[5].indices[0] = _ij5[0];
3282 vinfos[5].indices[1] = _ij5[1];
3283 vinfos[5].maxsolutions = _nj5;
3284 std::vector<int> vfree(0);
3285 solutions.AddSolution(vinfos,vfree);
3286 }
3287 }
3288 }
3289 
3290 }
3291 } while(0);
3292 if( bgotonextstatement )
3293 {
3294 bool bgotonextstatement = true;
3295 do
3296 {
3297 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3298 evalcond[1]=new_r12;
3299 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3300 {
3301 bgotonextstatement=false;
3302 {
3303 IkReal j5array[1], cj5array[1], sj5array[1];
3304 bool j5valid[1]={false};
3305 _nj5 = 1;
3306 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 )
3307  continue;
3308 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3309 sj5array[0]=IKsin(j5array[0]);
3310 cj5array[0]=IKcos(j5array[0]);
3311 if( j5array[0] > IKPI )
3312 {
3313  j5array[0]-=IK2PI;
3314 }
3315 else if( j5array[0] < -IKPI )
3316 { j5array[0]+=IK2PI;
3317 }
3318 j5valid[0] = true;
3319 for(int ij5 = 0; ij5 < 1; ++ij5)
3320 {
3321 if( !j5valid[ij5] )
3322 {
3323  continue;
3324 }
3325 _ij5[0] = ij5; _ij5[1] = -1;
3326 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3327 {
3328 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3329 {
3330  j5valid[iij5]=false; _ij5[1] = iij5; break;
3331 }
3332 }
3333 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3334 {
3335 IkReal evalcond[8];
3336 IkReal x266=IKsin(j5);
3337 IkReal x267=IKcos(j5);
3338 IkReal x268=((1.0)*new_r00);
3339 IkReal x269=((1.0)*new_r01);
3340 IkReal x270=((1.0)*x267);
3341 evalcond[0]=(x266+new_r10);
3342 evalcond[1]=(x267+new_r11);
3343 evalcond[2]=(new_r21+((new_r02*x266)));
3344 evalcond[3]=((((-1.0)*new_r02*x270))+new_r20);
3345 evalcond[4]=(((cj4*x266))+(((-1.0)*x269)));
3346 evalcond[5]=((((-1.0)*cj4*x270))+(((-1.0)*x268)));
3347 evalcond[6]=((((-1.0)*cj4*x269))+x266+((new_r21*sj4)));
3348 evalcond[7]=((((-1.0)*cj4*x268))+((new_r20*sj4))+(((-1.0)*x270)));
3349 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3350 {
3351 continue;
3352 }
3353 }
3354 
3355 {
3356 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3357 vinfos[0].jointtype = 1;
3358 vinfos[0].foffset = j0;
3359 vinfos[0].indices[0] = _ij0[0];
3360 vinfos[0].indices[1] = _ij0[1];
3361 vinfos[0].maxsolutions = _nj0;
3362 vinfos[1].jointtype = 1;
3363 vinfos[1].foffset = j1;
3364 vinfos[1].indices[0] = _ij1[0];
3365 vinfos[1].indices[1] = _ij1[1];
3366 vinfos[1].maxsolutions = _nj1;
3367 vinfos[2].jointtype = 1;
3368 vinfos[2].foffset = j2;
3369 vinfos[2].indices[0] = _ij2[0];
3370 vinfos[2].indices[1] = _ij2[1];
3371 vinfos[2].maxsolutions = _nj2;
3372 vinfos[3].jointtype = 1;
3373 vinfos[3].foffset = j3;
3374 vinfos[3].indices[0] = _ij3[0];
3375 vinfos[3].indices[1] = _ij3[1];
3376 vinfos[3].maxsolutions = _nj3;
3377 vinfos[4].jointtype = 1;
3378 vinfos[4].foffset = j4;
3379 vinfos[4].indices[0] = _ij4[0];
3380 vinfos[4].indices[1] = _ij4[1];
3381 vinfos[4].maxsolutions = _nj4;
3382 vinfos[5].jointtype = 1;
3383 vinfos[5].foffset = j5;
3384 vinfos[5].indices[0] = _ij5[0];
3385 vinfos[5].indices[1] = _ij5[1];
3386 vinfos[5].maxsolutions = _nj5;
3387 std::vector<int> vfree(0);
3388 solutions.AddSolution(vinfos,vfree);
3389 }
3390 }
3391 }
3392 
3393 }
3394 } while(0);
3395 if( bgotonextstatement )
3396 {
3397 bool bgotonextstatement = true;
3398 do
3399 {
3400 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3401 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3402 {
3403 bgotonextstatement=false;
3404 {
3405 IkReal j5eval[1];
3406 new_r21=0;
3407 new_r20=0;
3408 new_r02=0;
3409 new_r12=0;
3410 j5eval[0]=1.0;
3411 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3412 {
3413 continue; // no branches [j5]
3414 
3415 } else
3416 {
3417 IkReal op[2+1], zeror[2];
3418 int numroots;
3419 op[0]=-1.0;
3420 op[1]=0;
3421 op[2]=1.0;
3422 polyroots2(op,zeror,numroots);
3423 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3424 int numsolutions = 0;
3425 for(int ij5 = 0; ij5 < numroots; ++ij5)
3426 {
3427 IkReal htj5 = zeror[ij5];
3428 tempj5array[0]=((2.0)*(atan(htj5)));
3429 for(int kj5 = 0; kj5 < 1; ++kj5)
3430 {
3431 j5array[numsolutions] = tempj5array[kj5];
3432 if( j5array[numsolutions] > IKPI )
3433 {
3434  j5array[numsolutions]-=IK2PI;
3435 }
3436 else if( j5array[numsolutions] < -IKPI )
3437 {
3438  j5array[numsolutions]+=IK2PI;
3439 }
3440 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3441 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3442 numsolutions++;
3443 }
3444 }
3445 bool j5valid[2]={true,true};
3446 _nj5 = 2;
3447 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3448  {
3449 if( !j5valid[ij5] )
3450 {
3451  continue;
3452 }
3453  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3454 htj5 = IKtan(j5/2);
3455 
3456 _ij5[0] = ij5; _ij5[1] = -1;
3457 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3458 {
3459 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3460 {
3461  j5valid[iij5]=false; _ij5[1] = iij5; break;
3462 }
3463 }
3464 {
3465 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3466 vinfos[0].jointtype = 1;
3467 vinfos[0].foffset = j0;
3468 vinfos[0].indices[0] = _ij0[0];
3469 vinfos[0].indices[1] = _ij0[1];
3470 vinfos[0].maxsolutions = _nj0;
3471 vinfos[1].jointtype = 1;
3472 vinfos[1].foffset = j1;
3473 vinfos[1].indices[0] = _ij1[0];
3474 vinfos[1].indices[1] = _ij1[1];
3475 vinfos[1].maxsolutions = _nj1;
3476 vinfos[2].jointtype = 1;
3477 vinfos[2].foffset = j2;
3478 vinfos[2].indices[0] = _ij2[0];
3479 vinfos[2].indices[1] = _ij2[1];
3480 vinfos[2].maxsolutions = _nj2;
3481 vinfos[3].jointtype = 1;
3482 vinfos[3].foffset = j3;
3483 vinfos[3].indices[0] = _ij3[0];
3484 vinfos[3].indices[1] = _ij3[1];
3485 vinfos[3].maxsolutions = _nj3;
3486 vinfos[4].jointtype = 1;
3487 vinfos[4].foffset = j4;
3488 vinfos[4].indices[0] = _ij4[0];
3489 vinfos[4].indices[1] = _ij4[1];
3490 vinfos[4].maxsolutions = _nj4;
3491 vinfos[5].jointtype = 1;
3492 vinfos[5].foffset = j5;
3493 vinfos[5].indices[0] = _ij5[0];
3494 vinfos[5].indices[1] = _ij5[1];
3495 vinfos[5].maxsolutions = _nj5;
3496 std::vector<int> vfree(0);
3497 solutions.AddSolution(vinfos,vfree);
3498 }
3499  }
3500 
3501 }
3502 
3503 }
3504 
3505 }
3506 } while(0);
3507 if( bgotonextstatement )
3508 {
3509 bool bgotonextstatement = true;
3510 do
3511 {
3512 if( 1 )
3513 {
3514 bgotonextstatement=false;
3515 continue; // branch miss [j5]
3516 
3517 }
3518 } while(0);
3519 if( bgotonextstatement )
3520 {
3521 }
3522 }
3523 }
3524 }
3525 }
3526 }
3527 }
3528 }
3529 }
3530 
3531 } else
3532 {
3533 {
3534 IkReal j5array[1], cj5array[1], sj5array[1];
3535 bool j5valid[1]={false};
3536 _nj5 = 1;
3537 CheckValue<IkReal> x272=IKPowWithIntegerCheck(sj4,-1);
3538 if(!x272.valid){
3539 continue;
3540 }
3541 IkReal x271=x272.value;
3542 CheckValue<IkReal> x273=IKPowWithIntegerCheck(cj3,-1);
3543 if(!x273.valid){
3544 continue;
3545 }
3546 if( IKabs(((-1.0)*new_r21*x271)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x271))+IKsqr((x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3547  continue;
3548 j5array[0]=IKatan2(((-1.0)*new_r21*x271), (x271*(x273.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3549 sj5array[0]=IKsin(j5array[0]);
3550 cj5array[0]=IKcos(j5array[0]);
3551 if( j5array[0] > IKPI )
3552 {
3553  j5array[0]-=IK2PI;
3554 }
3555 else if( j5array[0] < -IKPI )
3556 { j5array[0]+=IK2PI;
3557 }
3558 j5valid[0] = true;
3559 for(int ij5 = 0; ij5 < 1; ++ij5)
3560 {
3561 if( !j5valid[ij5] )
3562 {
3563  continue;
3564 }
3565 _ij5[0] = ij5; _ij5[1] = -1;
3566 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3567 {
3568 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3569 {
3570  j5valid[iij5]=false; _ij5[1] = iij5; break;
3571 }
3572 }
3573 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3574 {
3575 IkReal evalcond[12];
3576 IkReal x274=IKsin(j5);
3577 IkReal x275=IKcos(j5);
3578 IkReal x276=(cj3*new_r00);
3579 IkReal x277=(cj3*cj4);
3580 IkReal x278=(cj4*sj3);
3581 IkReal x279=((1.0)*cj3);
3582 IkReal x280=(sj3*x274);
3583 IkReal x281=((1.0)*x275);
3584 evalcond[0]=(new_r21+((sj4*x274)));
3585 evalcond[1]=((((-1.0)*sj4*x281))+new_r20);
3586 evalcond[2]=(((new_r00*sj3))+x274+(((-1.0)*new_r10*x279)));
3587 evalcond[3]=(((new_r01*sj3))+x275+(((-1.0)*new_r11*x279)));
3588 evalcond[4]=(((cj4*x274))+((new_r11*sj3))+((cj3*new_r01)));
3589 evalcond[5]=(((sj3*x275))+((x274*x277))+new_r01);
3590 evalcond[6]=(((new_r10*sj3))+x276+(((-1.0)*cj4*x281)));
3591 evalcond[7]=((((-1.0)*x277*x281))+x280+new_r00);
3592 evalcond[8]=(((x274*x278))+(((-1.0)*x275*x279))+new_r11);
3593 evalcond[9]=((((-1.0)*x274*x279))+new_r10+(((-1.0)*x278*x281)));
3594 evalcond[10]=(x274+((new_r01*x277))+((new_r21*sj4))+((new_r11*x278)));
3595 evalcond[11]=(((new_r20*sj4))+((cj4*x276))+(((-1.0)*x281))+((new_r10*x278)));
3596 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3597 {
3598 continue;
3599 }
3600 }
3601 
3602 {
3603 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3604 vinfos[0].jointtype = 1;
3605 vinfos[0].foffset = j0;
3606 vinfos[0].indices[0] = _ij0[0];
3607 vinfos[0].indices[1] = _ij0[1];
3608 vinfos[0].maxsolutions = _nj0;
3609 vinfos[1].jointtype = 1;
3610 vinfos[1].foffset = j1;
3611 vinfos[1].indices[0] = _ij1[0];
3612 vinfos[1].indices[1] = _ij1[1];
3613 vinfos[1].maxsolutions = _nj1;
3614 vinfos[2].jointtype = 1;
3615 vinfos[2].foffset = j2;
3616 vinfos[2].indices[0] = _ij2[0];
3617 vinfos[2].indices[1] = _ij2[1];
3618 vinfos[2].maxsolutions = _nj2;
3619 vinfos[3].jointtype = 1;
3620 vinfos[3].foffset = j3;
3621 vinfos[3].indices[0] = _ij3[0];
3622 vinfos[3].indices[1] = _ij3[1];
3623 vinfos[3].maxsolutions = _nj3;
3624 vinfos[4].jointtype = 1;
3625 vinfos[4].foffset = j4;
3626 vinfos[4].indices[0] = _ij4[0];
3627 vinfos[4].indices[1] = _ij4[1];
3628 vinfos[4].maxsolutions = _nj4;
3629 vinfos[5].jointtype = 1;
3630 vinfos[5].foffset = j5;
3631 vinfos[5].indices[0] = _ij5[0];
3632 vinfos[5].indices[1] = _ij5[1];
3633 vinfos[5].maxsolutions = _nj5;
3634 std::vector<int> vfree(0);
3635 solutions.AddSolution(vinfos,vfree);
3636 }
3637 }
3638 }
3639 
3640 }
3641 
3642 }
3643 
3644 } else
3645 {
3646 {
3647 IkReal j5array[1], cj5array[1], sj5array[1];
3648 bool j5valid[1]={false};
3649 _nj5 = 1;
3650 CheckValue<IkReal> x283=IKPowWithIntegerCheck(sj4,-1);
3651 if(!x283.valid){
3652 continue;
3653 }
3654 IkReal x282=x283.value;
3655 CheckValue<IkReal> x284=IKPowWithIntegerCheck(sj3,-1);
3656 if(!x284.valid){
3657 continue;
3658 }
3659 if( IKabs(((-1.0)*new_r21*x282)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x282))+IKsqr((x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3660  continue;
3661 j5array[0]=IKatan2(((-1.0)*new_r21*x282), (x282*(x284.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3662 sj5array[0]=IKsin(j5array[0]);
3663 cj5array[0]=IKcos(j5array[0]);
3664 if( j5array[0] > IKPI )
3665 {
3666  j5array[0]-=IK2PI;
3667 }
3668 else if( j5array[0] < -IKPI )
3669 { j5array[0]+=IK2PI;
3670 }
3671 j5valid[0] = true;
3672 for(int ij5 = 0; ij5 < 1; ++ij5)
3673 {
3674 if( !j5valid[ij5] )
3675 {
3676  continue;
3677 }
3678 _ij5[0] = ij5; _ij5[1] = -1;
3679 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3680 {
3681 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3682 {
3683  j5valid[iij5]=false; _ij5[1] = iij5; break;
3684 }
3685 }
3686 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3687 {
3688 IkReal evalcond[12];
3689 IkReal x285=IKsin(j5);
3690 IkReal x286=IKcos(j5);
3691 IkReal x287=(cj3*new_r00);
3692 IkReal x288=(cj3*cj4);
3693 IkReal x289=(cj4*sj3);
3694 IkReal x290=((1.0)*cj3);
3695 IkReal x291=(sj3*x285);
3696 IkReal x292=((1.0)*x286);
3697 evalcond[0]=(new_r21+((sj4*x285)));
3698 evalcond[1]=(new_r20+(((-1.0)*sj4*x292)));
3699 evalcond[2]=((((-1.0)*new_r10*x290))+((new_r00*sj3))+x285);
3700 evalcond[3]=((((-1.0)*new_r11*x290))+((new_r01*sj3))+x286);
3701 evalcond[4]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x285)));
3702 evalcond[5]=(((x285*x288))+((sj3*x286))+new_r01);
3703 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x292))+x287);
3704 evalcond[7]=(x291+(((-1.0)*x288*x292))+new_r00);
3705 evalcond[8]=(((x285*x289))+(((-1.0)*x286*x290))+new_r11);
3706 evalcond[9]=((((-1.0)*x285*x290))+(((-1.0)*x289*x292))+new_r10);
3707 evalcond[10]=(x285+((new_r21*sj4))+((new_r11*x289))+((new_r01*x288)));
3708 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x292))+((cj4*x287))+((new_r10*x289)));
3709 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3710 {
3711 continue;
3712 }
3713 }
3714 
3715 {
3716 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3717 vinfos[0].jointtype = 1;
3718 vinfos[0].foffset = j0;
3719 vinfos[0].indices[0] = _ij0[0];
3720 vinfos[0].indices[1] = _ij0[1];
3721 vinfos[0].maxsolutions = _nj0;
3722 vinfos[1].jointtype = 1;
3723 vinfos[1].foffset = j1;
3724 vinfos[1].indices[0] = _ij1[0];
3725 vinfos[1].indices[1] = _ij1[1];
3726 vinfos[1].maxsolutions = _nj1;
3727 vinfos[2].jointtype = 1;
3728 vinfos[2].foffset = j2;
3729 vinfos[2].indices[0] = _ij2[0];
3730 vinfos[2].indices[1] = _ij2[1];
3731 vinfos[2].maxsolutions = _nj2;
3732 vinfos[3].jointtype = 1;
3733 vinfos[3].foffset = j3;
3734 vinfos[3].indices[0] = _ij3[0];
3735 vinfos[3].indices[1] = _ij3[1];
3736 vinfos[3].maxsolutions = _nj3;
3737 vinfos[4].jointtype = 1;
3738 vinfos[4].foffset = j4;
3739 vinfos[4].indices[0] = _ij4[0];
3740 vinfos[4].indices[1] = _ij4[1];
3741 vinfos[4].maxsolutions = _nj4;
3742 vinfos[5].jointtype = 1;
3743 vinfos[5].foffset = j5;
3744 vinfos[5].indices[0] = _ij5[0];
3745 vinfos[5].indices[1] = _ij5[1];
3746 vinfos[5].maxsolutions = _nj5;
3747 std::vector<int> vfree(0);
3748 solutions.AddSolution(vinfos,vfree);
3749 }
3750 }
3751 }
3752 
3753 }
3754 
3755 }
3756 
3757 } else
3758 {
3759 {
3760 IkReal j5array[1], cj5array[1], sj5array[1];
3761 bool j5valid[1]={false};
3762 _nj5 = 1;
3763 CheckValue<IkReal> x293 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3764 if(!x293.valid){
3765 continue;
3766 }
3767 CheckValue<IkReal> x294=IKPowWithIntegerCheck(IKsign(sj4),-1);
3768 if(!x294.valid){
3769 continue;
3770 }
3771 j5array[0]=((-1.5707963267949)+(x293.value)+(((1.5707963267949)*(x294.value))));
3772 sj5array[0]=IKsin(j5array[0]);
3773 cj5array[0]=IKcos(j5array[0]);
3774 if( j5array[0] > IKPI )
3775 {
3776  j5array[0]-=IK2PI;
3777 }
3778 else if( j5array[0] < -IKPI )
3779 { j5array[0]+=IK2PI;
3780 }
3781 j5valid[0] = true;
3782 for(int ij5 = 0; ij5 < 1; ++ij5)
3783 {
3784 if( !j5valid[ij5] )
3785 {
3786  continue;
3787 }
3788 _ij5[0] = ij5; _ij5[1] = -1;
3789 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3790 {
3791 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3792 {
3793  j5valid[iij5]=false; _ij5[1] = iij5; break;
3794 }
3795 }
3796 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3797 {
3798 IkReal evalcond[12];
3799 IkReal x295=IKsin(j5);
3800 IkReal x296=IKcos(j5);
3801 IkReal x297=(cj3*new_r00);
3802 IkReal x298=(cj3*cj4);
3803 IkReal x299=(cj4*sj3);
3804 IkReal x300=((1.0)*cj3);
3805 IkReal x301=(sj3*x295);
3806 IkReal x302=((1.0)*x296);
3807 evalcond[0]=(((sj4*x295))+new_r21);
3808 evalcond[1]=((((-1.0)*sj4*x302))+new_r20);
3809 evalcond[2]=(((new_r00*sj3))+x295+(((-1.0)*new_r10*x300)));
3810 evalcond[3]=(((new_r01*sj3))+x296+(((-1.0)*new_r11*x300)));
3811 evalcond[4]=(((new_r11*sj3))+((cj4*x295))+((cj3*new_r01)));
3812 evalcond[5]=(((x295*x298))+new_r01+((sj3*x296)));
3813 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x302))+x297);
3814 evalcond[7]=((((-1.0)*x298*x302))+x301+new_r00);
3815 evalcond[8]=((((-1.0)*x296*x300))+((x295*x299))+new_r11);
3816 evalcond[9]=((((-1.0)*x295*x300))+(((-1.0)*x299*x302))+new_r10);
3817 evalcond[10]=(((new_r11*x299))+x295+((new_r01*x298))+((new_r21*sj4)));
3818 evalcond[11]=(((new_r20*sj4))+((new_r10*x299))+((cj4*x297))+(((-1.0)*x302)));
3819 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3820 {
3821 continue;
3822 }
3823 }
3824 
3825 {
3826 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3827 vinfos[0].jointtype = 1;
3828 vinfos[0].foffset = j0;
3829 vinfos[0].indices[0] = _ij0[0];
3830 vinfos[0].indices[1] = _ij0[1];
3831 vinfos[0].maxsolutions = _nj0;
3832 vinfos[1].jointtype = 1;
3833 vinfos[1].foffset = j1;
3834 vinfos[1].indices[0] = _ij1[0];
3835 vinfos[1].indices[1] = _ij1[1];
3836 vinfos[1].maxsolutions = _nj1;
3837 vinfos[2].jointtype = 1;
3838 vinfos[2].foffset = j2;
3839 vinfos[2].indices[0] = _ij2[0];
3840 vinfos[2].indices[1] = _ij2[1];
3841 vinfos[2].maxsolutions = _nj2;
3842 vinfos[3].jointtype = 1;
3843 vinfos[3].foffset = j3;
3844 vinfos[3].indices[0] = _ij3[0];
3845 vinfos[3].indices[1] = _ij3[1];
3846 vinfos[3].maxsolutions = _nj3;
3847 vinfos[4].jointtype = 1;
3848 vinfos[4].foffset = j4;
3849 vinfos[4].indices[0] = _ij4[0];
3850 vinfos[4].indices[1] = _ij4[1];
3851 vinfos[4].maxsolutions = _nj4;
3852 vinfos[5].jointtype = 1;
3853 vinfos[5].foffset = j5;
3854 vinfos[5].indices[0] = _ij5[0];
3855 vinfos[5].indices[1] = _ij5[1];
3856 vinfos[5].maxsolutions = _nj5;
3857 std::vector<int> vfree(0);
3858 solutions.AddSolution(vinfos,vfree);
3859 }
3860 }
3861 }
3862 
3863 }
3864 
3865 }
3866 }
3867 }
3868 
3869 }
3870 
3871 }
3872 
3873 } else
3874 {
3875 {
3876 IkReal j3array[1], cj3array[1], sj3array[1];
3877 bool j3valid[1]={false};
3878 _nj3 = 1;
3879 CheckValue<IkReal> x303=IKPowWithIntegerCheck(IKsign(sj4),-1);
3880 if(!x303.valid){
3881 continue;
3882 }
3883 CheckValue<IkReal> x304 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3884 if(!x304.valid){
3885 continue;
3886 }
3887 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x303.value)))+(x304.value));
3888 sj3array[0]=IKsin(j3array[0]);
3889 cj3array[0]=IKcos(j3array[0]);
3890 if( j3array[0] > IKPI )
3891 {
3892  j3array[0]-=IK2PI;
3893 }
3894 else if( j3array[0] < -IKPI )
3895 { j3array[0]+=IK2PI;
3896 }
3897 j3valid[0] = true;
3898 for(int ij3 = 0; ij3 < 1; ++ij3)
3899 {
3900 if( !j3valid[ij3] )
3901 {
3902  continue;
3903 }
3904 _ij3[0] = ij3; _ij3[1] = -1;
3905 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3906 {
3907 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3908 {
3909  j3valid[iij3]=false; _ij3[1] = iij3; break;
3910 }
3911 }
3912 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3913 {
3914 IkReal evalcond[8];
3915 IkReal x305=IKcos(j3);
3916 IkReal x306=IKsin(j3);
3917 IkReal x307=((1.0)*cj4);
3918 IkReal x308=(new_r02*x305);
3919 IkReal x309=(sj4*x306);
3920 IkReal x310=(sj4*x305);
3921 IkReal x311=(new_r12*x306);
3922 evalcond[0]=(x310+new_r02);
3923 evalcond[1]=(x309+new_r12);
3924 evalcond[2]=((((-1.0)*new_r02*x306))+((new_r12*x305)));
3925 evalcond[3]=(sj4+x308+x311);
3926 evalcond[4]=(((cj4*x311))+((new_r22*sj4))+((cj4*x308)));
3927 evalcond[5]=(((new_r00*x310))+((new_r10*x309))+(((-1.0)*new_r20*x307)));
3928 evalcond[6]=(((new_r11*x309))+(((-1.0)*new_r21*x307))+((new_r01*x310)));
3929 evalcond[7]=((1.0)+(((-1.0)*new_r22*x307))+((new_r12*x309))+((sj4*x308)));
3930 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
3931 {
3932 continue;
3933 }
3934 }
3935 
3936 {
3937 IkReal j5eval[3];
3938 j5eval[0]=sj4;
3939 j5eval[1]=IKsign(sj4);
3940 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
3941 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
3942 {
3943 {
3944 IkReal j5eval[2];
3945 j5eval[0]=sj4;
3946 j5eval[1]=sj3;
3947 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3948 {
3949 {
3950 IkReal j5eval[2];
3951 j5eval[0]=sj4;
3952 j5eval[1]=cj3;
3953 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3954 {
3955 {
3956 IkReal evalcond[5];
3957 bool bgotonextstatement = true;
3958 do
3959 {
3960 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
3961 evalcond[1]=new_r02;
3962 evalcond[2]=new_r12;
3963 evalcond[3]=new_r21;
3964 evalcond[4]=new_r20;
3965 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 )
3966 {
3967 bgotonextstatement=false;
3968 {
3969 IkReal j5array[1], cj5array[1], sj5array[1];
3970 bool j5valid[1]={false};
3971 _nj5 = 1;
3972 IkReal x312=((1.0)*new_r01);
3973 if( IKabs(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x312))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312))))+IKsqr(((((-1.0)*sj3*x312))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
3974  continue;
3975 j5array[0]=IKatan2(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x312))), ((((-1.0)*sj3*x312))+((cj3*new_r00))));
3976 sj5array[0]=IKsin(j5array[0]);
3977 cj5array[0]=IKcos(j5array[0]);
3978 if( j5array[0] > IKPI )
3979 {
3980  j5array[0]-=IK2PI;
3981 }
3982 else if( j5array[0] < -IKPI )
3983 { j5array[0]+=IK2PI;
3984 }
3985 j5valid[0] = true;
3986 for(int ij5 = 0; ij5 < 1; ++ij5)
3987 {
3988 if( !j5valid[ij5] )
3989 {
3990  continue;
3991 }
3992 _ij5[0] = ij5; _ij5[1] = -1;
3993 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3994 {
3995 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3996 {
3997  j5valid[iij5]=false; _ij5[1] = iij5; break;
3998 }
3999 }
4000 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4001 {
4002 IkReal evalcond[8];
4003 IkReal x313=IKsin(j5);
4004 IkReal x314=IKcos(j5);
4005 IkReal x315=((1.0)*cj3);
4006 IkReal x316=(sj3*x313);
4007 IkReal x317=((1.0)*x314);
4008 IkReal x318=(x314*x315);
4009 evalcond[0]=(((new_r11*sj3))+x313+((cj3*new_r01)));
4010 evalcond[1]=(((new_r00*sj3))+x313+(((-1.0)*new_r10*x315)));
4011 evalcond[2]=(((new_r01*sj3))+(((-1.0)*new_r11*x315))+x314);
4012 evalcond[3]=(((cj3*x313))+new_r01+((sj3*x314)));
4013 evalcond[4]=(((new_r10*sj3))+(((-1.0)*x317))+((cj3*new_r00)));
4014 evalcond[5]=(x316+(((-1.0)*x318))+new_r00);
4015 evalcond[6]=(x316+(((-1.0)*x318))+new_r11);
4016 evalcond[7]=((((-1.0)*x313*x315))+(((-1.0)*sj3*x317))+new_r10);
4017 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4018 {
4019 continue;
4020 }
4021 }
4022 
4023 {
4024 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4025 vinfos[0].jointtype = 1;
4026 vinfos[0].foffset = j0;
4027 vinfos[0].indices[0] = _ij0[0];
4028 vinfos[0].indices[1] = _ij0[1];
4029 vinfos[0].maxsolutions = _nj0;
4030 vinfos[1].jointtype = 1;
4031 vinfos[1].foffset = j1;
4032 vinfos[1].indices[0] = _ij1[0];
4033 vinfos[1].indices[1] = _ij1[1];
4034 vinfos[1].maxsolutions = _nj1;
4035 vinfos[2].jointtype = 1;
4036 vinfos[2].foffset = j2;
4037 vinfos[2].indices[0] = _ij2[0];
4038 vinfos[2].indices[1] = _ij2[1];
4039 vinfos[2].maxsolutions = _nj2;
4040 vinfos[3].jointtype = 1;
4041 vinfos[3].foffset = j3;
4042 vinfos[3].indices[0] = _ij3[0];
4043 vinfos[3].indices[1] = _ij3[1];
4044 vinfos[3].maxsolutions = _nj3;
4045 vinfos[4].jointtype = 1;
4046 vinfos[4].foffset = j4;
4047 vinfos[4].indices[0] = _ij4[0];
4048 vinfos[4].indices[1] = _ij4[1];
4049 vinfos[4].maxsolutions = _nj4;
4050 vinfos[5].jointtype = 1;
4051 vinfos[5].foffset = j5;
4052 vinfos[5].indices[0] = _ij5[0];
4053 vinfos[5].indices[1] = _ij5[1];
4054 vinfos[5].maxsolutions = _nj5;
4055 std::vector<int> vfree(0);
4056 solutions.AddSolution(vinfos,vfree);
4057 }
4058 }
4059 }
4060 
4061 }
4062 } while(0);
4063 if( bgotonextstatement )
4064 {
4065 bool bgotonextstatement = true;
4066 do
4067 {
4068 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4069 evalcond[1]=new_r02;
4070 evalcond[2]=new_r12;
4071 evalcond[3]=new_r21;
4072 evalcond[4]=new_r20;
4073 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 )
4074 {
4075 bgotonextstatement=false;
4076 {
4077 IkReal j5array[1], cj5array[1], sj5array[1];
4078 bool j5valid[1]={false};
4079 _nj5 = 1;
4080 IkReal x319=((1.0)*sj3);
4081 if( IKabs(((((-1.0)*new_r00*x319))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x319))+((cj3*new_r01))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319))))-1) <= IKFAST_SINCOS_THRESH )
4082  continue;
4083 j5array[0]=IKatan2(((((-1.0)*new_r00*x319))+((cj3*new_r01))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x319))));
4084 sj5array[0]=IKsin(j5array[0]);
4085 cj5array[0]=IKcos(j5array[0]);
4086 if( j5array[0] > IKPI )
4087 {
4088  j5array[0]-=IK2PI;
4089 }
4090 else if( j5array[0] < -IKPI )
4091 { j5array[0]+=IK2PI;
4092 }
4093 j5valid[0] = true;
4094 for(int ij5 = 0; ij5 < 1; ++ij5)
4095 {
4096 if( !j5valid[ij5] )
4097 {
4098  continue;
4099 }
4100 _ij5[0] = ij5; _ij5[1] = -1;
4101 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4102 {
4103 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4104 {
4105  j5valid[iij5]=false; _ij5[1] = iij5; break;
4106 }
4107 }
4108 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4109 {
4110 IkReal evalcond[8];
4111 IkReal x320=IKcos(j5);
4112 IkReal x321=IKsin(j5);
4113 IkReal x322=((1.0)*cj3);
4114 IkReal x323=(sj3*x320);
4115 IkReal x324=((1.0)*x321);
4116 IkReal x325=(x321*x322);
4117 evalcond[0]=(((new_r10*sj3))+x320+((cj3*new_r00)));
4118 evalcond[1]=(((new_r00*sj3))+x321+(((-1.0)*new_r10*x322)));
4119 evalcond[2]=(((new_r01*sj3))+x320+(((-1.0)*new_r11*x322)));
4120 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x324))+((cj3*new_r01)));
4121 evalcond[4]=(((sj3*x321))+new_r00+((cj3*x320)));
4122 evalcond[5]=((((-1.0)*x325))+x323+new_r01);
4123 evalcond[6]=((((-1.0)*x325))+x323+new_r10);
4124 evalcond[7]=((((-1.0)*x320*x322))+new_r11+(((-1.0)*sj3*x324)));
4125 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4126 {
4127 continue;
4128 }
4129 }
4130 
4131 {
4132 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4133 vinfos[0].jointtype = 1;
4134 vinfos[0].foffset = j0;
4135 vinfos[0].indices[0] = _ij0[0];
4136 vinfos[0].indices[1] = _ij0[1];
4137 vinfos[0].maxsolutions = _nj0;
4138 vinfos[1].jointtype = 1;
4139 vinfos[1].foffset = j1;
4140 vinfos[1].indices[0] = _ij1[0];
4141 vinfos[1].indices[1] = _ij1[1];
4142 vinfos[1].maxsolutions = _nj1;
4143 vinfos[2].jointtype = 1;
4144 vinfos[2].foffset = j2;
4145 vinfos[2].indices[0] = _ij2[0];
4146 vinfos[2].indices[1] = _ij2[1];
4147 vinfos[2].maxsolutions = _nj2;
4148 vinfos[3].jointtype = 1;
4149 vinfos[3].foffset = j3;
4150 vinfos[3].indices[0] = _ij3[0];
4151 vinfos[3].indices[1] = _ij3[1];
4152 vinfos[3].maxsolutions = _nj3;
4153 vinfos[4].jointtype = 1;
4154 vinfos[4].foffset = j4;
4155 vinfos[4].indices[0] = _ij4[0];
4156 vinfos[4].indices[1] = _ij4[1];
4157 vinfos[4].maxsolutions = _nj4;
4158 vinfos[5].jointtype = 1;
4159 vinfos[5].foffset = j5;
4160 vinfos[5].indices[0] = _ij5[0];
4161 vinfos[5].indices[1] = _ij5[1];
4162 vinfos[5].maxsolutions = _nj5;
4163 std::vector<int> vfree(0);
4164 solutions.AddSolution(vinfos,vfree);
4165 }
4166 }
4167 }
4168 
4169 }
4170 } while(0);
4171 if( bgotonextstatement )
4172 {
4173 bool bgotonextstatement = true;
4174 do
4175 {
4176 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4177 evalcond[1]=new_r02;
4178 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4179 {
4180 bgotonextstatement=false;
4181 {
4182 IkReal j5array[1], cj5array[1], sj5array[1];
4183 bool j5valid[1]={false};
4184 _nj5 = 1;
4185 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 )
4186  continue;
4187 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4188 sj5array[0]=IKsin(j5array[0]);
4189 cj5array[0]=IKcos(j5array[0]);
4190 if( j5array[0] > IKPI )
4191 {
4192  j5array[0]-=IK2PI;
4193 }
4194 else if( j5array[0] < -IKPI )
4195 { j5array[0]+=IK2PI;
4196 }
4197 j5valid[0] = true;
4198 for(int ij5 = 0; ij5 < 1; ++ij5)
4199 {
4200 if( !j5valid[ij5] )
4201 {
4202  continue;
4203 }
4204 _ij5[0] = ij5; _ij5[1] = -1;
4205 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4206 {
4207 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4208 {
4209  j5valid[iij5]=false; _ij5[1] = iij5; break;
4210 }
4211 }
4212 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4213 {
4214 IkReal evalcond[8];
4215 IkReal x326=IKsin(j5);
4216 IkReal x327=IKcos(j5);
4217 IkReal x328=((1.0)*x327);
4218 evalcond[0]=(x326+new_r00);
4219 evalcond[1]=(x327+new_r01);
4220 evalcond[2]=(((sj4*x326))+new_r21);
4221 evalcond[3]=(((cj4*x326))+new_r11);
4222 evalcond[4]=(new_r20+(((-1.0)*sj4*x328)));
4223 evalcond[5]=(new_r10+(((-1.0)*cj4*x328)));
4224 evalcond[6]=(((cj4*new_r11))+x326+((new_r21*sj4)));
4225 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x328)));
4226 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4227 {
4228 continue;
4229 }
4230 }
4231 
4232 {
4233 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4234 vinfos[0].jointtype = 1;
4235 vinfos[0].foffset = j0;
4236 vinfos[0].indices[0] = _ij0[0];
4237 vinfos[0].indices[1] = _ij0[1];
4238 vinfos[0].maxsolutions = _nj0;
4239 vinfos[1].jointtype = 1;
4240 vinfos[1].foffset = j1;
4241 vinfos[1].indices[0] = _ij1[0];
4242 vinfos[1].indices[1] = _ij1[1];
4243 vinfos[1].maxsolutions = _nj1;
4244 vinfos[2].jointtype = 1;
4245 vinfos[2].foffset = j2;
4246 vinfos[2].indices[0] = _ij2[0];
4247 vinfos[2].indices[1] = _ij2[1];
4248 vinfos[2].maxsolutions = _nj2;
4249 vinfos[3].jointtype = 1;
4250 vinfos[3].foffset = j3;
4251 vinfos[3].indices[0] = _ij3[0];
4252 vinfos[3].indices[1] = _ij3[1];
4253 vinfos[3].maxsolutions = _nj3;
4254 vinfos[4].jointtype = 1;
4255 vinfos[4].foffset = j4;
4256 vinfos[4].indices[0] = _ij4[0];
4257 vinfos[4].indices[1] = _ij4[1];
4258 vinfos[4].maxsolutions = _nj4;
4259 vinfos[5].jointtype = 1;
4260 vinfos[5].foffset = j5;
4261 vinfos[5].indices[0] = _ij5[0];
4262 vinfos[5].indices[1] = _ij5[1];
4263 vinfos[5].maxsolutions = _nj5;
4264 std::vector<int> vfree(0);
4265 solutions.AddSolution(vinfos,vfree);
4266 }
4267 }
4268 }
4269 
4270 }
4271 } while(0);
4272 if( bgotonextstatement )
4273 {
4274 bool bgotonextstatement = true;
4275 do
4276 {
4277 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4278 evalcond[1]=new_r02;
4279 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4280 {
4281 bgotonextstatement=false;
4282 {
4283 IkReal j5array[1], cj5array[1], sj5array[1];
4284 bool j5valid[1]={false};
4285 _nj5 = 1;
4286 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4287  continue;
4288 j5array[0]=IKatan2(new_r00, new_r01);
4289 sj5array[0]=IKsin(j5array[0]);
4290 cj5array[0]=IKcos(j5array[0]);
4291 if( j5array[0] > IKPI )
4292 {
4293  j5array[0]-=IK2PI;
4294 }
4295 else if( j5array[0] < -IKPI )
4296 { j5array[0]+=IK2PI;
4297 }
4298 j5valid[0] = true;
4299 for(int ij5 = 0; ij5 < 1; ++ij5)
4300 {
4301 if( !j5valid[ij5] )
4302 {
4303  continue;
4304 }
4305 _ij5[0] = ij5; _ij5[1] = -1;
4306 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4307 {
4308 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4309 {
4310  j5valid[iij5]=false; _ij5[1] = iij5; break;
4311 }
4312 }
4313 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4314 {
4315 IkReal evalcond[8];
4316 IkReal x329=IKsin(j5);
4317 IkReal x330=IKcos(j5);
4318 IkReal x331=((1.0)*cj4);
4319 IkReal x332=((1.0)*x330);
4320 evalcond[0]=(((sj4*x329))+new_r21);
4321 evalcond[1]=(x329+(((-1.0)*new_r00)));
4322 evalcond[2]=(x330+(((-1.0)*new_r01)));
4323 evalcond[3]=((((-1.0)*sj4*x332))+new_r20);
4324 evalcond[4]=(((cj4*x329))+(((-1.0)*new_r11)));
4325 evalcond[5]=((((-1.0)*x330*x331))+(((-1.0)*new_r10)));
4326 evalcond[6]=((((-1.0)*new_r11*x331))+x329+((new_r21*sj4)));
4327 evalcond[7]=((((-1.0)*new_r10*x331))+((new_r20*sj4))+(((-1.0)*x332)));
4328 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4329 {
4330 continue;
4331 }
4332 }
4333 
4334 {
4335 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4336 vinfos[0].jointtype = 1;
4337 vinfos[0].foffset = j0;
4338 vinfos[0].indices[0] = _ij0[0];
4339 vinfos[0].indices[1] = _ij0[1];
4340 vinfos[0].maxsolutions = _nj0;
4341 vinfos[1].jointtype = 1;
4342 vinfos[1].foffset = j1;
4343 vinfos[1].indices[0] = _ij1[0];
4344 vinfos[1].indices[1] = _ij1[1];
4345 vinfos[1].maxsolutions = _nj1;
4346 vinfos[2].jointtype = 1;
4347 vinfos[2].foffset = j2;
4348 vinfos[2].indices[0] = _ij2[0];
4349 vinfos[2].indices[1] = _ij2[1];
4350 vinfos[2].maxsolutions = _nj2;
4351 vinfos[3].jointtype = 1;
4352 vinfos[3].foffset = j3;
4353 vinfos[3].indices[0] = _ij3[0];
4354 vinfos[3].indices[1] = _ij3[1];
4355 vinfos[3].maxsolutions = _nj3;
4356 vinfos[4].jointtype = 1;
4357 vinfos[4].foffset = j4;
4358 vinfos[4].indices[0] = _ij4[0];
4359 vinfos[4].indices[1] = _ij4[1];
4360 vinfos[4].maxsolutions = _nj4;
4361 vinfos[5].jointtype = 1;
4362 vinfos[5].foffset = j5;
4363 vinfos[5].indices[0] = _ij5[0];
4364 vinfos[5].indices[1] = _ij5[1];
4365 vinfos[5].maxsolutions = _nj5;
4366 std::vector<int> vfree(0);
4367 solutions.AddSolution(vinfos,vfree);
4368 }
4369 }
4370 }
4371 
4372 }
4373 } while(0);
4374 if( bgotonextstatement )
4375 {
4376 bool bgotonextstatement = true;
4377 do
4378 {
4379 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4380 evalcond[1]=new_r12;
4381 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4382 {
4383 bgotonextstatement=false;
4384 {
4385 IkReal j5array[1], cj5array[1], sj5array[1];
4386 bool j5valid[1]={false};
4387 _nj5 = 1;
4388 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4389  continue;
4390 j5array[0]=IKatan2(new_r10, new_r11);
4391 sj5array[0]=IKsin(j5array[0]);
4392 cj5array[0]=IKcos(j5array[0]);
4393 if( j5array[0] > IKPI )
4394 {
4395  j5array[0]-=IK2PI;
4396 }
4397 else if( j5array[0] < -IKPI )
4398 { j5array[0]+=IK2PI;
4399 }
4400 j5valid[0] = true;
4401 for(int ij5 = 0; ij5 < 1; ++ij5)
4402 {
4403 if( !j5valid[ij5] )
4404 {
4405  continue;
4406 }
4407 _ij5[0] = ij5; _ij5[1] = -1;
4408 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4409 {
4410 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4411 {
4412  j5valid[iij5]=false; _ij5[1] = iij5; break;
4413 }
4414 }
4415 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4416 {
4417 IkReal evalcond[8];
4418 IkReal x333=IKcos(j5);
4419 IkReal x334=IKsin(j5);
4420 IkReal x335=((1.0)*x333);
4421 evalcond[0]=(((new_r02*x333))+new_r20);
4422 evalcond[1]=(x334+(((-1.0)*new_r10)));
4423 evalcond[2]=(x333+(((-1.0)*new_r11)));
4424 evalcond[3]=(((cj4*x334))+new_r01);
4425 evalcond[4]=((((-1.0)*new_r02*x334))+new_r21);
4426 evalcond[5]=((((-1.0)*cj4*x335))+new_r00);
4427 evalcond[6]=(((cj4*new_r01))+x334+((new_r21*sj4)));
4428 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x335)));
4429 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4430 {
4431 continue;
4432 }
4433 }
4434 
4435 {
4436 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4437 vinfos[0].jointtype = 1;
4438 vinfos[0].foffset = j0;
4439 vinfos[0].indices[0] = _ij0[0];
4440 vinfos[0].indices[1] = _ij0[1];
4441 vinfos[0].maxsolutions = _nj0;
4442 vinfos[1].jointtype = 1;
4443 vinfos[1].foffset = j1;
4444 vinfos[1].indices[0] = _ij1[0];
4445 vinfos[1].indices[1] = _ij1[1];
4446 vinfos[1].maxsolutions = _nj1;
4447 vinfos[2].jointtype = 1;
4448 vinfos[2].foffset = j2;
4449 vinfos[2].indices[0] = _ij2[0];
4450 vinfos[2].indices[1] = _ij2[1];
4451 vinfos[2].maxsolutions = _nj2;
4452 vinfos[3].jointtype = 1;
4453 vinfos[3].foffset = j3;
4454 vinfos[3].indices[0] = _ij3[0];
4455 vinfos[3].indices[1] = _ij3[1];
4456 vinfos[3].maxsolutions = _nj3;
4457 vinfos[4].jointtype = 1;
4458 vinfos[4].foffset = j4;
4459 vinfos[4].indices[0] = _ij4[0];
4460 vinfos[4].indices[1] = _ij4[1];
4461 vinfos[4].maxsolutions = _nj4;
4462 vinfos[5].jointtype = 1;
4463 vinfos[5].foffset = j5;
4464 vinfos[5].indices[0] = _ij5[0];
4465 vinfos[5].indices[1] = _ij5[1];
4466 vinfos[5].maxsolutions = _nj5;
4467 std::vector<int> vfree(0);
4468 solutions.AddSolution(vinfos,vfree);
4469 }
4470 }
4471 }
4472 
4473 }
4474 } while(0);
4475 if( bgotonextstatement )
4476 {
4477 bool bgotonextstatement = true;
4478 do
4479 {
4480 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4481 evalcond[1]=new_r12;
4482 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4483 {
4484 bgotonextstatement=false;
4485 {
4486 IkReal j5array[1], cj5array[1], sj5array[1];
4487 bool j5valid[1]={false};
4488 _nj5 = 1;
4489 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 )
4490  continue;
4491 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4492 sj5array[0]=IKsin(j5array[0]);
4493 cj5array[0]=IKcos(j5array[0]);
4494 if( j5array[0] > IKPI )
4495 {
4496  j5array[0]-=IK2PI;
4497 }
4498 else if( j5array[0] < -IKPI )
4499 { j5array[0]+=IK2PI;
4500 }
4501 j5valid[0] = true;
4502 for(int ij5 = 0; ij5 < 1; ++ij5)
4503 {
4504 if( !j5valid[ij5] )
4505 {
4506  continue;
4507 }
4508 _ij5[0] = ij5; _ij5[1] = -1;
4509 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4510 {
4511 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4512 {
4513  j5valid[iij5]=false; _ij5[1] = iij5; break;
4514 }
4515 }
4516 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4517 {
4518 IkReal evalcond[8];
4519 IkReal x336=IKsin(j5);
4520 IkReal x337=IKcos(j5);
4521 IkReal x338=((1.0)*new_r00);
4522 IkReal x339=((1.0)*new_r01);
4523 IkReal x340=((1.0)*x337);
4524 evalcond[0]=(x336+new_r10);
4525 evalcond[1]=(x337+new_r11);
4526 evalcond[2]=(((new_r02*x336))+new_r21);
4527 evalcond[3]=((((-1.0)*new_r02*x340))+new_r20);
4528 evalcond[4]=((((-1.0)*x339))+((cj4*x336)));
4529 evalcond[5]=((((-1.0)*cj4*x340))+(((-1.0)*x338)));
4530 evalcond[6]=((((-1.0)*cj4*x339))+x336+((new_r21*sj4)));
4531 evalcond[7]=(((new_r20*sj4))+(((-1.0)*cj4*x338))+(((-1.0)*x340)));
4532 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4533 {
4534 continue;
4535 }
4536 }
4537 
4538 {
4539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4540 vinfos[0].jointtype = 1;
4541 vinfos[0].foffset = j0;
4542 vinfos[0].indices[0] = _ij0[0];
4543 vinfos[0].indices[1] = _ij0[1];
4544 vinfos[0].maxsolutions = _nj0;
4545 vinfos[1].jointtype = 1;
4546 vinfos[1].foffset = j1;
4547 vinfos[1].indices[0] = _ij1[0];
4548 vinfos[1].indices[1] = _ij1[1];
4549 vinfos[1].maxsolutions = _nj1;
4550 vinfos[2].jointtype = 1;
4551 vinfos[2].foffset = j2;
4552 vinfos[2].indices[0] = _ij2[0];
4553 vinfos[2].indices[1] = _ij2[1];
4554 vinfos[2].maxsolutions = _nj2;
4555 vinfos[3].jointtype = 1;
4556 vinfos[3].foffset = j3;
4557 vinfos[3].indices[0] = _ij3[0];
4558 vinfos[3].indices[1] = _ij3[1];
4559 vinfos[3].maxsolutions = _nj3;
4560 vinfos[4].jointtype = 1;
4561 vinfos[4].foffset = j4;
4562 vinfos[4].indices[0] = _ij4[0];
4563 vinfos[4].indices[1] = _ij4[1];
4564 vinfos[4].maxsolutions = _nj4;
4565 vinfos[5].jointtype = 1;
4566 vinfos[5].foffset = j5;
4567 vinfos[5].indices[0] = _ij5[0];
4568 vinfos[5].indices[1] = _ij5[1];
4569 vinfos[5].maxsolutions = _nj5;
4570 std::vector<int> vfree(0);
4571 solutions.AddSolution(vinfos,vfree);
4572 }
4573 }
4574 }
4575 
4576 }
4577 } while(0);
4578 if( bgotonextstatement )
4579 {
4580 bool bgotonextstatement = true;
4581 do
4582 {
4583 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4584 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4585 {
4586 bgotonextstatement=false;
4587 {
4588 IkReal j5eval[1];
4589 new_r21=0;
4590 new_r20=0;
4591 new_r02=0;
4592 new_r12=0;
4593 j5eval[0]=1.0;
4594 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4595 {
4596 continue; // no branches [j5]
4597 
4598 } else
4599 {
4600 IkReal op[2+1], zeror[2];
4601 int numroots;
4602 op[0]=-1.0;
4603 op[1]=0;
4604 op[2]=1.0;
4605 polyroots2(op,zeror,numroots);
4606 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4607 int numsolutions = 0;
4608 for(int ij5 = 0; ij5 < numroots; ++ij5)
4609 {
4610 IkReal htj5 = zeror[ij5];
4611 tempj5array[0]=((2.0)*(atan(htj5)));
4612 for(int kj5 = 0; kj5 < 1; ++kj5)
4613 {
4614 j5array[numsolutions] = tempj5array[kj5];
4615 if( j5array[numsolutions] > IKPI )
4616 {
4617  j5array[numsolutions]-=IK2PI;
4618 }
4619 else if( j5array[numsolutions] < -IKPI )
4620 {
4621  j5array[numsolutions]+=IK2PI;
4622 }
4623 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4624 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4625 numsolutions++;
4626 }
4627 }
4628 bool j5valid[2]={true,true};
4629 _nj5 = 2;
4630 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4631  {
4632 if( !j5valid[ij5] )
4633 {
4634  continue;
4635 }
4636  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4637 htj5 = IKtan(j5/2);
4638 
4639 _ij5[0] = ij5; _ij5[1] = -1;
4640 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4641 {
4642 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4643 {
4644  j5valid[iij5]=false; _ij5[1] = iij5; break;
4645 }
4646 }
4647 {
4648 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4649 vinfos[0].jointtype = 1;
4650 vinfos[0].foffset = j0;
4651 vinfos[0].indices[0] = _ij0[0];
4652 vinfos[0].indices[1] = _ij0[1];
4653 vinfos[0].maxsolutions = _nj0;
4654 vinfos[1].jointtype = 1;
4655 vinfos[1].foffset = j1;
4656 vinfos[1].indices[0] = _ij1[0];
4657 vinfos[1].indices[1] = _ij1[1];
4658 vinfos[1].maxsolutions = _nj1;
4659 vinfos[2].jointtype = 1;
4660 vinfos[2].foffset = j2;
4661 vinfos[2].indices[0] = _ij2[0];
4662 vinfos[2].indices[1] = _ij2[1];
4663 vinfos[2].maxsolutions = _nj2;
4664 vinfos[3].jointtype = 1;
4665 vinfos[3].foffset = j3;
4666 vinfos[3].indices[0] = _ij3[0];
4667 vinfos[3].indices[1] = _ij3[1];
4668 vinfos[3].maxsolutions = _nj3;
4669 vinfos[4].jointtype = 1;
4670 vinfos[4].foffset = j4;
4671 vinfos[4].indices[0] = _ij4[0];
4672 vinfos[4].indices[1] = _ij4[1];
4673 vinfos[4].maxsolutions = _nj4;
4674 vinfos[5].jointtype = 1;
4675 vinfos[5].foffset = j5;
4676 vinfos[5].indices[0] = _ij5[0];
4677 vinfos[5].indices[1] = _ij5[1];
4678 vinfos[5].maxsolutions = _nj5;
4679 std::vector<int> vfree(0);
4680 solutions.AddSolution(vinfos,vfree);
4681 }
4682  }
4683 
4684 }
4685 
4686 }
4687 
4688 }
4689 } while(0);
4690 if( bgotonextstatement )
4691 {
4692 bool bgotonextstatement = true;
4693 do
4694 {
4695 if( 1 )
4696 {
4697 bgotonextstatement=false;
4698 continue; // branch miss [j5]
4699 
4700 }
4701 } while(0);
4702 if( bgotonextstatement )
4703 {
4704 }
4705 }
4706 }
4707 }
4708 }
4709 }
4710 }
4711 }
4712 }
4713 
4714 } else
4715 {
4716 {
4717 IkReal j5array[1], cj5array[1], sj5array[1];
4718 bool j5valid[1]={false};
4719 _nj5 = 1;
4720 CheckValue<IkReal> x342=IKPowWithIntegerCheck(sj4,-1);
4721 if(!x342.valid){
4722 continue;
4723 }
4724 IkReal x341=x342.value;
4725 CheckValue<IkReal> x343=IKPowWithIntegerCheck(cj3,-1);
4726 if(!x343.valid){
4727 continue;
4728 }
4729 if( IKabs(((-1.0)*new_r21*x341)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x341))+IKsqr((x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4730  continue;
4731 j5array[0]=IKatan2(((-1.0)*new_r21*x341), (x341*(x343.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4732 sj5array[0]=IKsin(j5array[0]);
4733 cj5array[0]=IKcos(j5array[0]);
4734 if( j5array[0] > IKPI )
4735 {
4736  j5array[0]-=IK2PI;
4737 }
4738 else if( j5array[0] < -IKPI )
4739 { j5array[0]+=IK2PI;
4740 }
4741 j5valid[0] = true;
4742 for(int ij5 = 0; ij5 < 1; ++ij5)
4743 {
4744 if( !j5valid[ij5] )
4745 {
4746  continue;
4747 }
4748 _ij5[0] = ij5; _ij5[1] = -1;
4749 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4750 {
4751 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4752 {
4753  j5valid[iij5]=false; _ij5[1] = iij5; break;
4754 }
4755 }
4756 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4757 {
4758 IkReal evalcond[12];
4759 IkReal x344=IKsin(j5);
4760 IkReal x345=IKcos(j5);
4761 IkReal x346=(cj3*new_r00);
4762 IkReal x347=(cj3*cj4);
4763 IkReal x348=(cj4*sj3);
4764 IkReal x349=((1.0)*cj3);
4765 IkReal x350=(sj3*x344);
4766 IkReal x351=((1.0)*x345);
4767 evalcond[0]=(((sj4*x344))+new_r21);
4768 evalcond[1]=(new_r20+(((-1.0)*sj4*x351)));
4769 evalcond[2]=((((-1.0)*new_r10*x349))+((new_r00*sj3))+x344);
4770 evalcond[3]=((((-1.0)*new_r11*x349))+((new_r01*sj3))+x345);
4771 evalcond[4]=(((new_r11*sj3))+((cj4*x344))+((cj3*new_r01)));
4772 evalcond[5]=(((sj3*x345))+((x344*x347))+new_r01);
4773 evalcond[6]=(((new_r10*sj3))+x346+(((-1.0)*cj4*x351)));
4774 evalcond[7]=(x350+(((-1.0)*x347*x351))+new_r00);
4775 evalcond[8]=(((x344*x348))+(((-1.0)*x345*x349))+new_r11);
4776 evalcond[9]=((((-1.0)*x348*x351))+(((-1.0)*x344*x349))+new_r10);
4777 evalcond[10]=(((new_r11*x348))+((new_r01*x347))+x344+((new_r21*sj4)));
4778 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x351))+((new_r10*x348))+((cj4*x346)));
4779 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4780 {
4781 continue;
4782 }
4783 }
4784 
4785 {
4786 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4787 vinfos[0].jointtype = 1;
4788 vinfos[0].foffset = j0;
4789 vinfos[0].indices[0] = _ij0[0];
4790 vinfos[0].indices[1] = _ij0[1];
4791 vinfos[0].maxsolutions = _nj0;
4792 vinfos[1].jointtype = 1;
4793 vinfos[1].foffset = j1;
4794 vinfos[1].indices[0] = _ij1[0];
4795 vinfos[1].indices[1] = _ij1[1];
4796 vinfos[1].maxsolutions = _nj1;
4797 vinfos[2].jointtype = 1;
4798 vinfos[2].foffset = j2;
4799 vinfos[2].indices[0] = _ij2[0];
4800 vinfos[2].indices[1] = _ij2[1];
4801 vinfos[2].maxsolutions = _nj2;
4802 vinfos[3].jointtype = 1;
4803 vinfos[3].foffset = j3;
4804 vinfos[3].indices[0] = _ij3[0];
4805 vinfos[3].indices[1] = _ij3[1];
4806 vinfos[3].maxsolutions = _nj3;
4807 vinfos[4].jointtype = 1;
4808 vinfos[4].foffset = j4;
4809 vinfos[4].indices[0] = _ij4[0];
4810 vinfos[4].indices[1] = _ij4[1];
4811 vinfos[4].maxsolutions = _nj4;
4812 vinfos[5].jointtype = 1;
4813 vinfos[5].foffset = j5;
4814 vinfos[5].indices[0] = _ij5[0];
4815 vinfos[5].indices[1] = _ij5[1];
4816 vinfos[5].maxsolutions = _nj5;
4817 std::vector<int> vfree(0);
4818 solutions.AddSolution(vinfos,vfree);
4819 }
4820 }
4821 }
4822 
4823 }
4824 
4825 }
4826 
4827 } else
4828 {
4829 {
4830 IkReal j5array[1], cj5array[1], sj5array[1];
4831 bool j5valid[1]={false};
4832 _nj5 = 1;
4833 CheckValue<IkReal> x353=IKPowWithIntegerCheck(sj4,-1);
4834 if(!x353.valid){
4835 continue;
4836 }
4837 IkReal x352=x353.value;
4838 CheckValue<IkReal> x354=IKPowWithIntegerCheck(sj3,-1);
4839 if(!x354.valid){
4840 continue;
4841 }
4842 if( IKabs(((-1.0)*new_r21*x352)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x352))+IKsqr((x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4843  continue;
4844 j5array[0]=IKatan2(((-1.0)*new_r21*x352), (x352*(x354.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4845 sj5array[0]=IKsin(j5array[0]);
4846 cj5array[0]=IKcos(j5array[0]);
4847 if( j5array[0] > IKPI )
4848 {
4849  j5array[0]-=IK2PI;
4850 }
4851 else if( j5array[0] < -IKPI )
4852 { j5array[0]+=IK2PI;
4853 }
4854 j5valid[0] = true;
4855 for(int ij5 = 0; ij5 < 1; ++ij5)
4856 {
4857 if( !j5valid[ij5] )
4858 {
4859  continue;
4860 }
4861 _ij5[0] = ij5; _ij5[1] = -1;
4862 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4863 {
4864 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4865 {
4866  j5valid[iij5]=false; _ij5[1] = iij5; break;
4867 }
4868 }
4869 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4870 {
4871 IkReal evalcond[12];
4872 IkReal x355=IKsin(j5);
4873 IkReal x356=IKcos(j5);
4874 IkReal x357=(cj3*new_r00);
4875 IkReal x358=(cj3*cj4);
4876 IkReal x359=(cj4*sj3);
4877 IkReal x360=((1.0)*cj3);
4878 IkReal x361=(sj3*x355);
4879 IkReal x362=((1.0)*x356);
4880 evalcond[0]=(((sj4*x355))+new_r21);
4881 evalcond[1]=((((-1.0)*sj4*x362))+new_r20);
4882 evalcond[2]=(((new_r00*sj3))+x355+(((-1.0)*new_r10*x360)));
4883 evalcond[3]=(((new_r01*sj3))+x356+(((-1.0)*new_r11*x360)));
4884 evalcond[4]=(((new_r11*sj3))+((cj4*x355))+((cj3*new_r01)));
4885 evalcond[5]=(((x355*x358))+((sj3*x356))+new_r01);
4886 evalcond[6]=((((-1.0)*cj4*x362))+((new_r10*sj3))+x357);
4887 evalcond[7]=((((-1.0)*x358*x362))+x361+new_r00);
4888 evalcond[8]=(((x355*x359))+(((-1.0)*x356*x360))+new_r11);
4889 evalcond[9]=((((-1.0)*x355*x360))+(((-1.0)*x359*x362))+new_r10);
4890 evalcond[10]=(((new_r01*x358))+x355+((new_r21*sj4))+((new_r11*x359)));
4891 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x362))+((cj4*x357))+((new_r10*x359)));
4892 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
4893 {
4894 continue;
4895 }
4896 }
4897 
4898 {
4899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4900 vinfos[0].jointtype = 1;
4901 vinfos[0].foffset = j0;
4902 vinfos[0].indices[0] = _ij0[0];
4903 vinfos[0].indices[1] = _ij0[1];
4904 vinfos[0].maxsolutions = _nj0;
4905 vinfos[1].jointtype = 1;
4906 vinfos[1].foffset = j1;
4907 vinfos[1].indices[0] = _ij1[0];
4908 vinfos[1].indices[1] = _ij1[1];
4909 vinfos[1].maxsolutions = _nj1;
4910 vinfos[2].jointtype = 1;
4911 vinfos[2].foffset = j2;
4912 vinfos[2].indices[0] = _ij2[0];
4913 vinfos[2].indices[1] = _ij2[1];
4914 vinfos[2].maxsolutions = _nj2;
4915 vinfos[3].jointtype = 1;
4916 vinfos[3].foffset = j3;
4917 vinfos[3].indices[0] = _ij3[0];
4918 vinfos[3].indices[1] = _ij3[1];
4919 vinfos[3].maxsolutions = _nj3;
4920 vinfos[4].jointtype = 1;
4921 vinfos[4].foffset = j4;
4922 vinfos[4].indices[0] = _ij4[0];
4923 vinfos[4].indices[1] = _ij4[1];
4924 vinfos[4].maxsolutions = _nj4;
4925 vinfos[5].jointtype = 1;
4926 vinfos[5].foffset = j5;
4927 vinfos[5].indices[0] = _ij5[0];
4928 vinfos[5].indices[1] = _ij5[1];
4929 vinfos[5].maxsolutions = _nj5;
4930 std::vector<int> vfree(0);
4931 solutions.AddSolution(vinfos,vfree);
4932 }
4933 }
4934 }
4935 
4936 }
4937 
4938 }
4939 
4940 } else
4941 {
4942 {
4943 IkReal j5array[1], cj5array[1], sj5array[1];
4944 bool j5valid[1]={false};
4945 _nj5 = 1;
4946 CheckValue<IkReal> x363 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
4947 if(!x363.valid){
4948 continue;
4949 }
4950 CheckValue<IkReal> x364=IKPowWithIntegerCheck(IKsign(sj4),-1);
4951 if(!x364.valid){
4952 continue;
4953 }
4954 j5array[0]=((-1.5707963267949)+(x363.value)+(((1.5707963267949)*(x364.value))));
4955 sj5array[0]=IKsin(j5array[0]);
4956 cj5array[0]=IKcos(j5array[0]);
4957 if( j5array[0] > IKPI )
4958 {
4959  j5array[0]-=IK2PI;
4960 }
4961 else if( j5array[0] < -IKPI )
4962 { j5array[0]+=IK2PI;
4963 }
4964 j5valid[0] = true;
4965 for(int ij5 = 0; ij5 < 1; ++ij5)
4966 {
4967 if( !j5valid[ij5] )
4968 {
4969  continue;
4970 }
4971 _ij5[0] = ij5; _ij5[1] = -1;
4972 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4973 {
4974 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4975 {
4976  j5valid[iij5]=false; _ij5[1] = iij5; break;
4977 }
4978 }
4979 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4980 {
4981 IkReal evalcond[12];
4982 IkReal x365=IKsin(j5);
4983 IkReal x366=IKcos(j5);
4984 IkReal x367=(cj3*new_r00);
4985 IkReal x368=(cj3*cj4);
4986 IkReal x369=(cj4*sj3);
4987 IkReal x370=((1.0)*cj3);
4988 IkReal x371=(sj3*x365);
4989 IkReal x372=((1.0)*x366);
4990 evalcond[0]=(((sj4*x365))+new_r21);
4991 evalcond[1]=((((-1.0)*sj4*x372))+new_r20);
4992 evalcond[2]=(((new_r00*sj3))+x365+(((-1.0)*new_r10*x370)));
4993 evalcond[3]=(((new_r01*sj3))+x366+(((-1.0)*new_r11*x370)));
4994 evalcond[4]=(((new_r11*sj3))+((cj4*x365))+((cj3*new_r01)));
4995 evalcond[5]=(((x365*x368))+((sj3*x366))+new_r01);
4996 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x372))+x367);
4997 evalcond[7]=((((-1.0)*x368*x372))+x371+new_r00);
4998 evalcond[8]=(((x365*x369))+(((-1.0)*x366*x370))+new_r11);
4999 evalcond[9]=((((-1.0)*x369*x372))+new_r10+(((-1.0)*x365*x370)));
5000 evalcond[10]=(x365+((new_r01*x368))+((new_r21*sj4))+((new_r11*x369)));
5001 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x372))+((cj4*x367))+((new_r10*x369)));
5002 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || 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 )
5003 {
5004 continue;
5005 }
5006 }
5007 
5008 {
5009 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5010 vinfos[0].jointtype = 1;
5011 vinfos[0].foffset = j0;
5012 vinfos[0].indices[0] = _ij0[0];
5013 vinfos[0].indices[1] = _ij0[1];
5014 vinfos[0].maxsolutions = _nj0;
5015 vinfos[1].jointtype = 1;
5016 vinfos[1].foffset = j1;
5017 vinfos[1].indices[0] = _ij1[0];
5018 vinfos[1].indices[1] = _ij1[1];
5019 vinfos[1].maxsolutions = _nj1;
5020 vinfos[2].jointtype = 1;
5021 vinfos[2].foffset = j2;
5022 vinfos[2].indices[0] = _ij2[0];
5023 vinfos[2].indices[1] = _ij2[1];
5024 vinfos[2].maxsolutions = _nj2;
5025 vinfos[3].jointtype = 1;
5026 vinfos[3].foffset = j3;
5027 vinfos[3].indices[0] = _ij3[0];
5028 vinfos[3].indices[1] = _ij3[1];
5029 vinfos[3].maxsolutions = _nj3;
5030 vinfos[4].jointtype = 1;
5031 vinfos[4].foffset = j4;
5032 vinfos[4].indices[0] = _ij4[0];
5033 vinfos[4].indices[1] = _ij4[1];
5034 vinfos[4].maxsolutions = _nj4;
5035 vinfos[5].jointtype = 1;
5036 vinfos[5].foffset = j5;
5037 vinfos[5].indices[0] = _ij5[0];
5038 vinfos[5].indices[1] = _ij5[1];
5039 vinfos[5].maxsolutions = _nj5;
5040 std::vector<int> vfree(0);
5041 solutions.AddSolution(vinfos,vfree);
5042 }
5043 }
5044 }
5045 
5046 }
5047 
5048 }
5049 }
5050 }
5051 
5052 }
5053 
5054 }
5055 
5056 } else
5057 {
5058 {
5059 IkReal j5array[1], cj5array[1], sj5array[1];
5060 bool j5valid[1]={false};
5061 _nj5 = 1;
5062 CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5063 if(!x373.valid){
5064 continue;
5065 }
5066 CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5067 if(!x374.valid){
5068 continue;
5069 }
5070 j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5071 sj5array[0]=IKsin(j5array[0]);
5072 cj5array[0]=IKcos(j5array[0]);
5073 if( j5array[0] > IKPI )
5074 {
5075  j5array[0]-=IK2PI;
5076 }
5077 else if( j5array[0] < -IKPI )
5078 { j5array[0]+=IK2PI;
5079 }
5080 j5valid[0] = true;
5081 for(int ij5 = 0; ij5 < 1; ++ij5)
5082 {
5083 if( !j5valid[ij5] )
5084 {
5085  continue;
5086 }
5087 _ij5[0] = ij5; _ij5[1] = -1;
5088 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5089 {
5090 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5091 {
5092  j5valid[iij5]=false; _ij5[1] = iij5; break;
5093 }
5094 }
5095 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5096 {
5097 IkReal evalcond[2];
5098 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5099 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5100 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5101 {
5102 continue;
5103 }
5104 }
5105 
5106 {
5107 IkReal j3eval[3];
5108 j3eval[0]=sj4;
5109 j3eval[1]=IKsign(sj4);
5110 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5111 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5112 {
5113 {
5114 IkReal j3eval[2];
5115 j3eval[0]=new_r12;
5116 j3eval[1]=sj4;
5117 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5118 {
5119 {
5120 IkReal evalcond[5];
5121 bool bgotonextstatement = true;
5122 do
5123 {
5124 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5125 evalcond[1]=new_r02;
5126 evalcond[2]=new_r12;
5127 evalcond[3]=new_r21;
5128 evalcond[4]=new_r20;
5129 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 )
5130 {
5131 bgotonextstatement=false;
5132 {
5133 IkReal j3eval[3];
5134 sj4=0;
5135 cj4=1.0;
5136 j4=0;
5137 IkReal x375=((1.0)*new_r11);
5138 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5139 j3eval[0]=x376;
5140 j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5141 j3eval[2]=IKsign(x376);
5142 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5143 {
5144 {
5145 IkReal j3eval[3];
5146 sj4=0;
5147 cj4=1.0;
5148 j4=0;
5149 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5150 j3eval[0]=x377;
5151 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5152 j3eval[2]=IKsign(x377);
5153 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5154 {
5155 {
5156 IkReal j3eval[3];
5157 sj4=0;
5158 cj4=1.0;
5159 j4=0;
5160 IkReal x378=((1.0)*new_r11);
5161 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5162 j3eval[0]=x379;
5163 j3eval[1]=IKsign(x379);
5164 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5165 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5166 {
5167 {
5168 IkReal evalcond[1];
5169 bool bgotonextstatement = true;
5170 do
5171 {
5172 IkReal x380=((-1.0)*new_r01);
5173 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5174 if(IKabs(x382)==0){
5175 continue;
5176 }
5177 IkReal x381=pow(x382,-0.5);
5178 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5179 if(!x383.valid){
5180 continue;
5181 }
5182 IkReal gconst6=((-1.0)*(x383.value));
5183 IkReal gconst7=(new_r11*x381);
5184 IkReal gconst8=(x380*x381);
5185 CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5186 if(!x384.valid){
5187 continue;
5188 }
5189 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5190 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5191 {
5192 bgotonextstatement=false;
5193 {
5194 IkReal j3eval[3];
5195 IkReal x385=((-1.0)*new_r01);
5196 CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5197 if(!x388.valid){
5198 continue;
5199 }
5200 IkReal x386=((-1.0)*(x388.value));
5201 IkReal x387=x381;
5202 sj4=0;
5203 cj4=1.0;
5204 j4=0;
5205 sj5=gconst7;
5206 cj5=gconst8;
5207 j5=x386;
5208 IkReal gconst6=x386;
5209 IkReal gconst7=(new_r11*x387);
5210 IkReal gconst8=(x385*x387);
5211 IkReal x389=new_r11*new_r11;
5212 IkReal x390=(new_r10*new_r11);
5213 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5214 IkReal x392=x381;
5215 IkReal x393=(new_r11*x392);
5216 j3eval[0]=x391;
5217 j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5218 j3eval[2]=IKsign(x391);
5219 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5220 {
5221 {
5222 IkReal j3eval[1];
5223 IkReal x394=((-1.0)*new_r01);
5224 CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5225 if(!x397.valid){
5226 continue;
5227 }
5228 IkReal x395=((-1.0)*(x397.value));
5229 IkReal x396=x381;
5230 sj4=0;
5231 cj4=1.0;
5232 j4=0;
5233 sj5=gconst7;
5234 cj5=gconst8;
5235 j5=x395;
5236 IkReal gconst6=x395;
5237 IkReal gconst7=(new_r11*x396);
5238 IkReal gconst8=(x394*x396);
5239 IkReal x398=new_r11*new_r11;
5240 CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5241 if(!x401.valid){
5242 continue;
5243 }
5244 IkReal x399=x401.value;
5245 IkReal x400=(x398*x399);
5246 j3eval[0]=((IKabs((((new_r01*new_r10))+x400)))+(IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5247 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5248 {
5249 {
5250 IkReal j3eval[1];
5251 IkReal x402=((-1.0)*new_r01);
5252 CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5253 if(!x405.valid){
5254 continue;
5255 }
5256 IkReal x403=((-1.0)*(x405.value));
5257 IkReal x404=x381;
5258 sj4=0;
5259 cj4=1.0;
5260 j4=0;
5261 sj5=gconst7;
5262 cj5=gconst8;
5263 j5=x403;
5264 IkReal gconst6=x403;
5265 IkReal gconst7=(new_r11*x404);
5266 IkReal gconst8=(x402*x404);
5267 IkReal x406=new_r01*new_r01;
5268 IkReal x407=new_r11*new_r11;
5269 CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5270 if(!x414.valid){
5271 continue;
5272 }
5273 IkReal x408=x414.value;
5274 IkReal x409=(x407*x408);
5275 CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5276 if(!x415.valid){
5277 continue;
5278 }
5279 IkReal x410=x415.value;
5280 IkReal x411=((1.0)*x410);
5281 IkReal x412=(new_r11*x411);
5282 IkReal x413=(new_r01*x411);
5283 j3eval[0]=((IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5284 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5285 {
5286 {
5287 IkReal evalcond[2];
5288 bool bgotonextstatement = true;
5289 do
5290 {
5291 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5292 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5293 {
5294 bgotonextstatement=false;
5295 {
5296 IkReal j3array[2], cj3array[2], sj3array[2];
5297 bool j3valid[2]={false};
5298 _nj3 = 2;
5299 CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst8,-1);
5300 if(!x416.valid){
5301 continue;
5302 }
5303 sj3array[0]=(new_r10*(x416.value));
5304 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5305 {
5306  j3valid[0] = j3valid[1] = true;
5307  j3array[0] = IKasin(sj3array[0]);
5308  cj3array[0] = IKcos(j3array[0]);
5309  sj3array[1] = sj3array[0];
5310  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5311  cj3array[1] = -cj3array[0];
5312 }
5313 else if( isnan(sj3array[0]) )
5314 {
5315  // probably any value will work
5316  j3valid[0] = true;
5317  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5318 }
5319 for(int ij3 = 0; ij3 < 2; ++ij3)
5320 {
5321 if( !j3valid[ij3] )
5322 {
5323  continue;
5324 }
5325 _ij3[0] = ij3; _ij3[1] = -1;
5326 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5327 {
5328 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5329 {
5330  j3valid[iij3]=false; _ij3[1] = iij3; break;
5331 }
5332 }
5333 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5334 {
5335 IkReal evalcond[6];
5336 IkReal x417=IKcos(j3);
5337 IkReal x418=IKsin(j3);
5338 IkReal x419=((-1.0)*x417);
5339 evalcond[0]=(new_r01*x417);
5340 evalcond[1]=(new_r10*x419);
5341 evalcond[2]=(gconst8*x419);
5342 evalcond[3]=(gconst8+((new_r01*x418)));
5343 evalcond[4]=(((gconst8*x418))+new_r01);
5344 evalcond[5]=((((-1.0)*gconst8))+((new_r10*x418)));
5345 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5346 {
5347 continue;
5348 }
5349 }
5350 
5351 {
5352 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5353 vinfos[0].jointtype = 1;
5354 vinfos[0].foffset = j0;
5355 vinfos[0].indices[0] = _ij0[0];
5356 vinfos[0].indices[1] = _ij0[1];
5357 vinfos[0].maxsolutions = _nj0;
5358 vinfos[1].jointtype = 1;
5359 vinfos[1].foffset = j1;
5360 vinfos[1].indices[0] = _ij1[0];
5361 vinfos[1].indices[1] = _ij1[1];
5362 vinfos[1].maxsolutions = _nj1;
5363 vinfos[2].jointtype = 1;
5364 vinfos[2].foffset = j2;
5365 vinfos[2].indices[0] = _ij2[0];
5366 vinfos[2].indices[1] = _ij2[1];
5367 vinfos[2].maxsolutions = _nj2;
5368 vinfos[3].jointtype = 1;
5369 vinfos[3].foffset = j3;
5370 vinfos[3].indices[0] = _ij3[0];
5371 vinfos[3].indices[1] = _ij3[1];
5372 vinfos[3].maxsolutions = _nj3;
5373 vinfos[4].jointtype = 1;
5374 vinfos[4].foffset = j4;
5375 vinfos[4].indices[0] = _ij4[0];
5376 vinfos[4].indices[1] = _ij4[1];
5377 vinfos[4].maxsolutions = _nj4;
5378 vinfos[5].jointtype = 1;
5379 vinfos[5].foffset = j5;
5380 vinfos[5].indices[0] = _ij5[0];
5381 vinfos[5].indices[1] = _ij5[1];
5382 vinfos[5].maxsolutions = _nj5;
5383 std::vector<int> vfree(0);
5384 solutions.AddSolution(vinfos,vfree);
5385 }
5386 }
5387 }
5388 
5389 }
5390 } while(0);
5391 if( bgotonextstatement )
5392 {
5393 bool bgotonextstatement = true;
5394 do
5395 {
5396 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5397 evalcond[1]=gconst7;
5398 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5399 {
5400 bgotonextstatement=false;
5401 {
5402 IkReal j3eval[3];
5403 IkReal x420=((-1.0)*new_r01);
5404 CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5405 if(!x422.valid){
5406 continue;
5407 }
5408 IkReal x421=((-1.0)*(x422.value));
5409 sj4=0;
5410 cj4=1.0;
5411 j4=0;
5412 sj5=gconst7;
5413 cj5=gconst8;
5414 j5=x421;
5415 new_r00=0;
5416 new_r10=0;
5417 new_r21=0;
5418 new_r22=0;
5419 IkReal gconst6=x421;
5420 IkReal gconst7=new_r11;
5421 IkReal gconst8=x420;
5422 j3eval[0]=-1.0;
5423 j3eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5424 j3eval[2]=-1.0;
5425 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5426 {
5427 {
5428 IkReal j3eval[3];
5429 IkReal x423=((-1.0)*new_r01);
5430 CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5431 if(!x425.valid){
5432 continue;
5433 }
5434 IkReal x424=((-1.0)*(x425.value));
5435 sj4=0;
5436 cj4=1.0;
5437 j4=0;
5438 sj5=gconst7;
5439 cj5=gconst8;
5440 j5=x424;
5441 new_r00=0;
5442 new_r10=0;
5443 new_r21=0;
5444 new_r22=0;
5445 IkReal gconst6=x424;
5446 IkReal gconst7=new_r11;
5447 IkReal gconst8=x423;
5448 j3eval[0]=-1.0;
5449 j3eval[1]=-1.0;
5450 j3eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5451 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5452 {
5453 {
5454 IkReal j3eval[3];
5455 IkReal x426=((-1.0)*new_r01);
5456 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5457 if(!x428.valid){
5458 continue;
5459 }
5460 IkReal x427=((-1.0)*(x428.value));
5461 sj4=0;
5462 cj4=1.0;
5463 j4=0;
5464 sj5=gconst7;
5465 cj5=gconst8;
5466 j5=x427;
5467 new_r00=0;
5468 new_r10=0;
5469 new_r21=0;
5470 new_r22=0;
5471 IkReal gconst6=x427;
5472 IkReal gconst7=new_r11;
5473 IkReal gconst8=x426;
5474 j3eval[0]=1.0;
5475 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5476 j3eval[2]=1.0;
5477 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5478 {
5479 continue; // 3 cases reached
5480 
5481 } else
5482 {
5483 {
5484 IkReal j3array[1], cj3array[1], sj3array[1];
5485 bool j3valid[1]={false};
5486 _nj3 = 1;
5487 IkReal x429=((1.0)*new_r01);
5488 CheckValue<IkReal> x430=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*x429))+((gconst7*new_r11)))),-1);
5489 if(!x430.valid){
5490 continue;
5491 }
5492 CheckValue<IkReal> x431 = IKatan2WithCheck(IkReal(((((-1.0)*(gconst7*gconst7)))+(new_r01*new_r01))),IkReal(((((-1.0)*new_r11*x429))+((gconst7*gconst8)))),IKFAST_ATAN2_MAGTHRESH);
5493 if(!x431.valid){
5494 continue;
5495 }
5496 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x430.value)))+(x431.value));
5497 sj3array[0]=IKsin(j3array[0]);
5498 cj3array[0]=IKcos(j3array[0]);
5499 if( j3array[0] > IKPI )
5500 {
5501  j3array[0]-=IK2PI;
5502 }
5503 else if( j3array[0] < -IKPI )
5504 { j3array[0]+=IK2PI;
5505 }
5506 j3valid[0] = true;
5507 for(int ij3 = 0; ij3 < 1; ++ij3)
5508 {
5509 if( !j3valid[ij3] )
5510 {
5511  continue;
5512 }
5513 _ij3[0] = ij3; _ij3[1] = -1;
5514 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5515 {
5516 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5517 {
5518  j3valid[iij3]=false; _ij3[1] = iij3; break;
5519 }
5520 }
5521 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5522 {
5523 IkReal evalcond[6];
5524 IkReal x432=IKsin(j3);
5525 IkReal x433=IKcos(j3);
5526 IkReal x434=(gconst7*x432);
5527 IkReal x435=((1.0)*x433);
5528 IkReal x436=(gconst8*x432);
5529 IkReal x437=(gconst8*x435);
5530 evalcond[0]=(((new_r01*x433))+gconst7+((new_r11*x432)));
5531 evalcond[1]=(((gconst7*x433))+x436+new_r01);
5532 evalcond[2]=((((-1.0)*x437))+x434);
5533 evalcond[3]=(((new_r01*x432))+gconst8+(((-1.0)*new_r11*x435)));
5534 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5535 evalcond[5]=((((-1.0)*x436))+(((-1.0)*gconst7*x435)));
5536 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5537 {
5538 continue;
5539 }
5540 }
5541 
5542 {
5543 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5544 vinfos[0].jointtype = 1;
5545 vinfos[0].foffset = j0;
5546 vinfos[0].indices[0] = _ij0[0];
5547 vinfos[0].indices[1] = _ij0[1];
5548 vinfos[0].maxsolutions = _nj0;
5549 vinfos[1].jointtype = 1;
5550 vinfos[1].foffset = j1;
5551 vinfos[1].indices[0] = _ij1[0];
5552 vinfos[1].indices[1] = _ij1[1];
5553 vinfos[1].maxsolutions = _nj1;
5554 vinfos[2].jointtype = 1;
5555 vinfos[2].foffset = j2;
5556 vinfos[2].indices[0] = _ij2[0];
5557 vinfos[2].indices[1] = _ij2[1];
5558 vinfos[2].maxsolutions = _nj2;
5559 vinfos[3].jointtype = 1;
5560 vinfos[3].foffset = j3;
5561 vinfos[3].indices[0] = _ij3[0];
5562 vinfos[3].indices[1] = _ij3[1];
5563 vinfos[3].maxsolutions = _nj3;
5564 vinfos[4].jointtype = 1;
5565 vinfos[4].foffset = j4;
5566 vinfos[4].indices[0] = _ij4[0];
5567 vinfos[4].indices[1] = _ij4[1];
5568 vinfos[4].maxsolutions = _nj4;
5569 vinfos[5].jointtype = 1;
5570 vinfos[5].foffset = j5;
5571 vinfos[5].indices[0] = _ij5[0];
5572 vinfos[5].indices[1] = _ij5[1];
5573 vinfos[5].maxsolutions = _nj5;
5574 std::vector<int> vfree(0);
5575 solutions.AddSolution(vinfos,vfree);
5576 }
5577 }
5578 }
5579 
5580 }
5581 
5582 }
5583 
5584 } else
5585 {
5586 {
5587 IkReal j3array[1], cj3array[1], sj3array[1];
5588 bool j3valid[1]={false};
5589 _nj3 = 1;
5590 CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
5591 if(!x438.valid){
5592 continue;
5593 }
5594 CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst8*new_r01)),IkReal((gconst7*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5595 if(!x439.valid){
5596 continue;
5597 }
5598 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5599 sj3array[0]=IKsin(j3array[0]);
5600 cj3array[0]=IKcos(j3array[0]);
5601 if( j3array[0] > IKPI )
5602 {
5603  j3array[0]-=IK2PI;
5604 }
5605 else if( j3array[0] < -IKPI )
5606 { j3array[0]+=IK2PI;
5607 }
5608 j3valid[0] = true;
5609 for(int ij3 = 0; ij3 < 1; ++ij3)
5610 {
5611 if( !j3valid[ij3] )
5612 {
5613  continue;
5614 }
5615 _ij3[0] = ij3; _ij3[1] = -1;
5616 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5617 {
5618 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5619 {
5620  j3valid[iij3]=false; _ij3[1] = iij3; break;
5621 }
5622 }
5623 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5624 {
5625 IkReal evalcond[6];
5626 IkReal x440=IKsin(j3);
5627 IkReal x441=IKcos(j3);
5628 IkReal x442=(gconst7*x440);
5629 IkReal x443=((1.0)*x441);
5630 IkReal x444=(gconst8*x440);
5631 IkReal x445=(gconst8*x443);
5632 evalcond[0]=(((new_r01*x441))+gconst7+((new_r11*x440)));
5633 evalcond[1]=(((gconst7*x441))+x444+new_r01);
5634 evalcond[2]=((((-1.0)*x445))+x442);
5635 evalcond[3]=(((new_r01*x440))+gconst8+(((-1.0)*new_r11*x443)));
5636 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5637 evalcond[5]=((((-1.0)*x444))+(((-1.0)*gconst7*x443)));
5638 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5639 {
5640 continue;
5641 }
5642 }
5643 
5644 {
5645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5646 vinfos[0].jointtype = 1;
5647 vinfos[0].foffset = j0;
5648 vinfos[0].indices[0] = _ij0[0];
5649 vinfos[0].indices[1] = _ij0[1];
5650 vinfos[0].maxsolutions = _nj0;
5651 vinfos[1].jointtype = 1;
5652 vinfos[1].foffset = j1;
5653 vinfos[1].indices[0] = _ij1[0];
5654 vinfos[1].indices[1] = _ij1[1];
5655 vinfos[1].maxsolutions = _nj1;
5656 vinfos[2].jointtype = 1;
5657 vinfos[2].foffset = j2;
5658 vinfos[2].indices[0] = _ij2[0];
5659 vinfos[2].indices[1] = _ij2[1];
5660 vinfos[2].maxsolutions = _nj2;
5661 vinfos[3].jointtype = 1;
5662 vinfos[3].foffset = j3;
5663 vinfos[3].indices[0] = _ij3[0];
5664 vinfos[3].indices[1] = _ij3[1];
5665 vinfos[3].maxsolutions = _nj3;
5666 vinfos[4].jointtype = 1;
5667 vinfos[4].foffset = j4;
5668 vinfos[4].indices[0] = _ij4[0];
5669 vinfos[4].indices[1] = _ij4[1];
5670 vinfos[4].maxsolutions = _nj4;
5671 vinfos[5].jointtype = 1;
5672 vinfos[5].foffset = j5;
5673 vinfos[5].indices[0] = _ij5[0];
5674 vinfos[5].indices[1] = _ij5[1];
5675 vinfos[5].maxsolutions = _nj5;
5676 std::vector<int> vfree(0);
5677 solutions.AddSolution(vinfos,vfree);
5678 }
5679 }
5680 }
5681 
5682 }
5683 
5684 }
5685 
5686 } else
5687 {
5688 {
5689 IkReal j3array[1], cj3array[1], sj3array[1];
5690 bool j3valid[1]={false};
5691 _nj3 = 1;
5692 CheckValue<IkReal> x446=IKPowWithIntegerCheck(IKsign((((gconst8*new_r01))+(((-1.0)*gconst7*new_r11)))),-1);
5693 if(!x446.valid){
5694 continue;
5695 }
5696 CheckValue<IkReal> x447 = IKatan2WithCheck(IkReal(gconst7*gconst7),IkReal(((-1.0)*gconst7*gconst8)),IKFAST_ATAN2_MAGTHRESH);
5697 if(!x447.valid){
5698 continue;
5699 }
5700 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x446.value)))+(x447.value));
5701 sj3array[0]=IKsin(j3array[0]);
5702 cj3array[0]=IKcos(j3array[0]);
5703 if( j3array[0] > IKPI )
5704 {
5705  j3array[0]-=IK2PI;
5706 }
5707 else if( j3array[0] < -IKPI )
5708 { j3array[0]+=IK2PI;
5709 }
5710 j3valid[0] = true;
5711 for(int ij3 = 0; ij3 < 1; ++ij3)
5712 {
5713 if( !j3valid[ij3] )
5714 {
5715  continue;
5716 }
5717 _ij3[0] = ij3; _ij3[1] = -1;
5718 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5719 {
5720 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5721 {
5722  j3valid[iij3]=false; _ij3[1] = iij3; break;
5723 }
5724 }
5725 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5726 {
5727 IkReal evalcond[6];
5728 IkReal x448=IKsin(j3);
5729 IkReal x449=IKcos(j3);
5730 IkReal x450=(gconst7*x448);
5731 IkReal x451=((1.0)*x449);
5732 IkReal x452=(gconst8*x448);
5733 IkReal x453=(gconst8*x451);
5734 evalcond[0]=(((new_r01*x449))+gconst7+((new_r11*x448)));
5735 evalcond[1]=(((gconst7*x449))+x452+new_r01);
5736 evalcond[2]=((((-1.0)*x453))+x450);
5737 evalcond[3]=(((new_r01*x448))+gconst8+(((-1.0)*new_r11*x451)));
5738 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5739 evalcond[5]=((((-1.0)*x452))+(((-1.0)*gconst7*x451)));
5740 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5741 {
5742 continue;
5743 }
5744 }
5745 
5746 {
5747 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5748 vinfos[0].jointtype = 1;
5749 vinfos[0].foffset = j0;
5750 vinfos[0].indices[0] = _ij0[0];
5751 vinfos[0].indices[1] = _ij0[1];
5752 vinfos[0].maxsolutions = _nj0;
5753 vinfos[1].jointtype = 1;
5754 vinfos[1].foffset = j1;
5755 vinfos[1].indices[0] = _ij1[0];
5756 vinfos[1].indices[1] = _ij1[1];
5757 vinfos[1].maxsolutions = _nj1;
5758 vinfos[2].jointtype = 1;
5759 vinfos[2].foffset = j2;
5760 vinfos[2].indices[0] = _ij2[0];
5761 vinfos[2].indices[1] = _ij2[1];
5762 vinfos[2].maxsolutions = _nj2;
5763 vinfos[3].jointtype = 1;
5764 vinfos[3].foffset = j3;
5765 vinfos[3].indices[0] = _ij3[0];
5766 vinfos[3].indices[1] = _ij3[1];
5767 vinfos[3].maxsolutions = _nj3;
5768 vinfos[4].jointtype = 1;
5769 vinfos[4].foffset = j4;
5770 vinfos[4].indices[0] = _ij4[0];
5771 vinfos[4].indices[1] = _ij4[1];
5772 vinfos[4].maxsolutions = _nj4;
5773 vinfos[5].jointtype = 1;
5774 vinfos[5].foffset = j5;
5775 vinfos[5].indices[0] = _ij5[0];
5776 vinfos[5].indices[1] = _ij5[1];
5777 vinfos[5].maxsolutions = _nj5;
5778 std::vector<int> vfree(0);
5779 solutions.AddSolution(vinfos,vfree);
5780 }
5781 }
5782 }
5783 
5784 }
5785 
5786 }
5787 
5788 }
5789 } while(0);
5790 if( bgotonextstatement )
5791 {
5792 bool bgotonextstatement = true;
5793 do
5794 {
5795 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5796 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5797 {
5798 bgotonextstatement=false;
5799 {
5800 IkReal j3eval[1];
5801 CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5802 if(!x455.valid){
5803 continue;
5804 }
5805 IkReal x454=((-1.0)*(x455.value));
5806 sj4=0;
5807 cj4=1.0;
5808 j4=0;
5809 sj5=gconst7;
5810 cj5=gconst8;
5811 j5=x454;
5812 new_r01=0;
5813 new_r10=0;
5814 IkReal gconst6=x454;
5815 IkReal x456 = new_r11*new_r11;
5816 if(IKabs(x456)==0){
5817 continue;
5818 }
5819 IkReal gconst7=(new_r11*(pow(x456,-0.5)));
5820 IkReal gconst8=0;
5821 j3eval[0]=new_r00;
5822 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5823 {
5824 {
5825 IkReal j3eval[1];
5826 CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5827 if(!x458.valid){
5828 continue;
5829 }
5830 IkReal x457=((-1.0)*(x458.value));
5831 sj4=0;
5832 cj4=1.0;
5833 j4=0;
5834 sj5=gconst7;
5835 cj5=gconst8;
5836 j5=x457;
5837 new_r01=0;
5838 new_r10=0;
5839 IkReal gconst6=x457;
5840 IkReal x459 = new_r11*new_r11;
5841 if(IKabs(x459)==0){
5842 continue;
5843 }
5844 IkReal gconst7=(new_r11*(pow(x459,-0.5)));
5845 IkReal gconst8=0;
5846 j3eval[0]=new_r11;
5847 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5848 {
5849 {
5850 IkReal j3array[2], cj3array[2], sj3array[2];
5851 bool j3valid[2]={false};
5852 _nj3 = 2;
5853 CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst7,-1);
5854 if(!x460.valid){
5855 continue;
5856 }
5857 sj3array[0]=((-1.0)*new_r00*(x460.value));
5858 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5859 {
5860  j3valid[0] = j3valid[1] = true;
5861  j3array[0] = IKasin(sj3array[0]);
5862  cj3array[0] = IKcos(j3array[0]);
5863  sj3array[1] = sj3array[0];
5864  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5865  cj3array[1] = -cj3array[0];
5866 }
5867 else if( isnan(sj3array[0]) )
5868 {
5869  // probably any value will work
5870  j3valid[0] = true;
5871  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5872 }
5873 for(int ij3 = 0; ij3 < 2; ++ij3)
5874 {
5875 if( !j3valid[ij3] )
5876 {
5877  continue;
5878 }
5879 _ij3[0] = ij3; _ij3[1] = -1;
5880 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5881 {
5882 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5883 {
5884  j3valid[iij3]=false; _ij3[1] = iij3; break;
5885 }
5886 }
5887 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5888 {
5889 IkReal evalcond[6];
5890 IkReal x461=IKcos(j3);
5891 IkReal x462=IKsin(j3);
5892 evalcond[0]=(gconst7*x461);
5893 evalcond[1]=(new_r00*x461);
5894 evalcond[2]=((-1.0)*new_r11*x461);
5895 evalcond[3]=(((new_r00*x462))+gconst7);
5896 evalcond[4]=(((new_r11*x462))+gconst7);
5897 evalcond[5]=(((gconst7*x462))+new_r11);
5898 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5899 {
5900 continue;
5901 }
5902 }
5903 
5904 {
5905 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5906 vinfos[0].jointtype = 1;
5907 vinfos[0].foffset = j0;
5908 vinfos[0].indices[0] = _ij0[0];
5909 vinfos[0].indices[1] = _ij0[1];
5910 vinfos[0].maxsolutions = _nj0;
5911 vinfos[1].jointtype = 1;
5912 vinfos[1].foffset = j1;
5913 vinfos[1].indices[0] = _ij1[0];
5914 vinfos[1].indices[1] = _ij1[1];
5915 vinfos[1].maxsolutions = _nj1;
5916 vinfos[2].jointtype = 1;
5917 vinfos[2].foffset = j2;
5918 vinfos[2].indices[0] = _ij2[0];
5919 vinfos[2].indices[1] = _ij2[1];
5920 vinfos[2].maxsolutions = _nj2;
5921 vinfos[3].jointtype = 1;
5922 vinfos[3].foffset = j3;
5923 vinfos[3].indices[0] = _ij3[0];
5924 vinfos[3].indices[1] = _ij3[1];
5925 vinfos[3].maxsolutions = _nj3;
5926 vinfos[4].jointtype = 1;
5927 vinfos[4].foffset = j4;
5928 vinfos[4].indices[0] = _ij4[0];
5929 vinfos[4].indices[1] = _ij4[1];
5930 vinfos[4].maxsolutions = _nj4;
5931 vinfos[5].jointtype = 1;
5932 vinfos[5].foffset = j5;
5933 vinfos[5].indices[0] = _ij5[0];
5934 vinfos[5].indices[1] = _ij5[1];
5935 vinfos[5].maxsolutions = _nj5;
5936 std::vector<int> vfree(0);
5937 solutions.AddSolution(vinfos,vfree);
5938 }
5939 }
5940 }
5941 
5942 } else
5943 {
5944 {
5945 IkReal j3array[2], cj3array[2], sj3array[2];
5946 bool j3valid[2]={false};
5947 _nj3 = 2;
5948 CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
5949 if(!x463.valid){
5950 continue;
5951 }
5952 sj3array[0]=((-1.0)*gconst7*(x463.value));
5953 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5954 {
5955  j3valid[0] = j3valid[1] = true;
5956  j3array[0] = IKasin(sj3array[0]);
5957  cj3array[0] = IKcos(j3array[0]);
5958  sj3array[1] = sj3array[0];
5959  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5960  cj3array[1] = -cj3array[0];
5961 }
5962 else if( isnan(sj3array[0]) )
5963 {
5964  // probably any value will work
5965  j3valid[0] = true;
5966  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5967 }
5968 for(int ij3 = 0; ij3 < 2; ++ij3)
5969 {
5970 if( !j3valid[ij3] )
5971 {
5972  continue;
5973 }
5974 _ij3[0] = ij3; _ij3[1] = -1;
5975 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5976 {
5977 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5978 {
5979  j3valid[iij3]=false; _ij3[1] = iij3; break;
5980 }
5981 }
5982 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5983 {
5984 IkReal evalcond[6];
5985 IkReal x464=IKcos(j3);
5986 IkReal x465=IKsin(j3);
5987 IkReal x466=(gconst7*x465);
5988 evalcond[0]=(gconst7*x464);
5989 evalcond[1]=(new_r00*x464);
5990 evalcond[2]=((-1.0)*new_r11*x464);
5991 evalcond[3]=(((new_r00*x465))+gconst7);
5992 evalcond[4]=(x466+new_r00);
5993 evalcond[5]=(x466+new_r11);
5994 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5995 {
5996 continue;
5997 }
5998 }
5999 
6000 {
6001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6002 vinfos[0].jointtype = 1;
6003 vinfos[0].foffset = j0;
6004 vinfos[0].indices[0] = _ij0[0];
6005 vinfos[0].indices[1] = _ij0[1];
6006 vinfos[0].maxsolutions = _nj0;
6007 vinfos[1].jointtype = 1;
6008 vinfos[1].foffset = j1;
6009 vinfos[1].indices[0] = _ij1[0];
6010 vinfos[1].indices[1] = _ij1[1];
6011 vinfos[1].maxsolutions = _nj1;
6012 vinfos[2].jointtype = 1;
6013 vinfos[2].foffset = j2;
6014 vinfos[2].indices[0] = _ij2[0];
6015 vinfos[2].indices[1] = _ij2[1];
6016 vinfos[2].maxsolutions = _nj2;
6017 vinfos[3].jointtype = 1;
6018 vinfos[3].foffset = j3;
6019 vinfos[3].indices[0] = _ij3[0];
6020 vinfos[3].indices[1] = _ij3[1];
6021 vinfos[3].maxsolutions = _nj3;
6022 vinfos[4].jointtype = 1;
6023 vinfos[4].foffset = j4;
6024 vinfos[4].indices[0] = _ij4[0];
6025 vinfos[4].indices[1] = _ij4[1];
6026 vinfos[4].maxsolutions = _nj4;
6027 vinfos[5].jointtype = 1;
6028 vinfos[5].foffset = j5;
6029 vinfos[5].indices[0] = _ij5[0];
6030 vinfos[5].indices[1] = _ij5[1];
6031 vinfos[5].maxsolutions = _nj5;
6032 std::vector<int> vfree(0);
6033 solutions.AddSolution(vinfos,vfree);
6034 }
6035 }
6036 }
6037 
6038 }
6039 
6040 }
6041 
6042 } else
6043 {
6044 {
6045 IkReal j3array[2], cj3array[2], sj3array[2];
6046 bool j3valid[2]={false};
6047 _nj3 = 2;
6048 CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6049 if(!x467.valid){
6050 continue;
6051 }
6052 sj3array[0]=((-1.0)*gconst7*(x467.value));
6053 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6054 {
6055  j3valid[0] = j3valid[1] = true;
6056  j3array[0] = IKasin(sj3array[0]);
6057  cj3array[0] = IKcos(j3array[0]);
6058  sj3array[1] = sj3array[0];
6059  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6060  cj3array[1] = -cj3array[0];
6061 }
6062 else if( isnan(sj3array[0]) )
6063 {
6064  // probably any value will work
6065  j3valid[0] = true;
6066  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6067 }
6068 for(int ij3 = 0; ij3 < 2; ++ij3)
6069 {
6070 if( !j3valid[ij3] )
6071 {
6072  continue;
6073 }
6074 _ij3[0] = ij3; _ij3[1] = -1;
6075 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6076 {
6077 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6078 {
6079  j3valid[iij3]=false; _ij3[1] = iij3; break;
6080 }
6081 }
6082 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6083 {
6084 IkReal evalcond[6];
6085 IkReal x468=IKcos(j3);
6086 IkReal x469=IKsin(j3);
6087 IkReal x470=(gconst7*x469);
6088 evalcond[0]=(gconst7*x468);
6089 evalcond[1]=(new_r00*x468);
6090 evalcond[2]=((-1.0)*new_r11*x468);
6091 evalcond[3]=(((new_r11*x469))+gconst7);
6092 evalcond[4]=(x470+new_r00);
6093 evalcond[5]=(x470+new_r11);
6094 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6095 {
6096 continue;
6097 }
6098 }
6099 
6100 {
6101 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6102 vinfos[0].jointtype = 1;
6103 vinfos[0].foffset = j0;
6104 vinfos[0].indices[0] = _ij0[0];
6105 vinfos[0].indices[1] = _ij0[1];
6106 vinfos[0].maxsolutions = _nj0;
6107 vinfos[1].jointtype = 1;
6108 vinfos[1].foffset = j1;
6109 vinfos[1].indices[0] = _ij1[0];
6110 vinfos[1].indices[1] = _ij1[1];
6111 vinfos[1].maxsolutions = _nj1;
6112 vinfos[2].jointtype = 1;
6113 vinfos[2].foffset = j2;
6114 vinfos[2].indices[0] = _ij2[0];
6115 vinfos[2].indices[1] = _ij2[1];
6116 vinfos[2].maxsolutions = _nj2;
6117 vinfos[3].jointtype = 1;
6118 vinfos[3].foffset = j3;
6119 vinfos[3].indices[0] = _ij3[0];
6120 vinfos[3].indices[1] = _ij3[1];
6121 vinfos[3].maxsolutions = _nj3;
6122 vinfos[4].jointtype = 1;
6123 vinfos[4].foffset = j4;
6124 vinfos[4].indices[0] = _ij4[0];
6125 vinfos[4].indices[1] = _ij4[1];
6126 vinfos[4].maxsolutions = _nj4;
6127 vinfos[5].jointtype = 1;
6128 vinfos[5].foffset = j5;
6129 vinfos[5].indices[0] = _ij5[0];
6130 vinfos[5].indices[1] = _ij5[1];
6131 vinfos[5].maxsolutions = _nj5;
6132 std::vector<int> vfree(0);
6133 solutions.AddSolution(vinfos,vfree);
6134 }
6135 }
6136 }
6137 
6138 }
6139 
6140 }
6141 
6142 }
6143 } while(0);
6144 if( bgotonextstatement )
6145 {
6146 bool bgotonextstatement = true;
6147 do
6148 {
6149 evalcond[0]=IKabs(new_r11);
6150 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6151 {
6152 bgotonextstatement=false;
6153 {
6154 IkReal j3eval[1];
6155 IkReal x471=((-1.0)*new_r01);
6156 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6157 if(!x473.valid){
6158 continue;
6159 }
6160 IkReal x472=((-1.0)*(x473.value));
6161 sj4=0;
6162 cj4=1.0;
6163 j4=0;
6164 sj5=gconst7;
6165 cj5=gconst8;
6166 j5=x472;
6167 new_r11=0;
6168 IkReal gconst6=x472;
6169 IkReal gconst7=0;
6170 IkReal x474 = new_r01*new_r01;
6171 if(IKabs(x474)==0){
6172 continue;
6173 }
6174 IkReal gconst8=(x471*(pow(x474,-0.5)));
6175 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6176 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6177 {
6178 {
6179 IkReal j3eval[1];
6180 IkReal x475=((-1.0)*new_r01);
6181 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6182 if(!x477.valid){
6183 continue;
6184 }
6185 IkReal x476=((-1.0)*(x477.value));
6186 sj4=0;
6187 cj4=1.0;
6188 j4=0;
6189 sj5=gconst7;
6190 cj5=gconst8;
6191 j5=x476;
6192 new_r11=0;
6193 IkReal gconst6=x476;
6194 IkReal gconst7=0;
6195 IkReal x478 = new_r01*new_r01;
6196 if(IKabs(x478)==0){
6197 continue;
6198 }
6199 IkReal gconst8=(x475*(pow(x478,-0.5)));
6200 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6201 if( IKabs(j3eval[0]) < 0.0000010000000000 )