rs080n_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;
301 x0=IKsin(j[0]);
302 x1=IKcos(j[3]);
303 x2=IKcos(j[0]);
304 x3=IKsin(j[3]);
305 x4=IKcos(j[2]);
306 x5=IKsin(j[1]);
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=((1.0)*x1);
314 x13=((0.165)*x2);
315 x14=((1.08)*x2);
316 x15=((0.165)*x0);
317 x16=((1.08)*x0);
318 x17=((1.0)*x3);
319 x18=((1.0)*x2);
320 x19=((0.165)*x1);
321 x20=((1.0)*x0);
322 x21=(x4*x5);
323 x22=(x6*x7);
324 x23=((-1.0)*x11);
325 x24=((-1.0)*x10);
326 x25=(x4*x6);
327 x26=(x0*x5);
328 x27=(x5*x7);
329 x28=(x1*x11);
330 x29=(x17*x2);
331 x30=((1.0)*x22);
332 x31=(x18*x22);
333 x32=((((-1.0)*x30))+x21);
334 x33=((((-1.0)*x27))+(((-1.0)*x25)));
335 x34=(x1*x32);
336 x35=(((x0*x25))+((x26*x7)));
337 x36=(x10*x34);
338 x37=(x20*(((((-1.0)*x22))+x21)));
339 x38=(x18*(((((-1.0)*x27))+(((-1.0)*x25)))));
340 x39=(x3*x35);
341 x40=(x1*x38);
342 x41=(((x11*x37))+((x10*(((((-1.0)*x29))+((x1*x35)))))));
343 x42=(((x24*(((((-1.0)*x0*x3))+x40))))+((x23*(((((-1.0)*x18*x21))+x31)))));
344 eerot[0]=(((x41*x8))+((x9*((((x1*x2))+x39)))));
345 eerot[1]=(((x8*(((((-1.0)*x12*x2))+(((-1.0)*x17*x35))))))+((x41*x9)));
346 eerot[2]=(((x11*(((((-1.0)*x12*x35))+x29))))+((x10*x37)));
347 IkReal x43=((1.0)*x22);
348 eetrans[0]=(((x10*(((((-1.0)*x15*x43))+((x15*x21))))))+(((0.15)*x0))+((x16*x21))+((x11*(((((-1.0)*x19*x35))+((x13*x3))))))+(((-1.0)*x16*x43))+(((0.87)*x26)));
349 eerot[3]=(((x42*x8))+((x9*(((((-1.0)*x0*x12))+(((-1.0)*x17*x38)))))));
350 eerot[4]=(((x8*((((x0*x1))+((x3*x38))))))+((x42*x9)));
351 eerot[5]=(((x11*(((((-1.0)*x0*x17))+x40))))+((x10*(((((-1.0)*x31))+((x2*x21)))))));
352 IkReal x44=((1.0)*x22);
353 eetrans[1]=(((x11*((((x19*x38))+(((-1.0)*x15*x3))))))+(((0.87)*x2*x5))+((x10*((((x13*x21))+(((-1.0)*x13*x44))))))+(((0.15)*x2))+((x14*x21))+(((-1.0)*x14*x44)));
354 eerot[6]=(((x8*(((((-1.0)*x10*x12*x32))+(((-1.0)*x11*x33))))))+((x3*x9*(((((-1.0)*x21))+x30)))));
355 eerot[7]=(((x9*((((x23*x33))+((x24*x34))))))+((x3*x32*x8)));
356 eerot[8]=(((x28*x32))+((x10*((x25+x27)))));
357 eetrans[2]=((0.68)+((x10*(((((0.165)*x27))+(((0.165)*x25))))))+((x28*(((((-0.165)*x22))+(((0.165)*x21))))))+(((1.08)*x25))+(((1.08)*x27))+(((0.87)*x6)));
358 }
359 
360 IKFAST_API int GetNumFreeParameters() { return 0; }
361 IKFAST_API int* GetFreeParameters() { return NULL; }
362 IKFAST_API int GetNumJoints() { return 6; }
363 
364 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
365 
366 IKFAST_API int GetIkType() { return 0x67000001; }
367 
368 class IKSolver {
369 public:
370 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;
371 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
372 
373 IkReal j100, cj100, sj100;
374 unsigned char _ij100[2], _nj100;
375 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
376 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;
377 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
378  solutions.Clear();
379 r00 = eerot[0*3+0];
380 r01 = eerot[0*3+1];
381 r02 = eerot[0*3+2];
382 r10 = eerot[1*3+0];
383 r11 = eerot[1*3+1];
384 r12 = eerot[1*3+2];
385 r20 = eerot[2*3+0];
386 r21 = eerot[2*3+1];
387 r22 = eerot[2*3+2];
388 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
389 
390 new_r00=r00;
391 new_r01=r01;
392 new_r02=r02;
393 new_px=(px+(((-0.165)*r02)));
394 new_r10=((-1.0)*r10);
395 new_r11=((-1.0)*r11);
396 new_r12=((-1.0)*r12);
397 new_py=((((0.165)*r12))+(((-1.0)*py)));
398 new_r20=((-1.0)*r20);
399 new_r21=((-1.0)*r21);
400 new_r22=((-1.0)*r22);
401 new_pz=((0.68)+(((-1.0)*pz))+(((0.165)*r22)));
402 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;
403 IkReal x45=((1.0)*px);
404 IkReal x46=((1.0)*pz);
405 IkReal x47=((1.0)*py);
406 pp=((px*px)+(py*py)+(pz*pz));
407 npx=(((px*r00))+((py*r10))+((pz*r20)));
408 npy=(((px*r01))+((py*r11))+((pz*r21)));
409 npz=(((px*r02))+((py*r12))+((pz*r22)));
410 rxp0_0=((((-1.0)*r20*x47))+((pz*r10)));
411 rxp0_1=(((px*r20))+(((-1.0)*r00*x46)));
412 rxp0_2=((((-1.0)*r10*x45))+((py*r00)));
413 rxp1_0=((((-1.0)*r21*x47))+((pz*r11)));
414 rxp1_1=(((px*r21))+(((-1.0)*r01*x46)));
415 rxp1_2=((((-1.0)*r11*x45))+((py*r01)));
416 rxp2_0=((((-1.0)*r22*x47))+((pz*r12)));
417 rxp2_1=((((-1.0)*r02*x46))+((px*r22)));
418 rxp2_2=((((-1.0)*r12*x45))+((py*r02)));
419 {
420 IkReal j0eval[1];
421 j0eval[0]=((IKabs(px))+(IKabs(py)));
422 if( IKabs(j0eval[0]) < 0.0000010000000000 )
423 {
424 continue; // no branches [j0, j1, j2]
425 
426 } else
427 {
428 {
429 IkReal j0array[2], cj0array[2], sj0array[2];
430 bool j0valid[2]={false};
431 _nj0 = 2;
432 CheckValue<IkReal> x49 = IKatan2WithCheck(IkReal(((-1.0)*px)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH);
433 if(!x49.valid){
434 continue;
435 }
436 IkReal x48=x49.value;
437 j0array[0]=((-1.0)*x48);
438 sj0array[0]=IKsin(j0array[0]);
439 cj0array[0]=IKcos(j0array[0]);
440 j0array[1]=((3.14159265358979)+(((-1.0)*x48)));
441 sj0array[1]=IKsin(j0array[1]);
442 cj0array[1]=IKcos(j0array[1]);
443 if( j0array[0] > IKPI )
444 {
445  j0array[0]-=IK2PI;
446 }
447 else if( j0array[0] < -IKPI )
448 { j0array[0]+=IK2PI;
449 }
450 j0valid[0] = true;
451 if( j0array[1] > IKPI )
452 {
453  j0array[1]-=IK2PI;
454 }
455 else if( j0array[1] < -IKPI )
456 { j0array[1]+=IK2PI;
457 }
458 j0valid[1] = true;
459 for(int ij0 = 0; ij0 < 2; ++ij0)
460 {
461 if( !j0valid[ij0] )
462 {
463  continue;
464 }
465 _ij0[0] = ij0; _ij0[1] = -1;
466 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
467 {
468 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
469 {
470  j0valid[iij0]=false; _ij0[1] = iij0; break;
471 }
472 }
473 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
474 
475 {
476 IkReal j2array[2], cj2array[2], sj2array[2];
477 bool j2valid[2]={false};
478 _nj2 = 2;
479 cj2array[0]=((-1.01149425287356)+(((0.532141336739038)*pp))+(((0.159642401021711)*cj0*py))+(((-0.159642401021711)*px*sj0)));
480 if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
481 {
482  j2valid[0] = j2valid[1] = true;
483  j2array[0] = IKacos(cj2array[0]);
484  sj2array[0] = IKsin(j2array[0]);
485  cj2array[1] = cj2array[0];
486  j2array[1] = -j2array[0];
487  sj2array[1] = -sj2array[0];
488 }
489 else if( isnan(cj2array[0]) )
490 {
491  // probably any value will work
492  j2valid[0] = true;
493  cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
494 }
495 for(int ij2 = 0; ij2 < 2; ++ij2)
496 {
497 if( !j2valid[ij2] )
498 {
499  continue;
500 }
501 _ij2[0] = ij2; _ij2[1] = -1;
502 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
503 {
504 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
505 {
506  j2valid[iij2]=false; _ij2[1] = iij2; break;
507 }
508 }
509 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
510 
511 {
512 IkReal j1eval[3];
513 IkReal x50=(cj0*py);
514 IkReal x51=(px*sj0);
515 IkReal x52=((675.0)*cj2);
516 IkReal x53=((675.0)*sj2);
517 j1eval[0]=((-1.02346743295019)+(((-1.0)*cj2)));
518 j1eval[1]=((IKabs(((81.5625)+(((543.75)*x50))+(((-543.75)*x51))+(((-1.0)*x51*x52))+((x50*x52))+((pz*x53))+(((101.25)*cj2)))))+(IKabs(((((543.75)*pz))+(((-101.25)*sj2))+((x51*x53))+((pz*x52))+(((-1.0)*x50*x53))))));
519 j1eval[2]=IKsign(((-1202.0625)+(((-1174.5)*cj2))));
520 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
521 {
522 {
523 IkReal j1eval[3];
524 IkReal x54=((27.0)*cj2);
525 IkReal x55=(cj0*py);
526 IkReal x56=(px*sj0);
527 IkReal x57=(pz*sj2);
528 IkReal x58=((8.27586206896552)*cj2);
529 IkReal x59=((25.0)*pz);
530 j1eval[0]=((1.0)+(((-8.27586206896552)*x57))+(((-1.0)*x56*x58))+(((6.66666666666667)*x55))+(((1.24137931034483)*cj2))+((x55*x58))+(((-6.66666666666667)*x56)));
531 j1eval[1]=((IKabs(((((23.49)*sj2))+(((-1.0)*x55*x59))+(((29.16)*cj2*sj2))+((x56*x59))+(((-3.75)*pz)))))+(IKabs(((-18.9225)+(((-46.98)*cj2))+(((-29.16)*(cj2*cj2)))+((pz*x59))))));
532 j1eval[2]=IKsign(((3.2625)+(((-27.0)*x57))+(((21.75)*x55))+(((-1.0)*x54*x56))+(((-21.75)*x56))+(((4.05)*cj2))+((x54*x55))));
533 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
534 {
535 {
536 IkReal j1eval[3];
537 IkReal x60=cj0*cj0;
538 IkReal x61=px*px;
539 IkReal x62=pz*pz;
540 IkReal x63=py*py;
541 IkReal x64=(px*sj0);
542 IkReal x65=(cj0*py);
543 IkReal x66=((27.0)*cj2);
544 IkReal x67=((27.0)*sj2);
545 IkReal x68=((25.0)*x61);
546 IkReal x69=((44.4444444444444)*x61);
547 IkReal x70=(x60*x63);
548 j1eval[0]=((-1.0)+(((-44.4444444444444)*x62))+((x60*x69))+(((13.3333333333333)*x64))+(((-44.4444444444444)*x70))+(((-13.3333333333333)*x65))+(((-1.0)*x69))+(((88.8888888888889)*x64*x65)));
549 j1eval[1]=((IKabs(((3.2625)+(((21.75)*x65))+(((-1.0)*x64*x66))+(((-21.75)*x64))+((pz*x67))+((x65*x66))+(((4.05)*cj2)))))+(IKabs(((((-4.05)*sj2))+(((21.75)*pz))+(((-1.0)*x65*x67))+((pz*x66))+((x64*x67))))));
550 j1eval[2]=IKsign(((-0.5625)+(((-7.5)*x65))+((x60*x68))+(((50.0)*x64*x65))+(((-25.0)*x70))+(((-1.0)*x68))+(((-25.0)*x62))+(((7.5)*x64))));
551 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
552 {
553 continue; // no branches [j1]
554 
555 } else
556 {
557 {
558 IkReal j1array[1], cj1array[1], sj1array[1];
559 bool j1valid[1]={false};
560 _nj1 = 1;
561 IkReal x71=px*px;
562 IkReal x72=cj0*cj0;
563 IkReal x73=(px*sj0);
564 IkReal x74=(cj0*py);
565 IkReal x75=((27.0)*cj2);
566 IkReal x76=((27.0)*sj2);
567 IkReal x77=((25.0)*x71);
568 CheckValue<IkReal> x78 = IKatan2WithCheck(IkReal(((3.2625)+((pz*x76))+(((-21.75)*x73))+(((-1.0)*x73*x75))+(((21.75)*x74))+((x74*x75))+(((4.05)*cj2)))),IkReal((((pz*x75))+(((-4.05)*sj2))+(((21.75)*pz))+(((-1.0)*x74*x76))+((x73*x76)))),IKFAST_ATAN2_MAGTHRESH);
569 if(!x78.valid){
570 continue;
571 }
572 CheckValue<IkReal> x79=IKPowWithIntegerCheck(IKsign(((-0.5625)+(((50.0)*x73*x74))+(((-1.0)*x77))+(((-25.0)*(pz*pz)))+(((-25.0)*x72*(py*py)))+((x72*x77))+(((7.5)*x73))+(((-7.5)*x74)))),-1);
573 if(!x79.valid){
574 continue;
575 }
576 j1array[0]=((-1.5707963267949)+(x78.value)+(((1.5707963267949)*(x79.value))));
577 sj1array[0]=IKsin(j1array[0]);
578 cj1array[0]=IKcos(j1array[0]);
579 if( j1array[0] > IKPI )
580 {
581  j1array[0]-=IK2PI;
582 }
583 else if( j1array[0] < -IKPI )
584 { j1array[0]+=IK2PI;
585 }
586 j1valid[0] = true;
587 for(int ij1 = 0; ij1 < 1; ++ij1)
588 {
589 if( !j1valid[ij1] )
590 {
591  continue;
592 }
593 _ij1[0] = ij1; _ij1[1] = -1;
594 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
595 {
596 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
597 {
598  j1valid[iij1]=false; _ij1[1] = iij1; break;
599 }
600 }
601 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
602 {
603 IkReal evalcond[5];
604 IkReal x80=IKcos(j1);
605 IkReal x81=IKsin(j1);
606 IkReal x82=(cj0*py);
607 IkReal x83=(px*sj0);
608 IkReal x84=((1.08)*sj2);
609 IkReal x85=((1.08)*cj2);
610 IkReal x86=(pz*x80);
611 IkReal x87=((1.74)*x81);
612 evalcond[0]=(((x81*x84))+(((0.87)*x80))+pz+((x80*x85)));
613 evalcond[1]=((0.15)+(((-1.0)*x80*x84))+((x81*x85))+(((0.87)*x81))+x82+(((-1.0)*x83)));
614 evalcond[2]=((((-1.0)*x80*x82))+((pz*x81))+x84+(((-0.15)*x80))+((x80*x83)));
615 evalcond[3]=((0.87)+(((0.15)*x81))+((x81*x82))+(((-1.0)*x81*x83))+x86+x85);
616 evalcond[4]=((0.387)+(((-1.0)*x82*x87))+(((0.3)*x83))+(((-0.261)*x81))+(((-1.74)*x86))+(((-1.0)*pp))+((x83*x87))+(((-0.3)*x82)));
617 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
618 {
619 continue;
620 }
621 }
622 
623 rotationfunction0(solutions);
624 }
625 }
626 
627 }
628 
629 }
630 
631 } else
632 {
633 {
634 IkReal j1array[1], cj1array[1], sj1array[1];
635 bool j1valid[1]={false};
636 _nj1 = 1;
637 IkReal x863=(px*sj0);
638 IkReal x864=(cj0*py);
639 IkReal x865=((27.0)*cj2);
640 IkReal x866=((25.0)*pz);
641 CheckValue<IkReal> x867 = IKatan2WithCheck(IkReal(((-18.9225)+(((-46.98)*cj2))+(((-29.16)*(cj2*cj2)))+((pz*x866)))),IkReal(((((23.49)*sj2))+((x863*x866))+(((-1.0)*x864*x866))+(((29.16)*cj2*sj2))+(((-3.75)*pz)))),IKFAST_ATAN2_MAGTHRESH);
642 if(!x867.valid){
643 continue;
644 }
645 CheckValue<IkReal> x868=IKPowWithIntegerCheck(IKsign(((3.2625)+(((21.75)*x864))+(((-21.75)*x863))+(((-27.0)*pz*sj2))+((x864*x865))+(((-1.0)*x863*x865))+(((4.05)*cj2)))),-1);
646 if(!x868.valid){
647 continue;
648 }
649 j1array[0]=((-1.5707963267949)+(x867.value)+(((1.5707963267949)*(x868.value))));
650 sj1array[0]=IKsin(j1array[0]);
651 cj1array[0]=IKcos(j1array[0]);
652 if( j1array[0] > IKPI )
653 {
654  j1array[0]-=IK2PI;
655 }
656 else if( j1array[0] < -IKPI )
657 { j1array[0]+=IK2PI;
658 }
659 j1valid[0] = true;
660 for(int ij1 = 0; ij1 < 1; ++ij1)
661 {
662 if( !j1valid[ij1] )
663 {
664  continue;
665 }
666 _ij1[0] = ij1; _ij1[1] = -1;
667 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
668 {
669 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
670 {
671  j1valid[iij1]=false; _ij1[1] = iij1; break;
672 }
673 }
674 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
675 {
676 IkReal evalcond[5];
677 IkReal x869=IKcos(j1);
678 IkReal x870=IKsin(j1);
679 IkReal x871=(cj0*py);
680 IkReal x872=(px*sj0);
681 IkReal x873=((1.08)*sj2);
682 IkReal x874=((1.08)*cj2);
683 IkReal x875=(pz*x869);
684 IkReal x876=((1.74)*x870);
685 evalcond[0]=(((x870*x873))+(((0.87)*x869))+pz+((x869*x874)));
686 evalcond[1]=((0.15)+((x870*x874))+(((-1.0)*x869*x873))+(((-1.0)*x872))+(((0.87)*x870))+x871);
687 evalcond[2]=((((-1.0)*x869*x871))+(((-0.15)*x869))+((x869*x872))+x873+((pz*x870)));
688 evalcond[3]=((0.87)+((x870*x871))+(((-1.0)*x870*x872))+(((0.15)*x870))+x874+x875);
689 evalcond[4]=((0.387)+(((-0.3)*x871))+(((0.3)*x872))+((x872*x876))+(((-1.0)*x871*x876))+(((-0.261)*x870))+(((-1.0)*pp))+(((-1.74)*x875)));
690 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
691 {
692 continue;
693 }
694 }
695 
696 rotationfunction0(solutions);
697 }
698 }
699 
700 }
701 
702 }
703 
704 } else
705 {
706 {
707 IkReal j1array[1], cj1array[1], sj1array[1];
708 bool j1valid[1]={false};
709 _nj1 = 1;
710 IkReal x877=(cj0*py);
711 IkReal x878=(px*sj0);
712 IkReal x879=((675.0)*cj2);
713 IkReal x880=((675.0)*sj2);
714 CheckValue<IkReal> x881 = IKatan2WithCheck(IkReal(((81.5625)+(((-543.75)*x878))+((pz*x880))+(((543.75)*x877))+(((-1.0)*x878*x879))+((x877*x879))+(((101.25)*cj2)))),IkReal(((((543.75)*pz))+(((-1.0)*x877*x880))+((x878*x880))+(((-101.25)*sj2))+((pz*x879)))),IKFAST_ATAN2_MAGTHRESH);
715 if(!x881.valid){
716 continue;
717 }
718 CheckValue<IkReal> x882=IKPowWithIntegerCheck(IKsign(((-1202.0625)+(((-1174.5)*cj2)))),-1);
719 if(!x882.valid){
720 continue;
721 }
722 j1array[0]=((-1.5707963267949)+(x881.value)+(((1.5707963267949)*(x882.value))));
723 sj1array[0]=IKsin(j1array[0]);
724 cj1array[0]=IKcos(j1array[0]);
725 if( j1array[0] > IKPI )
726 {
727  j1array[0]-=IK2PI;
728 }
729 else if( j1array[0] < -IKPI )
730 { j1array[0]+=IK2PI;
731 }
732 j1valid[0] = true;
733 for(int ij1 = 0; ij1 < 1; ++ij1)
734 {
735 if( !j1valid[ij1] )
736 {
737  continue;
738 }
739 _ij1[0] = ij1; _ij1[1] = -1;
740 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
741 {
742 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
743 {
744  j1valid[iij1]=false; _ij1[1] = iij1; break;
745 }
746 }
747 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
748 {
749 IkReal evalcond[5];
750 IkReal x883=IKcos(j1);
751 IkReal x884=IKsin(j1);
752 IkReal x885=(cj0*py);
753 IkReal x886=(px*sj0);
754 IkReal x887=((1.08)*sj2);
755 IkReal x888=((1.08)*cj2);
756 IkReal x889=(pz*x883);
757 IkReal x890=((1.74)*x884);
758 evalcond[0]=(((x883*x888))+pz+(((0.87)*x883))+((x884*x887)));
759 evalcond[1]=((0.15)+(((-1.0)*x883*x887))+(((-1.0)*x886))+(((0.87)*x884))+x885+((x884*x888)));
760 evalcond[2]=((((-0.15)*x883))+(((-1.0)*x883*x885))+((pz*x884))+((x883*x886))+x887);
761 evalcond[3]=((0.87)+(((-1.0)*x884*x886))+(((0.15)*x884))+x889+x888+((x884*x885)));
762 evalcond[4]=((0.387)+((x886*x890))+(((-1.74)*x889))+(((0.3)*x886))+(((-1.0)*pp))+(((-0.3)*x885))+(((-1.0)*x885*x890))+(((-0.261)*x884)));
763 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
764 {
765 continue;
766 }
767 }
768 
769 rotationfunction0(solutions);
770 }
771 }
772 
773 }
774 
775 }
776 }
777 }
778 }
779 }
780 
781 }
782 
783 }
784 }
785 return solutions.GetNumSolutions()>0;
786 }
787 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
788 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
789 IkReal x88=((1.0)*sj0);
790 IkReal x89=(cj0*r12);
791 IkReal x90=((1.0)*sj1);
792 IkReal x91=((1.0)*cj1);
793 IkReal x92=(cj0*r11);
794 IkReal x93=(cj0*r10);
795 IkReal x94=(((cj2*sj1))+(((-1.0)*sj2*x91)));
796 IkReal x95=(((cj1*sj2))+(((-1.0)*cj2*x90)));
797 IkReal x96=((((-1.0)*sj2*x90))+(((-1.0)*cj2*x91)));
798 new_r00=(((r10*sj0))+((cj0*r00)));
799 new_r01=(((r11*sj0))+((cj0*r01)));
800 new_r02=(((r12*sj0))+((cj0*r02)));
801 new_r10=(((r20*x94))+(((-1.0)*r00*x88*x96))+((x93*x96)));
802 new_r11=(((r21*x94))+((x92*x96))+(((-1.0)*r01*x88*x96)));
803 new_r12=(((x89*x96))+(((-1.0)*r02*x88*x96))+((r22*x94)));
804 new_r20=(((r20*x96))+(((-1.0)*r00*x88*x95))+((x93*x95)));
805 new_r21=(((r21*x96))+((x92*x95))+(((-1.0)*r01*x88*x95)));
806 new_r22=(((x89*x95))+(((-1.0)*r02*x88*x95))+((r22*x96)));
807 {
808 IkReal j4array[2], cj4array[2], sj4array[2];
809 bool j4valid[2]={false};
810 _nj4 = 2;
811 cj4array[0]=new_r22;
812 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
813 {
814  j4valid[0] = j4valid[1] = true;
815  j4array[0] = IKacos(cj4array[0]);
816  sj4array[0] = IKsin(j4array[0]);
817  cj4array[1] = cj4array[0];
818  j4array[1] = -j4array[0];
819  sj4array[1] = -sj4array[0];
820 }
821 else if( isnan(cj4array[0]) )
822 {
823  // probably any value will work
824  j4valid[0] = true;
825  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
826 }
827 for(int ij4 = 0; ij4 < 2; ++ij4)
828 {
829 if( !j4valid[ij4] )
830 {
831  continue;
832 }
833 _ij4[0] = ij4; _ij4[1] = -1;
834 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
835 {
836 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
837 {
838  j4valid[iij4]=false; _ij4[1] = iij4; break;
839 }
840 }
841 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
842 
843 {
844 IkReal j5eval[3];
845 j5eval[0]=sj4;
846 j5eval[1]=IKsign(sj4);
847 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
848 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
849 {
850 {
851 IkReal j3eval[3];
852 j3eval[0]=sj4;
853 j3eval[1]=IKsign(sj4);
854 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
855 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
856 {
857 {
858 IkReal j3eval[2];
859 j3eval[0]=new_r02;
860 j3eval[1]=sj4;
861 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
862 {
863 {
864 IkReal evalcond[5];
865 bool bgotonextstatement = true;
866 do
867 {
868 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
869 evalcond[1]=new_r12;
870 evalcond[2]=new_r02;
871 evalcond[3]=new_r20;
872 evalcond[4]=new_r21;
873 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 )
874 {
875 bgotonextstatement=false;
876 IkReal j5mul = 1;
877 j5=0;
878 j3mul=-1.0;
879 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 )
880  continue;
881 j3=IKatan2(((-1.0)*new_r01), new_r00);
882 {
883 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
884 vinfos[0].jointtype = 1;
885 vinfos[0].foffset = j0;
886 vinfos[0].indices[0] = _ij0[0];
887 vinfos[0].indices[1] = _ij0[1];
888 vinfos[0].maxsolutions = _nj0;
889 vinfos[1].jointtype = 1;
890 vinfos[1].foffset = j1;
891 vinfos[1].indices[0] = _ij1[0];
892 vinfos[1].indices[1] = _ij1[1];
893 vinfos[1].maxsolutions = _nj1;
894 vinfos[2].jointtype = 1;
895 vinfos[2].foffset = j2;
896 vinfos[2].indices[0] = _ij2[0];
897 vinfos[2].indices[1] = _ij2[1];
898 vinfos[2].maxsolutions = _nj2;
899 vinfos[3].jointtype = 1;
900 vinfos[3].foffset = j3;
901 vinfos[3].fmul = j3mul;
902 vinfos[3].freeind = 0;
903 vinfos[3].maxsolutions = 0;
904 vinfos[4].jointtype = 1;
905 vinfos[4].foffset = j4;
906 vinfos[4].indices[0] = _ij4[0];
907 vinfos[4].indices[1] = _ij4[1];
908 vinfos[4].maxsolutions = _nj4;
909 vinfos[5].jointtype = 1;
910 vinfos[5].foffset = j5;
911 vinfos[5].fmul = j5mul;
912 vinfos[5].freeind = 0;
913 vinfos[5].maxsolutions = 0;
914 std::vector<int> vfree(1);
915 vfree[0] = 5;
916 solutions.AddSolution(vinfos,vfree);
917 }
918 
919 }
920 } while(0);
921 if( bgotonextstatement )
922 {
923 bool bgotonextstatement = true;
924 do
925 {
926 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
927 evalcond[1]=new_r12;
928 evalcond[2]=new_r02;
929 evalcond[3]=new_r20;
930 evalcond[4]=new_r21;
931 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 )
932 {
933 bgotonextstatement=false;
934 IkReal j5mul = 1;
935 j5=0;
936 j3mul=1.0;
937 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
938  continue;
939 j3=IKatan2(new_r01, ((-1.0)*new_r11));
940 {
941 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
942 vinfos[0].jointtype = 1;
943 vinfos[0].foffset = j0;
944 vinfos[0].indices[0] = _ij0[0];
945 vinfos[0].indices[1] = _ij0[1];
946 vinfos[0].maxsolutions = _nj0;
947 vinfos[1].jointtype = 1;
948 vinfos[1].foffset = j1;
949 vinfos[1].indices[0] = _ij1[0];
950 vinfos[1].indices[1] = _ij1[1];
951 vinfos[1].maxsolutions = _nj1;
952 vinfos[2].jointtype = 1;
953 vinfos[2].foffset = j2;
954 vinfos[2].indices[0] = _ij2[0];
955 vinfos[2].indices[1] = _ij2[1];
956 vinfos[2].maxsolutions = _nj2;
957 vinfos[3].jointtype = 1;
958 vinfos[3].foffset = j3;
959 vinfos[3].fmul = j3mul;
960 vinfos[3].freeind = 0;
961 vinfos[3].maxsolutions = 0;
962 vinfos[4].jointtype = 1;
963 vinfos[4].foffset = j4;
964 vinfos[4].indices[0] = _ij4[0];
965 vinfos[4].indices[1] = _ij4[1];
966 vinfos[4].maxsolutions = _nj4;
967 vinfos[5].jointtype = 1;
968 vinfos[5].foffset = j5;
969 vinfos[5].fmul = j5mul;
970 vinfos[5].freeind = 0;
971 vinfos[5].maxsolutions = 0;
972 std::vector<int> vfree(1);
973 vfree[0] = 5;
974 solutions.AddSolution(vinfos,vfree);
975 }
976 
977 }
978 } while(0);
979 if( bgotonextstatement )
980 {
981 bool bgotonextstatement = true;
982 do
983 {
984 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
985 if( IKabs(evalcond[0]) < 0.0000050000000000 )
986 {
987 bgotonextstatement=false;
988 {
989 IkReal j3eval[1];
990 new_r21=0;
991 new_r20=0;
992 new_r02=0;
993 new_r12=0;
994 IkReal x97=new_r22*new_r22;
995 IkReal x98=((16.0)*new_r10);
996 IkReal x99=((16.0)*new_r01);
997 IkReal x100=((16.0)*new_r00);
998 IkReal x101=(new_r11*new_r22);
999 IkReal x102=((8.0)*new_r00);
1000 IkReal x103=(x97*x98);
1001 IkReal x104=(x97*x99);
1002 j3eval[0]=((IKabs((((new_r22*x102))+(((-8.0)*new_r11)))))+(IKabs(((((32.0)*new_r00))+(((-16.0)*x101))+(((-1.0)*x100*x97)))))+(IKabs((((x102*x97))+(((-8.0)*x101)))))+(IKabs((x104+(((-1.0)*x99)))))+(IKabs(((((-1.0)*x104))+x99)))+(IKabs(((((-32.0)*new_r11*x97))+((new_r22*x100))+(((16.0)*new_r11)))))+(IKabs(((((-1.0)*x103))+x98)))+(IKabs((x103+(((-1.0)*x98))))));
1003 if( IKabs(j3eval[0]) < 0.0000000010000000 )
1004 {
1005 continue; // no branches [j3, j5]
1006 
1007 } else
1008 {
1009 IkReal op[4+1], zeror[4];
1010 int numroots;
1011 IkReal j3evalpoly[1];
1012 IkReal x105=new_r22*new_r22;
1013 IkReal x106=((16.0)*new_r01);
1014 IkReal x107=(new_r00*new_r22);
1015 IkReal x108=(x105*x106);
1016 IkReal x109=((((8.0)*x107))+(((-8.0)*new_r11)));
1017 op[0]=x109;
1018 op[1]=((((-1.0)*x106))+x108);
1019 op[2]=((((-32.0)*new_r11*x105))+(((16.0)*x107))+(((16.0)*new_r11)));
1020 op[3]=((((-1.0)*x108))+x106);
1021 op[4]=x109;
1022 polyroots4(op,zeror,numroots);
1023 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1024 int numsolutions = 0;
1025 for(int ij3 = 0; ij3 < numroots; ++ij3)
1026 {
1027 IkReal htj3 = zeror[ij3];
1028 tempj3array[0]=((2.0)*(atan(htj3)));
1029 for(int kj3 = 0; kj3 < 1; ++kj3)
1030 {
1031 j3array[numsolutions] = tempj3array[kj3];
1032 if( j3array[numsolutions] > IKPI )
1033 {
1034  j3array[numsolutions]-=IK2PI;
1035 }
1036 else if( j3array[numsolutions] < -IKPI )
1037 {
1038  j3array[numsolutions]+=IK2PI;
1039 }
1040 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1041 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1042 numsolutions++;
1043 }
1044 }
1045 bool j3valid[4]={true,true,true,true};
1046 _nj3 = 4;
1047 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1048  {
1049 if( !j3valid[ij3] )
1050 {
1051  continue;
1052 }
1053  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1054 htj3 = IKtan(j3/2);
1055 
1056 IkReal x110=new_r22*new_r22;
1057 IkReal x111=((16.0)*new_r10);
1058 IkReal x112=(new_r11*new_r22);
1059 IkReal x113=((8.0)*x112);
1060 IkReal x114=(new_r00*x110);
1061 IkReal x115=(x110*x111);
1062 IkReal x116=((8.0)*x114);
1063 j3evalpoly[0]=((((htj3*htj3)*(((((32.0)*new_r00))+(((-16.0)*x114))+(((-16.0)*x112))))))+(((htj3*htj3*htj3)*(((((-1.0)*x115))+x111))))+(((-1.0)*x113))+x116+((htj3*(((((-1.0)*x111))+x115))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x113))+x116)))));
1064 if( IKabs(j3evalpoly[0]) > 0.0000000010000000 )
1065 {
1066  continue;
1067 }
1068 _ij3[0] = ij3; _ij3[1] = -1;
1069 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1070 {
1071 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1072 {
1073  j3valid[iij3]=false; _ij3[1] = iij3; break;
1074 }
1075 }
1076 {
1077 IkReal j5eval[3];
1078 new_r21=0;
1079 new_r20=0;
1080 new_r02=0;
1081 new_r12=0;
1082 IkReal x117=cj3*cj3;
1083 IkReal x118=new_r22*new_r22;
1084 IkReal x119=(new_r22*sj3);
1085 IkReal x120=((((-1.0)*x117))+(((-1.0)*x118))+((x117*x118)));
1086 j5eval[0]=x120;
1087 j5eval[1]=IKsign(x120);
1088 j5eval[2]=((IKabs((((new_r01*x119))+(((-1.0)*cj3*new_r00)))))+(IKabs((((new_r00*x119))+((cj3*new_r01))))));
1089 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1090 {
1091 {
1092 IkReal j5eval[1];
1093 new_r21=0;
1094 new_r20=0;
1095 new_r02=0;
1096 new_r12=0;
1097 j5eval[0]=new_r22;
1098 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1099 {
1100 {
1101 IkReal j5eval[1];
1102 new_r21=0;
1103 new_r20=0;
1104 new_r02=0;
1105 new_r12=0;
1106 j5eval[0]=cj3;
1107 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1108 {
1109 {
1110 IkReal evalcond[1];
1111 bool bgotonextstatement = true;
1112 do
1113 {
1114 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
1115 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1116 {
1117 bgotonextstatement=false;
1118 {
1119 IkReal j5array[1], cj5array[1], sj5array[1];
1120 bool j5valid[1]={false};
1121 _nj5 = 1;
1122 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
1123  continue;
1124 j5array[0]=IKatan2(((-1.0)*new_r11), new_r10);
1125 sj5array[0]=IKsin(j5array[0]);
1126 cj5array[0]=IKcos(j5array[0]);
1127 if( j5array[0] > IKPI )
1128 {
1129  j5array[0]-=IK2PI;
1130 }
1131 else if( j5array[0] < -IKPI )
1132 { j5array[0]+=IK2PI;
1133 }
1134 j5valid[0] = true;
1135 for(int ij5 = 0; ij5 < 1; ++ij5)
1136 {
1137 if( !j5valid[ij5] )
1138 {
1139  continue;
1140 }
1141 _ij5[0] = ij5; _ij5[1] = -1;
1142 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1143 {
1144 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1145 {
1146  j5valid[iij5]=false; _ij5[1] = iij5; break;
1147 }
1148 }
1149 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1150 {
1151 IkReal evalcond[4];
1152 IkReal x121=IKsin(j5);
1153 IkReal x122=IKcos(j5);
1154 evalcond[0]=(x121+new_r11);
1155 evalcond[1]=((-1.0)*x121);
1156 evalcond[2]=((-1.0)*x122);
1157 evalcond[3]=((((-1.0)*x122))+new_r10);
1158 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1159 {
1160 continue;
1161 }
1162 }
1163 
1164 {
1165 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1166 vinfos[0].jointtype = 1;
1167 vinfos[0].foffset = j0;
1168 vinfos[0].indices[0] = _ij0[0];
1169 vinfos[0].indices[1] = _ij0[1];
1170 vinfos[0].maxsolutions = _nj0;
1171 vinfos[1].jointtype = 1;
1172 vinfos[1].foffset = j1;
1173 vinfos[1].indices[0] = _ij1[0];
1174 vinfos[1].indices[1] = _ij1[1];
1175 vinfos[1].maxsolutions = _nj1;
1176 vinfos[2].jointtype = 1;
1177 vinfos[2].foffset = j2;
1178 vinfos[2].indices[0] = _ij2[0];
1179 vinfos[2].indices[1] = _ij2[1];
1180 vinfos[2].maxsolutions = _nj2;
1181 vinfos[3].jointtype = 1;
1182 vinfos[3].foffset = j3;
1183 vinfos[3].indices[0] = _ij3[0];
1184 vinfos[3].indices[1] = _ij3[1];
1185 vinfos[3].maxsolutions = _nj3;
1186 vinfos[4].jointtype = 1;
1187 vinfos[4].foffset = j4;
1188 vinfos[4].indices[0] = _ij4[0];
1189 vinfos[4].indices[1] = _ij4[1];
1190 vinfos[4].maxsolutions = _nj4;
1191 vinfos[5].jointtype = 1;
1192 vinfos[5].foffset = j5;
1193 vinfos[5].indices[0] = _ij5[0];
1194 vinfos[5].indices[1] = _ij5[1];
1195 vinfos[5].maxsolutions = _nj5;
1196 std::vector<int> vfree(0);
1197 solutions.AddSolution(vinfos,vfree);
1198 }
1199 }
1200 }
1201 
1202 }
1203 } while(0);
1204 if( bgotonextstatement )
1205 {
1206 bool bgotonextstatement = true;
1207 do
1208 {
1209 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
1210 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1211 {
1212 bgotonextstatement=false;
1213 {
1214 IkReal j5array[1], cj5array[1], sj5array[1];
1215 bool j5valid[1]={false};
1216 _nj5 = 1;
1217 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
1218  continue;
1219 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
1220 sj5array[0]=IKsin(j5array[0]);
1221 cj5array[0]=IKcos(j5array[0]);
1222 if( j5array[0] > IKPI )
1223 {
1224  j5array[0]-=IK2PI;
1225 }
1226 else if( j5array[0] < -IKPI )
1227 { j5array[0]+=IK2PI;
1228 }
1229 j5valid[0] = true;
1230 for(int ij5 = 0; ij5 < 1; ++ij5)
1231 {
1232 if( !j5valid[ij5] )
1233 {
1234  continue;
1235 }
1236 _ij5[0] = ij5; _ij5[1] = -1;
1237 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1238 {
1239 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1240 {
1241  j5valid[iij5]=false; _ij5[1] = iij5; break;
1242 }
1243 }
1244 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1245 {
1246 IkReal evalcond[4];
1247 IkReal x123=IKsin(j5);
1248 IkReal x124=IKcos(j5);
1249 evalcond[0]=((-1.0)*x123);
1250 evalcond[1]=((-1.0)*x124);
1251 evalcond[2]=(x123+(((-1.0)*new_r11)));
1252 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x124)));
1253 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1254 {
1255 continue;
1256 }
1257 }
1258 
1259 {
1260 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1261 vinfos[0].jointtype = 1;
1262 vinfos[0].foffset = j0;
1263 vinfos[0].indices[0] = _ij0[0];
1264 vinfos[0].indices[1] = _ij0[1];
1265 vinfos[0].maxsolutions = _nj0;
1266 vinfos[1].jointtype = 1;
1267 vinfos[1].foffset = j1;
1268 vinfos[1].indices[0] = _ij1[0];
1269 vinfos[1].indices[1] = _ij1[1];
1270 vinfos[1].maxsolutions = _nj1;
1271 vinfos[2].jointtype = 1;
1272 vinfos[2].foffset = j2;
1273 vinfos[2].indices[0] = _ij2[0];
1274 vinfos[2].indices[1] = _ij2[1];
1275 vinfos[2].maxsolutions = _nj2;
1276 vinfos[3].jointtype = 1;
1277 vinfos[3].foffset = j3;
1278 vinfos[3].indices[0] = _ij3[0];
1279 vinfos[3].indices[1] = _ij3[1];
1280 vinfos[3].maxsolutions = _nj3;
1281 vinfos[4].jointtype = 1;
1282 vinfos[4].foffset = j4;
1283 vinfos[4].indices[0] = _ij4[0];
1284 vinfos[4].indices[1] = _ij4[1];
1285 vinfos[4].maxsolutions = _nj4;
1286 vinfos[5].jointtype = 1;
1287 vinfos[5].foffset = j5;
1288 vinfos[5].indices[0] = _ij5[0];
1289 vinfos[5].indices[1] = _ij5[1];
1290 vinfos[5].maxsolutions = _nj5;
1291 std::vector<int> vfree(0);
1292 solutions.AddSolution(vinfos,vfree);
1293 }
1294 }
1295 }
1296 
1297 }
1298 } while(0);
1299 if( bgotonextstatement )
1300 {
1301 bool bgotonextstatement = true;
1302 do
1303 {
1304 IkReal x125=new_r22*new_r22;
1305 CheckValue<IkReal> x126=IKPowWithIntegerCheck(((-1.0)+x125),-1);
1306 if(!x126.valid){
1307 continue;
1308 }
1309 if(((x125*(x126.value))) < -0.00001)
1310 continue;
1311 IkReal gconst12=IKsqrt((x125*(x126.value)));
1312 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst12)))))), 6.28318530717959)));
1313 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1314 {
1315 bgotonextstatement=false;
1316 {
1317 IkReal j5eval[1];
1318 IkReal x127=new_r22*new_r22;
1319 new_r21=0;
1320 new_r20=0;
1321 new_r02=0;
1322 new_r12=0;
1323 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1324 continue;
1325 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))));
1326 cj3=gconst12;
1327 if( (gconst12) < -1-IKFAST_SINCOS_THRESH || (gconst12) > 1+IKFAST_SINCOS_THRESH )
1328  continue;
1329 j3=IKacos(gconst12);
1330 CheckValue<IkReal> x128=IKPowWithIntegerCheck(((-1.0)+x127),-1);
1331 if(!x128.valid){
1332 continue;
1333 }
1334 if(((x127*(x128.value))) < -0.00001)
1335 continue;
1336 IkReal gconst12=IKsqrt((x127*(x128.value)));
1337 j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
1338 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1339 {
1340 {
1341 IkReal j5array[1], cj5array[1], sj5array[1];
1342 bool j5valid[1]={false};
1343 _nj5 = 1;
1344 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1345 continue;
1346 CheckValue<IkReal> x129=IKPowWithIntegerCheck(gconst12,-1);
1347 if(!x129.valid){
1348 continue;
1349 }
1350 if( IKabs(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x129.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))))+IKsqr((new_r00*(x129.value)))-1) <= IKFAST_SINCOS_THRESH )
1351  continue;
1352 j5array[0]=IKatan2(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))), (new_r00*(x129.value)));
1353 sj5array[0]=IKsin(j5array[0]);
1354 cj5array[0]=IKcos(j5array[0]);
1355 if( j5array[0] > IKPI )
1356 {
1357  j5array[0]-=IK2PI;
1358 }
1359 else if( j5array[0] < -IKPI )
1360 { j5array[0]+=IK2PI;
1361 }
1362 j5valid[0] = true;
1363 for(int ij5 = 0; ij5 < 1; ++ij5)
1364 {
1365 if( !j5valid[ij5] )
1366 {
1367  continue;
1368 }
1369 _ij5[0] = ij5; _ij5[1] = -1;
1370 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1371 {
1372 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1373 {
1374  j5valid[iij5]=false; _ij5[1] = iij5; break;
1375 }
1376 }
1377 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1378 {
1379 IkReal evalcond[8];
1380 IkReal x130=IKsin(j5);
1381 IkReal x131=IKcos(j5);
1382 IkReal x132=((1.0)*x131);
1383 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1384 continue;
1385 IkReal x133=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))));
1386 evalcond[0]=((-1.0)*x130);
1387 evalcond[1]=((-1.0)*x131);
1388 evalcond[2]=(((gconst12*x130))+new_r01);
1389 evalcond[3]=((((-1.0)*gconst12*x132))+new_r00);
1390 evalcond[4]=(((x130*x133))+new_r11);
1391 evalcond[5]=((((-1.0)*x132*x133))+new_r10);
1392 evalcond[6]=(((gconst12*new_r01))+x130+((new_r11*x133)));
1393 evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x132))+((new_r10*x133)));
1394 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1395 {
1396 continue;
1397 }
1398 }
1399 
1400 {
1401 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1402 vinfos[0].jointtype = 1;
1403 vinfos[0].foffset = j0;
1404 vinfos[0].indices[0] = _ij0[0];
1405 vinfos[0].indices[1] = _ij0[1];
1406 vinfos[0].maxsolutions = _nj0;
1407 vinfos[1].jointtype = 1;
1408 vinfos[1].foffset = j1;
1409 vinfos[1].indices[0] = _ij1[0];
1410 vinfos[1].indices[1] = _ij1[1];
1411 vinfos[1].maxsolutions = _nj1;
1412 vinfos[2].jointtype = 1;
1413 vinfos[2].foffset = j2;
1414 vinfos[2].indices[0] = _ij2[0];
1415 vinfos[2].indices[1] = _ij2[1];
1416 vinfos[2].maxsolutions = _nj2;
1417 vinfos[3].jointtype = 1;
1418 vinfos[3].foffset = j3;
1419 vinfos[3].indices[0] = _ij3[0];
1420 vinfos[3].indices[1] = _ij3[1];
1421 vinfos[3].maxsolutions = _nj3;
1422 vinfos[4].jointtype = 1;
1423 vinfos[4].foffset = j4;
1424 vinfos[4].indices[0] = _ij4[0];
1425 vinfos[4].indices[1] = _ij4[1];
1426 vinfos[4].maxsolutions = _nj4;
1427 vinfos[5].jointtype = 1;
1428 vinfos[5].foffset = j5;
1429 vinfos[5].indices[0] = _ij5[0];
1430 vinfos[5].indices[1] = _ij5[1];
1431 vinfos[5].maxsolutions = _nj5;
1432 std::vector<int> vfree(0);
1433 solutions.AddSolution(vinfos,vfree);
1434 }
1435 }
1436 }
1437 
1438 } else
1439 {
1440 {
1441 IkReal j5array[1], cj5array[1], sj5array[1];
1442 bool j5valid[1]={false};
1443 _nj5 = 1;
1444 CheckValue<IkReal> x134 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
1445 if(!x134.valid){
1446 continue;
1447 }
1448 CheckValue<IkReal> x135=IKPowWithIntegerCheck(IKsign(gconst12),-1);
1449 if(!x135.valid){
1450 continue;
1451 }
1452 j5array[0]=((-1.5707963267949)+(x134.value)+(((1.5707963267949)*(x135.value))));
1453 sj5array[0]=IKsin(j5array[0]);
1454 cj5array[0]=IKcos(j5array[0]);
1455 if( j5array[0] > IKPI )
1456 {
1457  j5array[0]-=IK2PI;
1458 }
1459 else if( j5array[0] < -IKPI )
1460 { j5array[0]+=IK2PI;
1461 }
1462 j5valid[0] = true;
1463 for(int ij5 = 0; ij5 < 1; ++ij5)
1464 {
1465 if( !j5valid[ij5] )
1466 {
1467  continue;
1468 }
1469 _ij5[0] = ij5; _ij5[1] = -1;
1470 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1471 {
1472 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1473 {
1474  j5valid[iij5]=false; _ij5[1] = iij5; break;
1475 }
1476 }
1477 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1478 {
1479 IkReal evalcond[8];
1480 IkReal x136=IKsin(j5);
1481 IkReal x137=IKcos(j5);
1482 IkReal x138=((1.0)*x137);
1483 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1484 continue;
1485 IkReal x139=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))));
1486 evalcond[0]=((-1.0)*x136);
1487 evalcond[1]=((-1.0)*x137);
1488 evalcond[2]=(((gconst12*x136))+new_r01);
1489 evalcond[3]=((((-1.0)*gconst12*x138))+new_r00);
1490 evalcond[4]=(((x136*x139))+new_r11);
1491 evalcond[5]=((((-1.0)*x138*x139))+new_r10);
1492 evalcond[6]=(((gconst12*new_r01))+x136+((new_r11*x139)));
1493 evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x138))+((new_r10*x139)));
1494 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1495 {
1496 continue;
1497 }
1498 }
1499 
1500 {
1501 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1502 vinfos[0].jointtype = 1;
1503 vinfos[0].foffset = j0;
1504 vinfos[0].indices[0] = _ij0[0];
1505 vinfos[0].indices[1] = _ij0[1];
1506 vinfos[0].maxsolutions = _nj0;
1507 vinfos[1].jointtype = 1;
1508 vinfos[1].foffset = j1;
1509 vinfos[1].indices[0] = _ij1[0];
1510 vinfos[1].indices[1] = _ij1[1];
1511 vinfos[1].maxsolutions = _nj1;
1512 vinfos[2].jointtype = 1;
1513 vinfos[2].foffset = j2;
1514 vinfos[2].indices[0] = _ij2[0];
1515 vinfos[2].indices[1] = _ij2[1];
1516 vinfos[2].maxsolutions = _nj2;
1517 vinfos[3].jointtype = 1;
1518 vinfos[3].foffset = j3;
1519 vinfos[3].indices[0] = _ij3[0];
1520 vinfos[3].indices[1] = _ij3[1];
1521 vinfos[3].maxsolutions = _nj3;
1522 vinfos[4].jointtype = 1;
1523 vinfos[4].foffset = j4;
1524 vinfos[4].indices[0] = _ij4[0];
1525 vinfos[4].indices[1] = _ij4[1];
1526 vinfos[4].maxsolutions = _nj4;
1527 vinfos[5].jointtype = 1;
1528 vinfos[5].foffset = j5;
1529 vinfos[5].indices[0] = _ij5[0];
1530 vinfos[5].indices[1] = _ij5[1];
1531 vinfos[5].maxsolutions = _nj5;
1532 std::vector<int> vfree(0);
1533 solutions.AddSolution(vinfos,vfree);
1534 }
1535 }
1536 }
1537 
1538 }
1539 
1540 }
1541 
1542 }
1543 } while(0);
1544 if( bgotonextstatement )
1545 {
1546 bool bgotonextstatement = true;
1547 do
1548 {
1549 IkReal x140=new_r22*new_r22;
1550 CheckValue<IkReal> x141=IKPowWithIntegerCheck(((-1.0)+x140),-1);
1551 if(!x141.valid){
1552 continue;
1553 }
1554 if(((x140*(x141.value))) < -0.00001)
1555 continue;
1556 IkReal gconst12=IKsqrt((x140*(x141.value)));
1557 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst12)))))), 6.28318530717959)));
1558 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1559 {
1560 bgotonextstatement=false;
1561 {
1562 IkReal j5eval[1];
1563 IkReal x142=new_r22*new_r22;
1564 new_r21=0;
1565 new_r20=0;
1566 new_r02=0;
1567 new_r12=0;
1568 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1569 continue;
1570 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))))));
1571 cj3=gconst12;
1572 if( (gconst12) < -1-IKFAST_SINCOS_THRESH || (gconst12) > 1+IKFAST_SINCOS_THRESH )
1573  continue;
1574 j3=((-1.0)*(IKacos(gconst12)));
1575 CheckValue<IkReal> x143=IKPowWithIntegerCheck(((-1.0)+x142),-1);
1576 if(!x143.valid){
1577 continue;
1578 }
1579 if(((x142*(x143.value))) < -0.00001)
1580 continue;
1581 IkReal gconst12=IKsqrt((x142*(x143.value)));
1582 j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
1583 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1584 {
1585 {
1586 IkReal j5array[1], cj5array[1], sj5array[1];
1587 bool j5valid[1]={false};
1588 _nj5 = 1;
1589 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1590 continue;
1591 CheckValue<IkReal> x144=IKPowWithIntegerCheck(gconst12,-1);
1592 if(!x144.valid){
1593 continue;
1594 }
1595 if( IKabs((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x144.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))))+IKsqr((new_r00*(x144.value)))-1) <= IKFAST_SINCOS_THRESH )
1596  continue;
1597 j5array[0]=IKatan2((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))), (new_r00*(x144.value)));
1598 sj5array[0]=IKsin(j5array[0]);
1599 cj5array[0]=IKcos(j5array[0]);
1600 if( j5array[0] > IKPI )
1601 {
1602  j5array[0]-=IK2PI;
1603 }
1604 else if( j5array[0] < -IKPI )
1605 { j5array[0]+=IK2PI;
1606 }
1607 j5valid[0] = true;
1608 for(int ij5 = 0; ij5 < 1; ++ij5)
1609 {
1610 if( !j5valid[ij5] )
1611 {
1612  continue;
1613 }
1614 _ij5[0] = ij5; _ij5[1] = -1;
1615 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1616 {
1617 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1618 {
1619  j5valid[iij5]=false; _ij5[1] = iij5; break;
1620 }
1621 }
1622 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1623 {
1624 IkReal evalcond[8];
1625 IkReal x145=IKsin(j5);
1626 IkReal x146=IKcos(j5);
1627 IkReal x147=((1.0)*x146);
1628 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1629 continue;
1630 IkReal x148=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))));
1631 IkReal x149=((1.0)*x148);
1632 evalcond[0]=((-1.0)*x145);
1633 evalcond[1]=((-1.0)*x146);
1634 evalcond[2]=(((gconst12*x145))+new_r01);
1635 evalcond[3]=((((-1.0)*gconst12*x147))+new_r00);
1636 evalcond[4]=(((x146*x148))+new_r10);
1637 evalcond[5]=((((-1.0)*x145*x149))+new_r11);
1638 evalcond[6]=(((gconst12*new_r01))+(((-1.0)*new_r11*x149))+x145);
1639 evalcond[7]=((((-1.0)*x147))+((gconst12*new_r00))+(((-1.0)*new_r10*x149)));
1640 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1641 {
1642 continue;
1643 }
1644 }
1645 
1646 {
1647 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1648 vinfos[0].jointtype = 1;
1649 vinfos[0].foffset = j0;
1650 vinfos[0].indices[0] = _ij0[0];
1651 vinfos[0].indices[1] = _ij0[1];
1652 vinfos[0].maxsolutions = _nj0;
1653 vinfos[1].jointtype = 1;
1654 vinfos[1].foffset = j1;
1655 vinfos[1].indices[0] = _ij1[0];
1656 vinfos[1].indices[1] = _ij1[1];
1657 vinfos[1].maxsolutions = _nj1;
1658 vinfos[2].jointtype = 1;
1659 vinfos[2].foffset = j2;
1660 vinfos[2].indices[0] = _ij2[0];
1661 vinfos[2].indices[1] = _ij2[1];
1662 vinfos[2].maxsolutions = _nj2;
1663 vinfos[3].jointtype = 1;
1664 vinfos[3].foffset = j3;
1665 vinfos[3].indices[0] = _ij3[0];
1666 vinfos[3].indices[1] = _ij3[1];
1667 vinfos[3].maxsolutions = _nj3;
1668 vinfos[4].jointtype = 1;
1669 vinfos[4].foffset = j4;
1670 vinfos[4].indices[0] = _ij4[0];
1671 vinfos[4].indices[1] = _ij4[1];
1672 vinfos[4].maxsolutions = _nj4;
1673 vinfos[5].jointtype = 1;
1674 vinfos[5].foffset = j5;
1675 vinfos[5].indices[0] = _ij5[0];
1676 vinfos[5].indices[1] = _ij5[1];
1677 vinfos[5].maxsolutions = _nj5;
1678 std::vector<int> vfree(0);
1679 solutions.AddSolution(vinfos,vfree);
1680 }
1681 }
1682 }
1683 
1684 } else
1685 {
1686 {
1687 IkReal j5array[1], cj5array[1], sj5array[1];
1688 bool j5valid[1]={false};
1689 _nj5 = 1;
1690 CheckValue<IkReal> x150 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
1691 if(!x150.valid){
1692 continue;
1693 }
1694 CheckValue<IkReal> x151=IKPowWithIntegerCheck(IKsign(gconst12),-1);
1695 if(!x151.valid){
1696 continue;
1697 }
1698 j5array[0]=((-1.5707963267949)+(x150.value)+(((1.5707963267949)*(x151.value))));
1699 sj5array[0]=IKsin(j5array[0]);
1700 cj5array[0]=IKcos(j5array[0]);
1701 if( j5array[0] > IKPI )
1702 {
1703  j5array[0]-=IK2PI;
1704 }
1705 else if( j5array[0] < -IKPI )
1706 { j5array[0]+=IK2PI;
1707 }
1708 j5valid[0] = true;
1709 for(int ij5 = 0; ij5 < 1; ++ij5)
1710 {
1711 if( !j5valid[ij5] )
1712 {
1713  continue;
1714 }
1715 _ij5[0] = ij5; _ij5[1] = -1;
1716 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1717 {
1718 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1719 {
1720  j5valid[iij5]=false; _ij5[1] = iij5; break;
1721 }
1722 }
1723 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1724 {
1725 IkReal evalcond[8];
1726 IkReal x152=IKsin(j5);
1727 IkReal x153=IKcos(j5);
1728 IkReal x154=((1.0)*x153);
1729 if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001)
1730 continue;
1731 IkReal x155=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12)))));
1732 IkReal x156=((1.0)*x155);
1733 evalcond[0]=((-1.0)*x152);
1734 evalcond[1]=((-1.0)*x153);
1735 evalcond[2]=(((gconst12*x152))+new_r01);
1736 evalcond[3]=(new_r00+(((-1.0)*gconst12*x154)));
1737 evalcond[4]=(((x153*x155))+new_r10);
1738 evalcond[5]=((((-1.0)*x152*x156))+new_r11);
1739 evalcond[6]=(((gconst12*new_r01))+x152+(((-1.0)*new_r11*x156)));
1740 evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x154))+(((-1.0)*new_r10*x156)));
1741 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1742 {
1743 continue;
1744 }
1745 }
1746 
1747 {
1748 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1749 vinfos[0].jointtype = 1;
1750 vinfos[0].foffset = j0;
1751 vinfos[0].indices[0] = _ij0[0];
1752 vinfos[0].indices[1] = _ij0[1];
1753 vinfos[0].maxsolutions = _nj0;
1754 vinfos[1].jointtype = 1;
1755 vinfos[1].foffset = j1;
1756 vinfos[1].indices[0] = _ij1[0];
1757 vinfos[1].indices[1] = _ij1[1];
1758 vinfos[1].maxsolutions = _nj1;
1759 vinfos[2].jointtype = 1;
1760 vinfos[2].foffset = j2;
1761 vinfos[2].indices[0] = _ij2[0];
1762 vinfos[2].indices[1] = _ij2[1];
1763 vinfos[2].maxsolutions = _nj2;
1764 vinfos[3].jointtype = 1;
1765 vinfos[3].foffset = j3;
1766 vinfos[3].indices[0] = _ij3[0];
1767 vinfos[3].indices[1] = _ij3[1];
1768 vinfos[3].maxsolutions = _nj3;
1769 vinfos[4].jointtype = 1;
1770 vinfos[4].foffset = j4;
1771 vinfos[4].indices[0] = _ij4[0];
1772 vinfos[4].indices[1] = _ij4[1];
1773 vinfos[4].maxsolutions = _nj4;
1774 vinfos[5].jointtype = 1;
1775 vinfos[5].foffset = j5;
1776 vinfos[5].indices[0] = _ij5[0];
1777 vinfos[5].indices[1] = _ij5[1];
1778 vinfos[5].maxsolutions = _nj5;
1779 std::vector<int> vfree(0);
1780 solutions.AddSolution(vinfos,vfree);
1781 }
1782 }
1783 }
1784 
1785 }
1786 
1787 }
1788 
1789 }
1790 } while(0);
1791 if( bgotonextstatement )
1792 {
1793 bool bgotonextstatement = true;
1794 do
1795 {
1796 IkReal x157=new_r22*new_r22;
1797 CheckValue<IkReal> x158=IKPowWithIntegerCheck(((-1.0)+x157),-1);
1798 if(!x158.valid){
1799 continue;
1800 }
1801 if(((x157*(x158.value))) < -0.00001)
1802 continue;
1803 IkReal gconst13=((-1.0)*(IKsqrt((x157*(x158.value)))));
1804 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst13)))))), 6.28318530717959)));
1805 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1806 {
1807 bgotonextstatement=false;
1808 {
1809 IkReal j5eval[1];
1810 IkReal x159=new_r22*new_r22;
1811 new_r21=0;
1812 new_r20=0;
1813 new_r02=0;
1814 new_r12=0;
1815 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
1816 continue;
1817 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))));
1818 cj3=gconst13;
1819 if( (gconst13) < -1-IKFAST_SINCOS_THRESH || (gconst13) > 1+IKFAST_SINCOS_THRESH )
1820  continue;
1821 j3=IKacos(gconst13);
1822 CheckValue<IkReal> x160=IKPowWithIntegerCheck(((-1.0)+x159),-1);
1823 if(!x160.valid){
1824 continue;
1825 }
1826 if(((x159*(x160.value))) < -0.00001)
1827 continue;
1828 IkReal gconst13=((-1.0)*(IKsqrt((x159*(x160.value)))));
1829 j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
1830 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1831 {
1832 {
1833 IkReal j5array[1], cj5array[1], sj5array[1];
1834 bool j5valid[1]={false};
1835 _nj5 = 1;
1836 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
1837 continue;
1838 CheckValue<IkReal> x161=IKPowWithIntegerCheck(gconst13,-1);
1839 if(!x161.valid){
1840 continue;
1841 }
1842 if( IKabs(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x161.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))))+IKsqr((new_r00*(x161.value)))-1) <= IKFAST_SINCOS_THRESH )
1843  continue;
1844 j5array[0]=IKatan2(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))), (new_r00*(x161.value)));
1845 sj5array[0]=IKsin(j5array[0]);
1846 cj5array[0]=IKcos(j5array[0]);
1847 if( j5array[0] > IKPI )
1848 {
1849  j5array[0]-=IK2PI;
1850 }
1851 else if( j5array[0] < -IKPI )
1852 { j5array[0]+=IK2PI;
1853 }
1854 j5valid[0] = true;
1855 for(int ij5 = 0; ij5 < 1; ++ij5)
1856 {
1857 if( !j5valid[ij5] )
1858 {
1859  continue;
1860 }
1861 _ij5[0] = ij5; _ij5[1] = -1;
1862 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1863 {
1864 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1865 {
1866  j5valid[iij5]=false; _ij5[1] = iij5; break;
1867 }
1868 }
1869 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1870 {
1871 IkReal evalcond[8];
1872 IkReal x162=IKsin(j5);
1873 IkReal x163=IKcos(j5);
1874 IkReal x164=((1.0)*x163);
1875 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
1876 continue;
1877 IkReal x165=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))));
1878 evalcond[0]=((-1.0)*x162);
1879 evalcond[1]=((-1.0)*x163);
1880 evalcond[2]=(new_r01+((gconst13*x162)));
1881 evalcond[3]=(new_r00+(((-1.0)*gconst13*x164)));
1882 evalcond[4]=(((x162*x165))+new_r11);
1883 evalcond[5]=(new_r10+(((-1.0)*x164*x165)));
1884 evalcond[6]=(((new_r11*x165))+x162+((gconst13*new_r01)));
1885 evalcond[7]=((((-1.0)*x164))+((new_r10*x165))+((gconst13*new_r00)));
1886 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1887 {
1888 continue;
1889 }
1890 }
1891 
1892 {
1893 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1894 vinfos[0].jointtype = 1;
1895 vinfos[0].foffset = j0;
1896 vinfos[0].indices[0] = _ij0[0];
1897 vinfos[0].indices[1] = _ij0[1];
1898 vinfos[0].maxsolutions = _nj0;
1899 vinfos[1].jointtype = 1;
1900 vinfos[1].foffset = j1;
1901 vinfos[1].indices[0] = _ij1[0];
1902 vinfos[1].indices[1] = _ij1[1];
1903 vinfos[1].maxsolutions = _nj1;
1904 vinfos[2].jointtype = 1;
1905 vinfos[2].foffset = j2;
1906 vinfos[2].indices[0] = _ij2[0];
1907 vinfos[2].indices[1] = _ij2[1];
1908 vinfos[2].maxsolutions = _nj2;
1909 vinfos[3].jointtype = 1;
1910 vinfos[3].foffset = j3;
1911 vinfos[3].indices[0] = _ij3[0];
1912 vinfos[3].indices[1] = _ij3[1];
1913 vinfos[3].maxsolutions = _nj3;
1914 vinfos[4].jointtype = 1;
1915 vinfos[4].foffset = j4;
1916 vinfos[4].indices[0] = _ij4[0];
1917 vinfos[4].indices[1] = _ij4[1];
1918 vinfos[4].maxsolutions = _nj4;
1919 vinfos[5].jointtype = 1;
1920 vinfos[5].foffset = j5;
1921 vinfos[5].indices[0] = _ij5[0];
1922 vinfos[5].indices[1] = _ij5[1];
1923 vinfos[5].maxsolutions = _nj5;
1924 std::vector<int> vfree(0);
1925 solutions.AddSolution(vinfos,vfree);
1926 }
1927 }
1928 }
1929 
1930 } else
1931 {
1932 {
1933 IkReal j5array[1], cj5array[1], sj5array[1];
1934 bool j5valid[1]={false};
1935 _nj5 = 1;
1936 CheckValue<IkReal> x166 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
1937 if(!x166.valid){
1938 continue;
1939 }
1940 CheckValue<IkReal> x167=IKPowWithIntegerCheck(IKsign(gconst13),-1);
1941 if(!x167.valid){
1942 continue;
1943 }
1944 j5array[0]=((-1.5707963267949)+(x166.value)+(((1.5707963267949)*(x167.value))));
1945 sj5array[0]=IKsin(j5array[0]);
1946 cj5array[0]=IKcos(j5array[0]);
1947 if( j5array[0] > IKPI )
1948 {
1949  j5array[0]-=IK2PI;
1950 }
1951 else if( j5array[0] < -IKPI )
1952 { j5array[0]+=IK2PI;
1953 }
1954 j5valid[0] = true;
1955 for(int ij5 = 0; ij5 < 1; ++ij5)
1956 {
1957 if( !j5valid[ij5] )
1958 {
1959  continue;
1960 }
1961 _ij5[0] = ij5; _ij5[1] = -1;
1962 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1963 {
1964 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1965 {
1966  j5valid[iij5]=false; _ij5[1] = iij5; break;
1967 }
1968 }
1969 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1970 {
1971 IkReal evalcond[8];
1972 IkReal x168=IKsin(j5);
1973 IkReal x169=IKcos(j5);
1974 IkReal x170=((1.0)*x169);
1975 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
1976 continue;
1977 IkReal x171=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))));
1978 evalcond[0]=((-1.0)*x168);
1979 evalcond[1]=((-1.0)*x169);
1980 evalcond[2]=(new_r01+((gconst13*x168)));
1981 evalcond[3]=((((-1.0)*gconst13*x170))+new_r00);
1982 evalcond[4]=(((x168*x171))+new_r11);
1983 evalcond[5]=((((-1.0)*x170*x171))+new_r10);
1984 evalcond[6]=(((new_r11*x171))+x168+((gconst13*new_r01)));
1985 evalcond[7]=(((new_r10*x171))+(((-1.0)*x170))+((gconst13*new_r00)));
1986 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1987 {
1988 continue;
1989 }
1990 }
1991 
1992 {
1993 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1994 vinfos[0].jointtype = 1;
1995 vinfos[0].foffset = j0;
1996 vinfos[0].indices[0] = _ij0[0];
1997 vinfos[0].indices[1] = _ij0[1];
1998 vinfos[0].maxsolutions = _nj0;
1999 vinfos[1].jointtype = 1;
2000 vinfos[1].foffset = j1;
2001 vinfos[1].indices[0] = _ij1[0];
2002 vinfos[1].indices[1] = _ij1[1];
2003 vinfos[1].maxsolutions = _nj1;
2004 vinfos[2].jointtype = 1;
2005 vinfos[2].foffset = j2;
2006 vinfos[2].indices[0] = _ij2[0];
2007 vinfos[2].indices[1] = _ij2[1];
2008 vinfos[2].maxsolutions = _nj2;
2009 vinfos[3].jointtype = 1;
2010 vinfos[3].foffset = j3;
2011 vinfos[3].indices[0] = _ij3[0];
2012 vinfos[3].indices[1] = _ij3[1];
2013 vinfos[3].maxsolutions = _nj3;
2014 vinfos[4].jointtype = 1;
2015 vinfos[4].foffset = j4;
2016 vinfos[4].indices[0] = _ij4[0];
2017 vinfos[4].indices[1] = _ij4[1];
2018 vinfos[4].maxsolutions = _nj4;
2019 vinfos[5].jointtype = 1;
2020 vinfos[5].foffset = j5;
2021 vinfos[5].indices[0] = _ij5[0];
2022 vinfos[5].indices[1] = _ij5[1];
2023 vinfos[5].maxsolutions = _nj5;
2024 std::vector<int> vfree(0);
2025 solutions.AddSolution(vinfos,vfree);
2026 }
2027 }
2028 }
2029 
2030 }
2031 
2032 }
2033 
2034 }
2035 } while(0);
2036 if( bgotonextstatement )
2037 {
2038 bool bgotonextstatement = true;
2039 do
2040 {
2041 IkReal x172=new_r22*new_r22;
2042 CheckValue<IkReal> x173=IKPowWithIntegerCheck(((-1.0)+x172),-1);
2043 if(!x173.valid){
2044 continue;
2045 }
2046 if(((x172*(x173.value))) < -0.00001)
2047 continue;
2048 IkReal gconst13=((-1.0)*(IKsqrt((x172*(x173.value)))));
2049 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst13)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
2050 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2051 {
2052 bgotonextstatement=false;
2053 {
2054 IkReal j5eval[1];
2055 IkReal x174=new_r22*new_r22;
2056 new_r21=0;
2057 new_r20=0;
2058 new_r02=0;
2059 new_r12=0;
2060 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
2061 continue;
2062 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))))));
2063 cj3=gconst13;
2064 if( (gconst13) < -1-IKFAST_SINCOS_THRESH || (gconst13) > 1+IKFAST_SINCOS_THRESH )
2065  continue;
2066 j3=((-1.0)*(IKacos(gconst13)));
2067 CheckValue<IkReal> x175=IKPowWithIntegerCheck(((-1.0)+x174),-1);
2068 if(!x175.valid){
2069 continue;
2070 }
2071 if(((x174*(x175.value))) < -0.00001)
2072 continue;
2073 IkReal gconst13=((-1.0)*(IKsqrt((x174*(x175.value)))));
2074 j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
2075 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2076 {
2077 {
2078 IkReal j5array[1], cj5array[1], sj5array[1];
2079 bool j5valid[1]={false};
2080 _nj5 = 1;
2081 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
2082 continue;
2083 CheckValue<IkReal> x176=IKPowWithIntegerCheck(gconst13,-1);
2084 if(!x176.valid){
2085 continue;
2086 }
2087 if( IKabs(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x176.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))))+IKsqr((new_r00*(x176.value)))-1) <= IKFAST_SINCOS_THRESH )
2088  continue;
2089 j5array[0]=IKatan2(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))), (new_r00*(x176.value)));
2090 sj5array[0]=IKsin(j5array[0]);
2091 cj5array[0]=IKcos(j5array[0]);
2092 if( j5array[0] > IKPI )
2093 {
2094  j5array[0]-=IK2PI;
2095 }
2096 else if( j5array[0] < -IKPI )
2097 { j5array[0]+=IK2PI;
2098 }
2099 j5valid[0] = true;
2100 for(int ij5 = 0; ij5 < 1; ++ij5)
2101 {
2102 if( !j5valid[ij5] )
2103 {
2104  continue;
2105 }
2106 _ij5[0] = ij5; _ij5[1] = -1;
2107 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2108 {
2109 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2110 {
2111  j5valid[iij5]=false; _ij5[1] = iij5; break;
2112 }
2113 }
2114 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2115 {
2116 IkReal evalcond[8];
2117 IkReal x177=IKsin(j5);
2118 IkReal x178=IKcos(j5);
2119 IkReal x179=((1.0)*x178);
2120 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
2121 continue;
2122 IkReal x180=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))));
2123 IkReal x181=((1.0)*x180);
2124 evalcond[0]=((-1.0)*x177);
2125 evalcond[1]=((-1.0)*x178);
2126 evalcond[2]=(new_r01+((gconst13*x177)));
2127 evalcond[3]=((((-1.0)*gconst13*x179))+new_r00);
2128 evalcond[4]=(((x178*x180))+new_r10);
2129 evalcond[5]=((((-1.0)*x177*x181))+new_r11);
2130 evalcond[6]=(x177+(((-1.0)*new_r11*x181))+((gconst13*new_r01)));
2131 evalcond[7]=((((-1.0)*x179))+((gconst13*new_r00))+(((-1.0)*new_r10*x181)));
2132 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2133 {
2134 continue;
2135 }
2136 }
2137 
2138 {
2139 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2140 vinfos[0].jointtype = 1;
2141 vinfos[0].foffset = j0;
2142 vinfos[0].indices[0] = _ij0[0];
2143 vinfos[0].indices[1] = _ij0[1];
2144 vinfos[0].maxsolutions = _nj0;
2145 vinfos[1].jointtype = 1;
2146 vinfos[1].foffset = j1;
2147 vinfos[1].indices[0] = _ij1[0];
2148 vinfos[1].indices[1] = _ij1[1];
2149 vinfos[1].maxsolutions = _nj1;
2150 vinfos[2].jointtype = 1;
2151 vinfos[2].foffset = j2;
2152 vinfos[2].indices[0] = _ij2[0];
2153 vinfos[2].indices[1] = _ij2[1];
2154 vinfos[2].maxsolutions = _nj2;
2155 vinfos[3].jointtype = 1;
2156 vinfos[3].foffset = j3;
2157 vinfos[3].indices[0] = _ij3[0];
2158 vinfos[3].indices[1] = _ij3[1];
2159 vinfos[3].maxsolutions = _nj3;
2160 vinfos[4].jointtype = 1;
2161 vinfos[4].foffset = j4;
2162 vinfos[4].indices[0] = _ij4[0];
2163 vinfos[4].indices[1] = _ij4[1];
2164 vinfos[4].maxsolutions = _nj4;
2165 vinfos[5].jointtype = 1;
2166 vinfos[5].foffset = j5;
2167 vinfos[5].indices[0] = _ij5[0];
2168 vinfos[5].indices[1] = _ij5[1];
2169 vinfos[5].maxsolutions = _nj5;
2170 std::vector<int> vfree(0);
2171 solutions.AddSolution(vinfos,vfree);
2172 }
2173 }
2174 }
2175 
2176 } else
2177 {
2178 {
2179 IkReal j5array[1], cj5array[1], sj5array[1];
2180 bool j5valid[1]={false};
2181 _nj5 = 1;
2182 CheckValue<IkReal> x182 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
2183 if(!x182.valid){
2184 continue;
2185 }
2186 CheckValue<IkReal> x183=IKPowWithIntegerCheck(IKsign(gconst13),-1);
2187 if(!x183.valid){
2188 continue;
2189 }
2190 j5array[0]=((-1.5707963267949)+(x182.value)+(((1.5707963267949)*(x183.value))));
2191 sj5array[0]=IKsin(j5array[0]);
2192 cj5array[0]=IKcos(j5array[0]);
2193 if( j5array[0] > IKPI )
2194 {
2195  j5array[0]-=IK2PI;
2196 }
2197 else if( j5array[0] < -IKPI )
2198 { j5array[0]+=IK2PI;
2199 }
2200 j5valid[0] = true;
2201 for(int ij5 = 0; ij5 < 1; ++ij5)
2202 {
2203 if( !j5valid[ij5] )
2204 {
2205  continue;
2206 }
2207 _ij5[0] = ij5; _ij5[1] = -1;
2208 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2209 {
2210 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2211 {
2212  j5valid[iij5]=false; _ij5[1] = iij5; break;
2213 }
2214 }
2215 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2216 {
2217 IkReal evalcond[8];
2218 IkReal x184=IKsin(j5);
2219 IkReal x185=IKcos(j5);
2220 IkReal x186=((1.0)*x185);
2221 if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001)
2222 continue;
2223 IkReal x187=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))));
2224 IkReal x188=((1.0)*x187);
2225 evalcond[0]=((-1.0)*x184);
2226 evalcond[1]=((-1.0)*x185);
2227 evalcond[2]=(((gconst13*x184))+new_r01);
2228 evalcond[3]=((((-1.0)*gconst13*x186))+new_r00);
2229 evalcond[4]=(((x185*x187))+new_r10);
2230 evalcond[5]=((((-1.0)*x184*x188))+new_r11);
2231 evalcond[6]=(x184+(((-1.0)*new_r11*x188))+((gconst13*new_r01)));
2232 evalcond[7]=((((-1.0)*x186))+((gconst13*new_r00))+(((-1.0)*new_r10*x188)));
2233 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2234 {
2235 continue;
2236 }
2237 }
2238 
2239 {
2240 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2241 vinfos[0].jointtype = 1;
2242 vinfos[0].foffset = j0;
2243 vinfos[0].indices[0] = _ij0[0];
2244 vinfos[0].indices[1] = _ij0[1];
2245 vinfos[0].maxsolutions = _nj0;
2246 vinfos[1].jointtype = 1;
2247 vinfos[1].foffset = j1;
2248 vinfos[1].indices[0] = _ij1[0];
2249 vinfos[1].indices[1] = _ij1[1];
2250 vinfos[1].maxsolutions = _nj1;
2251 vinfos[2].jointtype = 1;
2252 vinfos[2].foffset = j2;
2253 vinfos[2].indices[0] = _ij2[0];
2254 vinfos[2].indices[1] = _ij2[1];
2255 vinfos[2].maxsolutions = _nj2;
2256 vinfos[3].jointtype = 1;
2257 vinfos[3].foffset = j3;
2258 vinfos[3].indices[0] = _ij3[0];
2259 vinfos[3].indices[1] = _ij3[1];
2260 vinfos[3].maxsolutions = _nj3;
2261 vinfos[4].jointtype = 1;
2262 vinfos[4].foffset = j4;
2263 vinfos[4].indices[0] = _ij4[0];
2264 vinfos[4].indices[1] = _ij4[1];
2265 vinfos[4].maxsolutions = _nj4;
2266 vinfos[5].jointtype = 1;
2267 vinfos[5].foffset = j5;
2268 vinfos[5].indices[0] = _ij5[0];
2269 vinfos[5].indices[1] = _ij5[1];
2270 vinfos[5].maxsolutions = _nj5;
2271 std::vector<int> vfree(0);
2272 solutions.AddSolution(vinfos,vfree);
2273 }
2274 }
2275 }
2276 
2277 }
2278 
2279 }
2280 
2281 }
2282 } while(0);
2283 if( bgotonextstatement )
2284 {
2285 bool bgotonextstatement = true;
2286 do
2287 {
2288 if( 1 )
2289 {
2290 bgotonextstatement=false;
2291 continue; // branch miss [j5]
2292 
2293 }
2294 } while(0);
2295 if( bgotonextstatement )
2296 {
2297 }
2298 }
2299 }
2300 }
2301 }
2302 }
2303 }
2304 }
2305 
2306 } else
2307 {
2308 {
2309 IkReal j5array[1], cj5array[1], sj5array[1];
2310 bool j5valid[1]={false};
2311 _nj5 = 1;
2312 IkReal x189=((1.0)*new_r22);
2313 IkReal x190=(cj3*new_r01);
2314 CheckValue<IkReal> x191=IKPowWithIntegerCheck(cj3,-1);
2315 if(!x191.valid){
2316 continue;
2317 }
2318 if( IKabs(((((-1.0)*x190))+(((-1.0)*new_r11*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x190))+(((-1.0)*new_r11*sj3))))+IKsqr(((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190))))))-1) <= IKFAST_SINCOS_THRESH )
2319  continue;
2320 j5array[0]=IKatan2(((((-1.0)*x190))+(((-1.0)*new_r11*sj3))), ((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190))))));
2321 sj5array[0]=IKsin(j5array[0]);
2322 cj5array[0]=IKcos(j5array[0]);
2323 if( j5array[0] > IKPI )
2324 {
2325  j5array[0]-=IK2PI;
2326 }
2327 else if( j5array[0] < -IKPI )
2328 { j5array[0]+=IK2PI;
2329 }
2330 j5valid[0] = true;
2331 for(int ij5 = 0; ij5 < 1; ++ij5)
2332 {
2333 if( !j5valid[ij5] )
2334 {
2335  continue;
2336 }
2337 _ij5[0] = ij5; _ij5[1] = -1;
2338 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2339 {
2340 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2341 {
2342  j5valid[iij5]=false; _ij5[1] = iij5; break;
2343 }
2344 }
2345 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2346 {
2347 IkReal evalcond[10];
2348 IkReal x192=IKsin(j5);
2349 IkReal x193=IKcos(j5);
2350 IkReal x194=((1.0)*cj3);
2351 IkReal x195=((1.0)*sj3);
2352 IkReal x196=(cj3*new_r10);
2353 IkReal x197=(cj3*new_r11);
2354 IkReal x198=((1.0)*x193);
2355 IkReal x199=(new_r22*x193);
2356 IkReal x200=(sj3*x192);
2357 IkReal x201=(new_r22*x192);
2358 evalcond[0]=(((new_r11*sj3))+x192+((cj3*new_r01)));
2359 evalcond[1]=(((new_r10*sj3))+(((-1.0)*x198))+((cj3*new_r00)));
2360 evalcond[2]=(((sj3*x199))+((cj3*x192))+new_r01);
2361 evalcond[3]=(((new_r22*x200))+new_r00+(((-1.0)*x193*x194)));
2362 evalcond[4]=((((-1.0)*x194*x199))+x200+new_r11);
2363 evalcond[5]=(x196+(((-1.0)*new_r00*x195))+(((-1.0)*x201)));
2364 evalcond[6]=((((-1.0)*new_r22*x198))+x197+(((-1.0)*new_r01*x195)));
2365 evalcond[7]=((((-1.0)*x194*x201))+new_r10+(((-1.0)*x193*x195)));
2366 evalcond[8]=((((-1.0)*new_r00*new_r22*x195))+(((-1.0)*x192))+((new_r22*x196)));
2367 evalcond[9]=((((-1.0)*x198))+((new_r22*x197))+(((-1.0)*new_r01*new_r22*x195)));
2368 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2369 {
2370 continue;
2371 }
2372 }
2373 
2374 {
2375 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2376 vinfos[0].jointtype = 1;
2377 vinfos[0].foffset = j0;
2378 vinfos[0].indices[0] = _ij0[0];
2379 vinfos[0].indices[1] = _ij0[1];
2380 vinfos[0].maxsolutions = _nj0;
2381 vinfos[1].jointtype = 1;
2382 vinfos[1].foffset = j1;
2383 vinfos[1].indices[0] = _ij1[0];
2384 vinfos[1].indices[1] = _ij1[1];
2385 vinfos[1].maxsolutions = _nj1;
2386 vinfos[2].jointtype = 1;
2387 vinfos[2].foffset = j2;
2388 vinfos[2].indices[0] = _ij2[0];
2389 vinfos[2].indices[1] = _ij2[1];
2390 vinfos[2].maxsolutions = _nj2;
2391 vinfos[3].jointtype = 1;
2392 vinfos[3].foffset = j3;
2393 vinfos[3].indices[0] = _ij3[0];
2394 vinfos[3].indices[1] = _ij3[1];
2395 vinfos[3].maxsolutions = _nj3;
2396 vinfos[4].jointtype = 1;
2397 vinfos[4].foffset = j4;
2398 vinfos[4].indices[0] = _ij4[0];
2399 vinfos[4].indices[1] = _ij4[1];
2400 vinfos[4].maxsolutions = _nj4;
2401 vinfos[5].jointtype = 1;
2402 vinfos[5].foffset = j5;
2403 vinfos[5].indices[0] = _ij5[0];
2404 vinfos[5].indices[1] = _ij5[1];
2405 vinfos[5].maxsolutions = _nj5;
2406 std::vector<int> vfree(0);
2407 solutions.AddSolution(vinfos,vfree);
2408 }
2409 }
2410 }
2411 
2412 }
2413 
2414 }
2415 
2416 } else
2417 {
2418 {
2419 IkReal j5array[1], cj5array[1], sj5array[1];
2420 bool j5valid[1]={false};
2421 _nj5 = 1;
2422 IkReal x202=((1.0)*new_r01);
2423 CheckValue<IkReal> x203=IKPowWithIntegerCheck(new_r22,-1);
2424 if(!x203.valid){
2425 continue;
2426 }
2427 if( IKabs(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202))))+IKsqr(((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202))))))-1) <= IKFAST_SINCOS_THRESH )
2428  continue;
2429 j5array[0]=IKatan2(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202))), ((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202))))));
2430 sj5array[0]=IKsin(j5array[0]);
2431 cj5array[0]=IKcos(j5array[0]);
2432 if( j5array[0] > IKPI )
2433 {
2434  j5array[0]-=IK2PI;
2435 }
2436 else if( j5array[0] < -IKPI )
2437 { j5array[0]+=IK2PI;
2438 }
2439 j5valid[0] = true;
2440 for(int ij5 = 0; ij5 < 1; ++ij5)
2441 {
2442 if( !j5valid[ij5] )
2443 {
2444  continue;
2445 }
2446 _ij5[0] = ij5; _ij5[1] = -1;
2447 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2448 {
2449 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2450 {
2451  j5valid[iij5]=false; _ij5[1] = iij5; break;
2452 }
2453 }
2454 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2455 {
2456 IkReal evalcond[10];
2457 IkReal x204=IKsin(j5);
2458 IkReal x205=IKcos(j5);
2459 IkReal x206=((1.0)*cj3);
2460 IkReal x207=((1.0)*sj3);
2461 IkReal x208=(cj3*new_r10);
2462 IkReal x209=(cj3*new_r11);
2463 IkReal x210=((1.0)*x205);
2464 IkReal x211=(new_r22*x205);
2465 IkReal x212=(sj3*x204);
2466 IkReal x213=(new_r22*x204);
2467 evalcond[0]=(((new_r11*sj3))+x204+((cj3*new_r01)));
2468 evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x210)));
2469 evalcond[2]=(((cj3*x204))+((sj3*x211))+new_r01);
2470 evalcond[3]=(((new_r22*x212))+(((-1.0)*x205*x206))+new_r00);
2471 evalcond[4]=((((-1.0)*x206*x211))+x212+new_r11);
2472 evalcond[5]=((((-1.0)*new_r00*x207))+x208+(((-1.0)*x213)));
2473 evalcond[6]=(x209+(((-1.0)*new_r22*x210))+(((-1.0)*new_r01*x207)));
2474 evalcond[7]=((((-1.0)*x206*x213))+(((-1.0)*x205*x207))+new_r10);
2475 evalcond[8]=(((new_r22*x208))+(((-1.0)*new_r00*new_r22*x207))+(((-1.0)*x204)));
2476 evalcond[9]=((((-1.0)*new_r01*new_r22*x207))+((new_r22*x209))+(((-1.0)*x210)));
2477 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2478 {
2479 continue;
2480 }
2481 }
2482 
2483 {
2484 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2485 vinfos[0].jointtype = 1;
2486 vinfos[0].foffset = j0;
2487 vinfos[0].indices[0] = _ij0[0];
2488 vinfos[0].indices[1] = _ij0[1];
2489 vinfos[0].maxsolutions = _nj0;
2490 vinfos[1].jointtype = 1;
2491 vinfos[1].foffset = j1;
2492 vinfos[1].indices[0] = _ij1[0];
2493 vinfos[1].indices[1] = _ij1[1];
2494 vinfos[1].maxsolutions = _nj1;
2495 vinfos[2].jointtype = 1;
2496 vinfos[2].foffset = j2;
2497 vinfos[2].indices[0] = _ij2[0];
2498 vinfos[2].indices[1] = _ij2[1];
2499 vinfos[2].maxsolutions = _nj2;
2500 vinfos[3].jointtype = 1;
2501 vinfos[3].foffset = j3;
2502 vinfos[3].indices[0] = _ij3[0];
2503 vinfos[3].indices[1] = _ij3[1];
2504 vinfos[3].maxsolutions = _nj3;
2505 vinfos[4].jointtype = 1;
2506 vinfos[4].foffset = j4;
2507 vinfos[4].indices[0] = _ij4[0];
2508 vinfos[4].indices[1] = _ij4[1];
2509 vinfos[4].maxsolutions = _nj4;
2510 vinfos[5].jointtype = 1;
2511 vinfos[5].foffset = j5;
2512 vinfos[5].indices[0] = _ij5[0];
2513 vinfos[5].indices[1] = _ij5[1];
2514 vinfos[5].maxsolutions = _nj5;
2515 std::vector<int> vfree(0);
2516 solutions.AddSolution(vinfos,vfree);
2517 }
2518 }
2519 }
2520 
2521 }
2522 
2523 }
2524 
2525 } else
2526 {
2527 {
2528 IkReal j5array[1], cj5array[1], sj5array[1];
2529 bool j5valid[1]={false};
2530 _nj5 = 1;
2531 IkReal x214=cj3*cj3;
2532 IkReal x215=new_r22*new_r22;
2533 IkReal x216=(new_r22*sj3);
2534 CheckValue<IkReal> x217=IKPowWithIntegerCheck(IKsign((((x214*x215))+(((-1.0)*x214))+(((-1.0)*x215)))),-1);
2535 if(!x217.valid){
2536 continue;
2537 }
2538 CheckValue<IkReal> x218 = IKatan2WithCheck(IkReal((((new_r00*x216))+((cj3*new_r01)))),IkReal(((((-1.0)*cj3*new_r00))+((new_r01*x216)))),IKFAST_ATAN2_MAGTHRESH);
2539 if(!x218.valid){
2540 continue;
2541 }
2542 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x217.value)))+(x218.value));
2543 sj5array[0]=IKsin(j5array[0]);
2544 cj5array[0]=IKcos(j5array[0]);
2545 if( j5array[0] > IKPI )
2546 {
2547  j5array[0]-=IK2PI;
2548 }
2549 else if( j5array[0] < -IKPI )
2550 { j5array[0]+=IK2PI;
2551 }
2552 j5valid[0] = true;
2553 for(int ij5 = 0; ij5 < 1; ++ij5)
2554 {
2555 if( !j5valid[ij5] )
2556 {
2557  continue;
2558 }
2559 _ij5[0] = ij5; _ij5[1] = -1;
2560 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2561 {
2562 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2563 {
2564  j5valid[iij5]=false; _ij5[1] = iij5; break;
2565 }
2566 }
2567 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2568 {
2569 IkReal evalcond[10];
2570 IkReal x219=IKsin(j5);
2571 IkReal x220=IKcos(j5);
2572 IkReal x221=((1.0)*cj3);
2573 IkReal x222=((1.0)*sj3);
2574 IkReal x223=(cj3*new_r10);
2575 IkReal x224=(cj3*new_r11);
2576 IkReal x225=((1.0)*x220);
2577 IkReal x226=(new_r22*x220);
2578 IkReal x227=(sj3*x219);
2579 IkReal x228=(new_r22*x219);
2580 evalcond[0]=(((new_r11*sj3))+x219+((cj3*new_r01)));
2581 evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x225)));
2582 evalcond[2]=(((cj3*x219))+((sj3*x226))+new_r01);
2583 evalcond[3]=((((-1.0)*x220*x221))+((new_r22*x227))+new_r00);
2584 evalcond[4]=(x227+(((-1.0)*x221*x226))+new_r11);
2585 evalcond[5]=(x223+(((-1.0)*new_r00*x222))+(((-1.0)*x228)));
2586 evalcond[6]=((((-1.0)*new_r01*x222))+x224+(((-1.0)*new_r22*x225)));
2587 evalcond[7]=((((-1.0)*x220*x222))+(((-1.0)*x221*x228))+new_r10);
2588 evalcond[8]=((((-1.0)*new_r00*new_r22*x222))+(((-1.0)*x219))+((new_r22*x223)));
2589 evalcond[9]=((((-1.0)*new_r01*new_r22*x222))+((new_r22*x224))+(((-1.0)*x225)));
2590 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2591 {
2592 continue;
2593 }
2594 }
2595 
2596 {
2597 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2598 vinfos[0].jointtype = 1;
2599 vinfos[0].foffset = j0;
2600 vinfos[0].indices[0] = _ij0[0];
2601 vinfos[0].indices[1] = _ij0[1];
2602 vinfos[0].maxsolutions = _nj0;
2603 vinfos[1].jointtype = 1;
2604 vinfos[1].foffset = j1;
2605 vinfos[1].indices[0] = _ij1[0];
2606 vinfos[1].indices[1] = _ij1[1];
2607 vinfos[1].maxsolutions = _nj1;
2608 vinfos[2].jointtype = 1;
2609 vinfos[2].foffset = j2;
2610 vinfos[2].indices[0] = _ij2[0];
2611 vinfos[2].indices[1] = _ij2[1];
2612 vinfos[2].maxsolutions = _nj2;
2613 vinfos[3].jointtype = 1;
2614 vinfos[3].foffset = j3;
2615 vinfos[3].indices[0] = _ij3[0];
2616 vinfos[3].indices[1] = _ij3[1];
2617 vinfos[3].maxsolutions = _nj3;
2618 vinfos[4].jointtype = 1;
2619 vinfos[4].foffset = j4;
2620 vinfos[4].indices[0] = _ij4[0];
2621 vinfos[4].indices[1] = _ij4[1];
2622 vinfos[4].maxsolutions = _nj4;
2623 vinfos[5].jointtype = 1;
2624 vinfos[5].foffset = j5;
2625 vinfos[5].indices[0] = _ij5[0];
2626 vinfos[5].indices[1] = _ij5[1];
2627 vinfos[5].maxsolutions = _nj5;
2628 std::vector<int> vfree(0);
2629 solutions.AddSolution(vinfos,vfree);
2630 }
2631 }
2632 }
2633 
2634 }
2635 
2636 }
2637  }
2638 
2639 }
2640 
2641 }
2642 
2643 }
2644 } while(0);
2645 if( bgotonextstatement )
2646 {
2647 bool bgotonextstatement = true;
2648 do
2649 {
2650 if( 1 )
2651 {
2652 bgotonextstatement=false;
2653 continue; // branch miss [j3, j5]
2654 
2655 }
2656 } while(0);
2657 if( bgotonextstatement )
2658 {
2659 }
2660 }
2661 }
2662 }
2663 }
2664 
2665 } else
2666 {
2667 {
2668 IkReal j3array[1], cj3array[1], sj3array[1];
2669 bool j3valid[1]={false};
2670 _nj3 = 1;
2671 CheckValue<IkReal> x230=IKPowWithIntegerCheck(sj4,-1);
2672 if(!x230.valid){
2673 continue;
2674 }
2675 IkReal x229=x230.value;
2676 CheckValue<IkReal> x231=IKPowWithIntegerCheck(new_r02,-1);
2677 if(!x231.valid){
2678 continue;
2679 }
2680 if( IKabs((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x229)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12)))))))+IKsqr(((-1.0)*new_r12*x229))-1) <= IKFAST_SINCOS_THRESH )
2681  continue;
2682 j3array[0]=IKatan2((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12)))))), ((-1.0)*new_r12*x229));
2683 sj3array[0]=IKsin(j3array[0]);
2684 cj3array[0]=IKcos(j3array[0]);
2685 if( j3array[0] > IKPI )
2686 {
2687  j3array[0]-=IK2PI;
2688 }
2689 else if( j3array[0] < -IKPI )
2690 { j3array[0]+=IK2PI;
2691 }
2692 j3valid[0] = true;
2693 for(int ij3 = 0; ij3 < 1; ++ij3)
2694 {
2695 if( !j3valid[ij3] )
2696 {
2697  continue;
2698 }
2699 _ij3[0] = ij3; _ij3[1] = -1;
2700 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2701 {
2702 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2703 {
2704  j3valid[iij3]=false; _ij3[1] = iij3; break;
2705 }
2706 }
2707 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2708 {
2709 IkReal evalcond[8];
2710 IkReal x232=IKsin(j3);
2711 IkReal x233=IKcos(j3);
2712 IkReal x234=((1.0)*cj4);
2713 IkReal x235=((1.0)*sj4);
2714 IkReal x236=(new_r02*x232);
2715 IkReal x237=(new_r12*x233);
2716 IkReal x238=(sj4*x233);
2717 evalcond[0]=(x238+new_r12);
2718 evalcond[1]=(new_r02+(((-1.0)*x232*x235)));
2719 evalcond[2]=(((new_r02*x233))+((new_r12*x232)));
2720 evalcond[3]=(sj4+x237+(((-1.0)*x236)));
2721 evalcond[4]=(((new_r22*sj4))+((cj4*x237))+(((-1.0)*x234*x236)));
2722 evalcond[5]=(((new_r10*x238))+(((-1.0)*new_r20*x234))+(((-1.0)*new_r00*x232*x235)));
2723 evalcond[6]=((((-1.0)*new_r01*x232*x235))+(((-1.0)*new_r21*x234))+((new_r11*x238)));
2724 evalcond[7]=((1.0)+(((-1.0)*new_r22*x234))+(((-1.0)*x235*x236))+((sj4*x237)));
2725 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2726 {
2727 continue;
2728 }
2729 }
2730 
2731 {
2732 IkReal j5eval[3];
2733 j5eval[0]=sj4;
2734 j5eval[1]=IKsign(sj4);
2735 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2736 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2737 {
2738 {
2739 IkReal j5eval[2];
2740 j5eval[0]=sj4;
2741 j5eval[1]=cj3;
2742 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2743 {
2744 {
2745 IkReal j5eval[3];
2746 j5eval[0]=sj4;
2747 j5eval[1]=cj4;
2748 j5eval[2]=sj3;
2749 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2750 {
2751 {
2752 IkReal evalcond[5];
2753 bool bgotonextstatement = true;
2754 do
2755 {
2756 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2757 evalcond[1]=new_r12;
2758 evalcond[2]=new_r02;
2759 evalcond[3]=new_r20;
2760 evalcond[4]=new_r21;
2761 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 )
2762 {
2763 bgotonextstatement=false;
2764 {
2765 IkReal j5array[1], cj5array[1], sj5array[1];
2766 bool j5valid[1]={false};
2767 _nj5 = 1;
2768 IkReal x239=((1.0)*new_r01);
2769 if( IKabs(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r00))+(((-1.0)*sj3*x239)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3))))+IKsqr((((cj3*new_r00))+(((-1.0)*sj3*x239))))-1) <= IKFAST_SINCOS_THRESH )
2770  continue;
2771 j5array[0]=IKatan2(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3))), (((cj3*new_r00))+(((-1.0)*sj3*x239))));
2772 sj5array[0]=IKsin(j5array[0]);
2773 cj5array[0]=IKcos(j5array[0]);
2774 if( j5array[0] > IKPI )
2775 {
2776  j5array[0]-=IK2PI;
2777 }
2778 else if( j5array[0] < -IKPI )
2779 { j5array[0]+=IK2PI;
2780 }
2781 j5valid[0] = true;
2782 for(int ij5 = 0; ij5 < 1; ++ij5)
2783 {
2784 if( !j5valid[ij5] )
2785 {
2786  continue;
2787 }
2788 _ij5[0] = ij5; _ij5[1] = -1;
2789 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2790 {
2791 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2792 {
2793  j5valid[iij5]=false; _ij5[1] = iij5; break;
2794 }
2795 }
2796 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2797 {
2798 IkReal evalcond[8];
2799 IkReal x240=IKsin(j5);
2800 IkReal x241=IKcos(j5);
2801 IkReal x242=((1.0)*sj3);
2802 IkReal x243=(sj3*x240);
2803 IkReal x244=((1.0)*x241);
2804 IkReal x245=((1.0)*x240);
2805 IkReal x246=(cj3*x244);
2806 evalcond[0]=(((new_r11*sj3))+x240+((cj3*new_r01)));
2807 evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x244)));
2808 evalcond[2]=(((sj3*x241))+new_r01+((cj3*x240)));
2809 evalcond[3]=(x243+(((-1.0)*x246))+new_r00);
2810 evalcond[4]=(x243+(((-1.0)*x246))+new_r11);
2811 evalcond[5]=((((-1.0)*new_r00*x242))+((cj3*new_r10))+(((-1.0)*x245)));
2812 evalcond[6]=((((-1.0)*new_r01*x242))+((cj3*new_r11))+(((-1.0)*x244)));
2813 evalcond[7]=((((-1.0)*x241*x242))+(((-1.0)*cj3*x245))+new_r10);
2814 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2815 {
2816 continue;
2817 }
2818 }
2819 
2820 {
2821 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2822 vinfos[0].jointtype = 1;
2823 vinfos[0].foffset = j0;
2824 vinfos[0].indices[0] = _ij0[0];
2825 vinfos[0].indices[1] = _ij0[1];
2826 vinfos[0].maxsolutions = _nj0;
2827 vinfos[1].jointtype = 1;
2828 vinfos[1].foffset = j1;
2829 vinfos[1].indices[0] = _ij1[0];
2830 vinfos[1].indices[1] = _ij1[1];
2831 vinfos[1].maxsolutions = _nj1;
2832 vinfos[2].jointtype = 1;
2833 vinfos[2].foffset = j2;
2834 vinfos[2].indices[0] = _ij2[0];
2835 vinfos[2].indices[1] = _ij2[1];
2836 vinfos[2].maxsolutions = _nj2;
2837 vinfos[3].jointtype = 1;
2838 vinfos[3].foffset = j3;
2839 vinfos[3].indices[0] = _ij3[0];
2840 vinfos[3].indices[1] = _ij3[1];
2841 vinfos[3].maxsolutions = _nj3;
2842 vinfos[4].jointtype = 1;
2843 vinfos[4].foffset = j4;
2844 vinfos[4].indices[0] = _ij4[0];
2845 vinfos[4].indices[1] = _ij4[1];
2846 vinfos[4].maxsolutions = _nj4;
2847 vinfos[5].jointtype = 1;
2848 vinfos[5].foffset = j5;
2849 vinfos[5].indices[0] = _ij5[0];
2850 vinfos[5].indices[1] = _ij5[1];
2851 vinfos[5].maxsolutions = _nj5;
2852 std::vector<int> vfree(0);
2853 solutions.AddSolution(vinfos,vfree);
2854 }
2855 }
2856 }
2857 
2858 }
2859 } while(0);
2860 if( bgotonextstatement )
2861 {
2862 bool bgotonextstatement = true;
2863 do
2864 {
2865 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2866 evalcond[1]=new_r12;
2867 evalcond[2]=new_r02;
2868 evalcond[3]=new_r20;
2869 evalcond[4]=new_r21;
2870 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 )
2871 {
2872 bgotonextstatement=false;
2873 {
2874 IkReal j5array[1], cj5array[1], sj5array[1];
2875 bool j5valid[1]={false};
2876 _nj5 = 1;
2877 IkReal x247=((1.0)*cj3);
2878 if( IKabs(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj3))+(((-1.0)*new_r11*x247)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3))))+IKsqr((((new_r01*sj3))+(((-1.0)*new_r11*x247))))-1) <= IKFAST_SINCOS_THRESH )
2879  continue;
2880 j5array[0]=IKatan2(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3))), (((new_r01*sj3))+(((-1.0)*new_r11*x247))));
2881 sj5array[0]=IKsin(j5array[0]);
2882 cj5array[0]=IKcos(j5array[0]);
2883 if( j5array[0] > IKPI )
2884 {
2885  j5array[0]-=IK2PI;
2886 }
2887 else if( j5array[0] < -IKPI )
2888 { j5array[0]+=IK2PI;
2889 }
2890 j5valid[0] = true;
2891 for(int ij5 = 0; ij5 < 1; ++ij5)
2892 {
2893 if( !j5valid[ij5] )
2894 {
2895  continue;
2896 }
2897 _ij5[0] = ij5; _ij5[1] = -1;
2898 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2899 {
2900 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2901 {
2902  j5valid[iij5]=false; _ij5[1] = iij5; break;
2903 }
2904 }
2905 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2906 {
2907 IkReal evalcond[8];
2908 IkReal x248=IKsin(j5);
2909 IkReal x249=IKcos(j5);
2910 IkReal x250=((1.0)*sj3);
2911 IkReal x251=(cj3*x248);
2912 IkReal x252=((1.0)*x249);
2913 IkReal x253=(x249*x250);
2914 evalcond[0]=(((new_r11*sj3))+x248+((cj3*new_r01)));
2915 evalcond[1]=(x248+((cj3*new_r10))+(((-1.0)*new_r00*x250)));
2916 evalcond[2]=((((-1.0)*new_r01*x250))+x249+((cj3*new_r11)));
2917 evalcond[3]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x252)));
2918 evalcond[4]=(((sj3*x248))+new_r11+((cj3*x249)));
2919 evalcond[5]=(x251+new_r01+(((-1.0)*x253)));
2920 evalcond[6]=(x251+new_r10+(((-1.0)*x253)));
2921 evalcond[7]=((((-1.0)*x248*x250))+(((-1.0)*cj3*x252))+new_r00);
2922 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2923 {
2924 continue;
2925 }
2926 }
2927 
2928 {
2929 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2930 vinfos[0].jointtype = 1;
2931 vinfos[0].foffset = j0;
2932 vinfos[0].indices[0] = _ij0[0];
2933 vinfos[0].indices[1] = _ij0[1];
2934 vinfos[0].maxsolutions = _nj0;
2935 vinfos[1].jointtype = 1;
2936 vinfos[1].foffset = j1;
2937 vinfos[1].indices[0] = _ij1[0];
2938 vinfos[1].indices[1] = _ij1[1];
2939 vinfos[1].maxsolutions = _nj1;
2940 vinfos[2].jointtype = 1;
2941 vinfos[2].foffset = j2;
2942 vinfos[2].indices[0] = _ij2[0];
2943 vinfos[2].indices[1] = _ij2[1];
2944 vinfos[2].maxsolutions = _nj2;
2945 vinfos[3].jointtype = 1;
2946 vinfos[3].foffset = j3;
2947 vinfos[3].indices[0] = _ij3[0];
2948 vinfos[3].indices[1] = _ij3[1];
2949 vinfos[3].maxsolutions = _nj3;
2950 vinfos[4].jointtype = 1;
2951 vinfos[4].foffset = j4;
2952 vinfos[4].indices[0] = _ij4[0];
2953 vinfos[4].indices[1] = _ij4[1];
2954 vinfos[4].maxsolutions = _nj4;
2955 vinfos[5].jointtype = 1;
2956 vinfos[5].foffset = j5;
2957 vinfos[5].indices[0] = _ij5[0];
2958 vinfos[5].indices[1] = _ij5[1];
2959 vinfos[5].maxsolutions = _nj5;
2960 std::vector<int> vfree(0);
2961 solutions.AddSolution(vinfos,vfree);
2962 }
2963 }
2964 }
2965 
2966 }
2967 } while(0);
2968 if( bgotonextstatement )
2969 {
2970 bool bgotonextstatement = true;
2971 do
2972 {
2973 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
2974 evalcond[1]=new_r22;
2975 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
2976 {
2977 bgotonextstatement=false;
2978 {
2979 IkReal j5array[1], cj5array[1], sj5array[1];
2980 bool j5valid[1]={false};
2981 _nj5 = 1;
2982 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
2983  continue;
2984 j5array[0]=IKatan2(new_r20, new_r21);
2985 sj5array[0]=IKsin(j5array[0]);
2986 cj5array[0]=IKcos(j5array[0]);
2987 if( j5array[0] > IKPI )
2988 {
2989  j5array[0]-=IK2PI;
2990 }
2991 else if( j5array[0] < -IKPI )
2992 { j5array[0]+=IK2PI;
2993 }
2994 j5valid[0] = true;
2995 for(int ij5 = 0; ij5 < 1; ++ij5)
2996 {
2997 if( !j5valid[ij5] )
2998 {
2999  continue;
3000 }
3001 _ij5[0] = ij5; _ij5[1] = -1;
3002 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3003 {
3004 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3005 {
3006  j5valid[iij5]=false; _ij5[1] = iij5; break;
3007 }
3008 }
3009 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3010 {
3011 IkReal evalcond[8];
3012 IkReal x254=IKsin(j5);
3013 IkReal x255=IKcos(j5);
3014 IkReal x256=((1.0)*x255);
3015 IkReal x257=((1.0)*x254);
3016 evalcond[0]=(new_r20+(((-1.0)*x257)));
3017 evalcond[1]=(new_r21+(((-1.0)*x256)));
3018 evalcond[2]=(((sj3*x254))+new_r11);
3019 evalcond[3]=(new_r01+(((-1.0)*new_r12*x257)));
3020 evalcond[4]=((((-1.0)*cj3*x256))+new_r00);
3021 evalcond[5]=((((-1.0)*sj3*x256))+new_r10);
3022 evalcond[6]=(((new_r11*sj3))+x254+((cj3*new_r01)));
3023 evalcond[7]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x256)));
3024 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3025 {
3026 continue;
3027 }
3028 }
3029 
3030 {
3031 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3032 vinfos[0].jointtype = 1;
3033 vinfos[0].foffset = j0;
3034 vinfos[0].indices[0] = _ij0[0];
3035 vinfos[0].indices[1] = _ij0[1];
3036 vinfos[0].maxsolutions = _nj0;
3037 vinfos[1].jointtype = 1;
3038 vinfos[1].foffset = j1;
3039 vinfos[1].indices[0] = _ij1[0];
3040 vinfos[1].indices[1] = _ij1[1];
3041 vinfos[1].maxsolutions = _nj1;
3042 vinfos[2].jointtype = 1;
3043 vinfos[2].foffset = j2;
3044 vinfos[2].indices[0] = _ij2[0];
3045 vinfos[2].indices[1] = _ij2[1];
3046 vinfos[2].maxsolutions = _nj2;
3047 vinfos[3].jointtype = 1;
3048 vinfos[3].foffset = j3;
3049 vinfos[3].indices[0] = _ij3[0];
3050 vinfos[3].indices[1] = _ij3[1];
3051 vinfos[3].maxsolutions = _nj3;
3052 vinfos[4].jointtype = 1;
3053 vinfos[4].foffset = j4;
3054 vinfos[4].indices[0] = _ij4[0];
3055 vinfos[4].indices[1] = _ij4[1];
3056 vinfos[4].maxsolutions = _nj4;
3057 vinfos[5].jointtype = 1;
3058 vinfos[5].foffset = j5;
3059 vinfos[5].indices[0] = _ij5[0];
3060 vinfos[5].indices[1] = _ij5[1];
3061 vinfos[5].maxsolutions = _nj5;
3062 std::vector<int> vfree(0);
3063 solutions.AddSolution(vinfos,vfree);
3064 }
3065 }
3066 }
3067 
3068 }
3069 } while(0);
3070 if( bgotonextstatement )
3071 {
3072 bool bgotonextstatement = true;
3073 do
3074 {
3075 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
3076 evalcond[1]=new_r22;
3077 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3078 {
3079 bgotonextstatement=false;
3080 {
3081 IkReal j5array[1], cj5array[1], sj5array[1];
3082 bool j5valid[1]={false};
3083 _nj5 = 1;
3084 if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
3085  continue;
3086 j5array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21));
3087 sj5array[0]=IKsin(j5array[0]);
3088 cj5array[0]=IKcos(j5array[0]);
3089 if( j5array[0] > IKPI )
3090 {
3091  j5array[0]-=IK2PI;
3092 }
3093 else if( j5array[0] < -IKPI )
3094 { j5array[0]+=IK2PI;
3095 }
3096 j5valid[0] = true;
3097 for(int ij5 = 0; ij5 < 1; ++ij5)
3098 {
3099 if( !j5valid[ij5] )
3100 {
3101  continue;
3102 }
3103 _ij5[0] = ij5; _ij5[1] = -1;
3104 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3105 {
3106 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3107 {
3108  j5valid[iij5]=false; _ij5[1] = iij5; break;
3109 }
3110 }
3111 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3112 {
3113 IkReal evalcond[8];
3114 IkReal x258=IKsin(j5);
3115 IkReal x259=IKcos(j5);
3116 IkReal x260=((1.0)*x259);
3117 evalcond[0]=(x258+new_r20);
3118 evalcond[1]=(x259+new_r21);
3119 evalcond[2]=(((new_r12*x258))+new_r01);
3120 evalcond[3]=(((sj3*x258))+new_r11);
3121 evalcond[4]=((((-1.0)*cj3*x260))+new_r00);
3122 evalcond[5]=((((-1.0)*sj3*x260))+new_r10);
3123 evalcond[6]=(((new_r11*sj3))+x258+((cj3*new_r01)));
3124 evalcond[7]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x260)));
3125 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3126 {
3127 continue;
3128 }
3129 }
3130 
3131 {
3132 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3133 vinfos[0].jointtype = 1;
3134 vinfos[0].foffset = j0;
3135 vinfos[0].indices[0] = _ij0[0];
3136 vinfos[0].indices[1] = _ij0[1];
3137 vinfos[0].maxsolutions = _nj0;
3138 vinfos[1].jointtype = 1;
3139 vinfos[1].foffset = j1;
3140 vinfos[1].indices[0] = _ij1[0];
3141 vinfos[1].indices[1] = _ij1[1];
3142 vinfos[1].maxsolutions = _nj1;
3143 vinfos[2].jointtype = 1;
3144 vinfos[2].foffset = j2;
3145 vinfos[2].indices[0] = _ij2[0];
3146 vinfos[2].indices[1] = _ij2[1];
3147 vinfos[2].maxsolutions = _nj2;
3148 vinfos[3].jointtype = 1;
3149 vinfos[3].foffset = j3;
3150 vinfos[3].indices[0] = _ij3[0];
3151 vinfos[3].indices[1] = _ij3[1];
3152 vinfos[3].maxsolutions = _nj3;
3153 vinfos[4].jointtype = 1;
3154 vinfos[4].foffset = j4;
3155 vinfos[4].indices[0] = _ij4[0];
3156 vinfos[4].indices[1] = _ij4[1];
3157 vinfos[4].maxsolutions = _nj4;
3158 vinfos[5].jointtype = 1;
3159 vinfos[5].foffset = j5;
3160 vinfos[5].indices[0] = _ij5[0];
3161 vinfos[5].indices[1] = _ij5[1];
3162 vinfos[5].maxsolutions = _nj5;
3163 std::vector<int> vfree(0);
3164 solutions.AddSolution(vinfos,vfree);
3165 }
3166 }
3167 }
3168 
3169 }
3170 } while(0);
3171 if( bgotonextstatement )
3172 {
3173 bool bgotonextstatement = true;
3174 do
3175 {
3176 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3177 evalcond[1]=new_r02;
3178 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3179 {
3180 bgotonextstatement=false;
3181 {
3182 IkReal j5array[1], cj5array[1], sj5array[1];
3183 bool j5valid[1]={false};
3184 _nj5 = 1;
3185 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 )
3186  continue;
3187 j5array[0]=IKatan2(((-1.0)*new_r01), new_r00);
3188 sj5array[0]=IKsin(j5array[0]);
3189 cj5array[0]=IKcos(j5array[0]);
3190 if( j5array[0] > IKPI )
3191 {
3192  j5array[0]-=IK2PI;
3193 }
3194 else if( j5array[0] < -IKPI )
3195 { j5array[0]+=IK2PI;
3196 }
3197 j5valid[0] = true;
3198 for(int ij5 = 0; ij5 < 1; ++ij5)
3199 {
3200 if( !j5valid[ij5] )
3201 {
3202  continue;
3203 }
3204 _ij5[0] = ij5; _ij5[1] = -1;
3205 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3206 {
3207 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3208 {
3209  j5valid[iij5]=false; _ij5[1] = iij5; break;
3210 }
3211 }
3212 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3213 {
3214 IkReal evalcond[8];
3215 IkReal x261=IKsin(j5);
3216 IkReal x262=IKcos(j5);
3217 IkReal x263=((1.0)*cj4);
3218 IkReal x264=((1.0)*x262);
3219 IkReal x265=((1.0)*x261);
3220 evalcond[0]=(x261+new_r01);
3221 evalcond[1]=(new_r00+(((-1.0)*x264)));
3222 evalcond[2]=((((-1.0)*sj4*x265))+new_r20);
3223 evalcond[3]=((((-1.0)*sj4*x264))+new_r21);
3224 evalcond[4]=((((-1.0)*x262*x263))+new_r11);
3225 evalcond[5]=((((-1.0)*x261*x263))+new_r10);
3226 evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x265)));
3227 evalcond[7]=(((cj4*new_r11))+((new_r21*sj4))+(((-1.0)*x264)));
3228 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3229 {
3230 continue;
3231 }
3232 }
3233 
3234 {
3235 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3236 vinfos[0].jointtype = 1;
3237 vinfos[0].foffset = j0;
3238 vinfos[0].indices[0] = _ij0[0];
3239 vinfos[0].indices[1] = _ij0[1];
3240 vinfos[0].maxsolutions = _nj0;
3241 vinfos[1].jointtype = 1;
3242 vinfos[1].foffset = j1;
3243 vinfos[1].indices[0] = _ij1[0];
3244 vinfos[1].indices[1] = _ij1[1];
3245 vinfos[1].maxsolutions = _nj1;
3246 vinfos[2].jointtype = 1;
3247 vinfos[2].foffset = j2;
3248 vinfos[2].indices[0] = _ij2[0];
3249 vinfos[2].indices[1] = _ij2[1];
3250 vinfos[2].maxsolutions = _nj2;
3251 vinfos[3].jointtype = 1;
3252 vinfos[3].foffset = j3;
3253 vinfos[3].indices[0] = _ij3[0];
3254 vinfos[3].indices[1] = _ij3[1];
3255 vinfos[3].maxsolutions = _nj3;
3256 vinfos[4].jointtype = 1;
3257 vinfos[4].foffset = j4;
3258 vinfos[4].indices[0] = _ij4[0];
3259 vinfos[4].indices[1] = _ij4[1];
3260 vinfos[4].maxsolutions = _nj4;
3261 vinfos[5].jointtype = 1;
3262 vinfos[5].foffset = j5;
3263 vinfos[5].indices[0] = _ij5[0];
3264 vinfos[5].indices[1] = _ij5[1];
3265 vinfos[5].maxsolutions = _nj5;
3266 std::vector<int> vfree(0);
3267 solutions.AddSolution(vinfos,vfree);
3268 }
3269 }
3270 }
3271 
3272 }
3273 } while(0);
3274 if( bgotonextstatement )
3275 {
3276 bool bgotonextstatement = true;
3277 do
3278 {
3279 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3280 evalcond[1]=new_r02;
3281 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3282 {
3283 bgotonextstatement=false;
3284 {
3285 IkReal j5array[1], cj5array[1], sj5array[1];
3286 bool j5valid[1]={false};
3287 _nj5 = 1;
3288 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
3289  continue;
3290 j5array[0]=IKatan2(new_r01, ((-1.0)*new_r00));
3291 sj5array[0]=IKsin(j5array[0]);
3292 cj5array[0]=IKcos(j5array[0]);
3293 if( j5array[0] > IKPI )
3294 {
3295  j5array[0]-=IK2PI;
3296 }
3297 else if( j5array[0] < -IKPI )
3298 { j5array[0]+=IK2PI;
3299 }
3300 j5valid[0] = true;
3301 for(int ij5 = 0; ij5 < 1; ++ij5)
3302 {
3303 if( !j5valid[ij5] )
3304 {
3305  continue;
3306 }
3307 _ij5[0] = ij5; _ij5[1] = -1;
3308 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3309 {
3310 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3311 {
3312  j5valid[iij5]=false; _ij5[1] = iij5; break;
3313 }
3314 }
3315 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3316 {
3317 IkReal evalcond[8];
3318 IkReal x266=IKsin(j5);
3319 IkReal x267=IKcos(j5);
3320 IkReal x268=((1.0)*cj4);
3321 IkReal x269=((1.0)*x267);
3322 IkReal x270=((1.0)*x266);
3323 evalcond[0]=(x266+(((-1.0)*new_r01)));
3324 evalcond[1]=(((cj4*x267))+new_r11);
3325 evalcond[2]=((((-1.0)*sj4*x270))+new_r20);
3326 evalcond[3]=((((-1.0)*sj4*x269))+new_r21);
3327 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x269)));
3328 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x266*x268)));
3329 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r10*x268))+(((-1.0)*x270)));
3330 evalcond[7]=((((-1.0)*new_r11*x268))+((new_r21*sj4))+(((-1.0)*x269)));
3331 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3332 {
3333 continue;
3334 }
3335 }
3336 
3337 {
3338 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3339 vinfos[0].jointtype = 1;
3340 vinfos[0].foffset = j0;
3341 vinfos[0].indices[0] = _ij0[0];
3342 vinfos[0].indices[1] = _ij0[1];
3343 vinfos[0].maxsolutions = _nj0;
3344 vinfos[1].jointtype = 1;
3345 vinfos[1].foffset = j1;
3346 vinfos[1].indices[0] = _ij1[0];
3347 vinfos[1].indices[1] = _ij1[1];
3348 vinfos[1].maxsolutions = _nj1;
3349 vinfos[2].jointtype = 1;
3350 vinfos[2].foffset = j2;
3351 vinfos[2].indices[0] = _ij2[0];
3352 vinfos[2].indices[1] = _ij2[1];
3353 vinfos[2].maxsolutions = _nj2;
3354 vinfos[3].jointtype = 1;
3355 vinfos[3].foffset = j3;
3356 vinfos[3].indices[0] = _ij3[0];
3357 vinfos[3].indices[1] = _ij3[1];
3358 vinfos[3].maxsolutions = _nj3;
3359 vinfos[4].jointtype = 1;
3360 vinfos[4].foffset = j4;
3361 vinfos[4].indices[0] = _ij4[0];
3362 vinfos[4].indices[1] = _ij4[1];
3363 vinfos[4].maxsolutions = _nj4;
3364 vinfos[5].jointtype = 1;
3365 vinfos[5].foffset = j5;
3366 vinfos[5].indices[0] = _ij5[0];
3367 vinfos[5].indices[1] = _ij5[1];
3368 vinfos[5].maxsolutions = _nj5;
3369 std::vector<int> vfree(0);
3370 solutions.AddSolution(vinfos,vfree);
3371 }
3372 }
3373 }
3374 
3375 }
3376 } while(0);
3377 if( bgotonextstatement )
3378 {
3379 bool bgotonextstatement = true;
3380 do
3381 {
3382 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3383 evalcond[1]=new_r12;
3384 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3385 {
3386 bgotonextstatement=false;
3387 {
3388 IkReal j5array[1], cj5array[1], sj5array[1];
3389 bool j5valid[1]={false};
3390 _nj5 = 1;
3391 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
3392  continue;
3393 j5array[0]=IKatan2(((-1.0)*new_r11), new_r10);
3394 sj5array[0]=IKsin(j5array[0]);
3395 cj5array[0]=IKcos(j5array[0]);
3396 if( j5array[0] > IKPI )
3397 {
3398  j5array[0]-=IK2PI;
3399 }
3400 else if( j5array[0] < -IKPI )
3401 { j5array[0]+=IK2PI;
3402 }
3403 j5valid[0] = true;
3404 for(int ij5 = 0; ij5 < 1; ++ij5)
3405 {
3406 if( !j5valid[ij5] )
3407 {
3408  continue;
3409 }
3410 _ij5[0] = ij5; _ij5[1] = -1;
3411 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3412 {
3413 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3414 {
3415  j5valid[iij5]=false; _ij5[1] = iij5; break;
3416 }
3417 }
3418 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3419 {
3420 IkReal evalcond[8];
3421 IkReal x271=IKcos(j5);
3422 IkReal x272=IKsin(j5);
3423 IkReal x273=((1.0)*cj4);
3424 IkReal x274=((1.0)*x271);
3425 IkReal x275=((1.0)*x272);
3426 evalcond[0]=(x272+new_r11);
3427 evalcond[1]=((((-1.0)*x274))+new_r10);
3428 evalcond[2]=(((cj4*x271))+new_r01);
3429 evalcond[3]=(((cj4*x272))+new_r00);
3430 evalcond[4]=((((-1.0)*new_r02*x275))+new_r20);
3431 evalcond[5]=((((-1.0)*new_r02*x274))+new_r21);
3432 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r00*x273))+(((-1.0)*x275)));
3433 evalcond[7]=((((-1.0)*new_r01*x273))+(((-1.0)*x274))+((new_r21*sj4)));
3434 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3435 {
3436 continue;
3437 }
3438 }
3439 
3440 {
3441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3442 vinfos[0].jointtype = 1;
3443 vinfos[0].foffset = j0;
3444 vinfos[0].indices[0] = _ij0[0];
3445 vinfos[0].indices[1] = _ij0[1];
3446 vinfos[0].maxsolutions = _nj0;
3447 vinfos[1].jointtype = 1;
3448 vinfos[1].foffset = j1;
3449 vinfos[1].indices[0] = _ij1[0];
3450 vinfos[1].indices[1] = _ij1[1];
3451 vinfos[1].maxsolutions = _nj1;
3452 vinfos[2].jointtype = 1;
3453 vinfos[2].foffset = j2;
3454 vinfos[2].indices[0] = _ij2[0];
3455 vinfos[2].indices[1] = _ij2[1];
3456 vinfos[2].maxsolutions = _nj2;
3457 vinfos[3].jointtype = 1;
3458 vinfos[3].foffset = j3;
3459 vinfos[3].indices[0] = _ij3[0];
3460 vinfos[3].indices[1] = _ij3[1];
3461 vinfos[3].maxsolutions = _nj3;
3462 vinfos[4].jointtype = 1;
3463 vinfos[4].foffset = j4;
3464 vinfos[4].indices[0] = _ij4[0];
3465 vinfos[4].indices[1] = _ij4[1];
3466 vinfos[4].maxsolutions = _nj4;
3467 vinfos[5].jointtype = 1;
3468 vinfos[5].foffset = j5;
3469 vinfos[5].indices[0] = _ij5[0];
3470 vinfos[5].indices[1] = _ij5[1];
3471 vinfos[5].maxsolutions = _nj5;
3472 std::vector<int> vfree(0);
3473 solutions.AddSolution(vinfos,vfree);
3474 }
3475 }
3476 }
3477 
3478 }
3479 } while(0);
3480 if( bgotonextstatement )
3481 {
3482 bool bgotonextstatement = true;
3483 do
3484 {
3485 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3486 evalcond[1]=new_r12;
3487 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3488 {
3489 bgotonextstatement=false;
3490 {
3491 IkReal j5eval[3];
3492 sj3=-1.0;
3493 cj3=0;
3494 j3=-1.5707963267949;
3495 j5eval[0]=new_r02;
3496 j5eval[1]=IKsign(new_r02);
3497 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
3498 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
3499 {
3500 {
3501 IkReal j5eval[1];
3502 sj3=-1.0;
3503 cj3=0;
3504 j3=-1.5707963267949;
3505 j5eval[0]=new_r02;
3506 if( IKabs(j5eval[0]) < 0.0000010000000000 )
3507 {
3508 {
3509 IkReal j5eval[2];
3510 sj3=-1.0;
3511 cj3=0;
3512 j3=-1.5707963267949;
3513 j5eval[0]=new_r02;
3514 j5eval[1]=cj4;
3515 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3516 {
3517 {
3518 IkReal evalcond[4];
3519 bool bgotonextstatement = true;
3520 do
3521 {
3522 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
3523 evalcond[1]=new_r22;
3524 evalcond[2]=new_r01;
3525 evalcond[3]=new_r00;
3526 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3527 {
3528 bgotonextstatement=false;
3529 {
3530 IkReal j5array[1], cj5array[1], sj5array[1];
3531 bool j5valid[1]={false};
3532 _nj5 = 1;
3533 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
3534  continue;
3535 j5array[0]=IKatan2(new_r20, new_r21);
3536 sj5array[0]=IKsin(j5array[0]);
3537 cj5array[0]=IKcos(j5array[0]);
3538 if( j5array[0] > IKPI )
3539 {
3540  j5array[0]-=IK2PI;
3541 }
3542 else if( j5array[0] < -IKPI )
3543 { j5array[0]+=IK2PI;
3544 }
3545 j5valid[0] = true;
3546 for(int ij5 = 0; ij5 < 1; ++ij5)
3547 {
3548 if( !j5valid[ij5] )
3549 {
3550  continue;
3551 }
3552 _ij5[0] = ij5; _ij5[1] = -1;
3553 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3554 {
3555 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3556 {
3557  j5valid[iij5]=false; _ij5[1] = iij5; break;
3558 }
3559 }
3560 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3561 {
3562 IkReal evalcond[4];
3563 IkReal x276=IKsin(j5);
3564 IkReal x277=((1.0)*(IKcos(j5)));
3565 evalcond[0]=((((-1.0)*x276))+new_r20);
3566 evalcond[1]=((((-1.0)*x277))+new_r21);
3567 evalcond[2]=(x276+(((-1.0)*new_r11)));
3568 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x277)));
3569 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3570 {
3571 continue;
3572 }
3573 }
3574 
3575 {
3576 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3577 vinfos[0].jointtype = 1;
3578 vinfos[0].foffset = j0;
3579 vinfos[0].indices[0] = _ij0[0];
3580 vinfos[0].indices[1] = _ij0[1];
3581 vinfos[0].maxsolutions = _nj0;
3582 vinfos[1].jointtype = 1;
3583 vinfos[1].foffset = j1;
3584 vinfos[1].indices[0] = _ij1[0];
3585 vinfos[1].indices[1] = _ij1[1];
3586 vinfos[1].maxsolutions = _nj1;
3587 vinfos[2].jointtype = 1;
3588 vinfos[2].foffset = j2;
3589 vinfos[2].indices[0] = _ij2[0];
3590 vinfos[2].indices[1] = _ij2[1];
3591 vinfos[2].maxsolutions = _nj2;
3592 vinfos[3].jointtype = 1;
3593 vinfos[3].foffset = j3;
3594 vinfos[3].indices[0] = _ij3[0];
3595 vinfos[3].indices[1] = _ij3[1];
3596 vinfos[3].maxsolutions = _nj3;
3597 vinfos[4].jointtype = 1;
3598 vinfos[4].foffset = j4;
3599 vinfos[4].indices[0] = _ij4[0];
3600 vinfos[4].indices[1] = _ij4[1];
3601 vinfos[4].maxsolutions = _nj4;
3602 vinfos[5].jointtype = 1;
3603 vinfos[5].foffset = j5;
3604 vinfos[5].indices[0] = _ij5[0];
3605 vinfos[5].indices[1] = _ij5[1];
3606 vinfos[5].maxsolutions = _nj5;
3607 std::vector<int> vfree(0);
3608 solutions.AddSolution(vinfos,vfree);
3609 }
3610 }
3611 }
3612 
3613 }
3614 } while(0);
3615 if( bgotonextstatement )
3616 {
3617 bool bgotonextstatement = true;
3618 do
3619 {
3620 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
3621 evalcond[1]=new_r22;
3622 evalcond[2]=new_r01;
3623 evalcond[3]=new_r00;
3624 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
3625 {
3626 bgotonextstatement=false;
3627 {
3628 IkReal j5array[1], cj5array[1], sj5array[1];
3629 bool j5valid[1]={false};
3630 _nj5 = 1;
3631 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
3632  continue;
3633 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21));
3634 sj5array[0]=IKsin(j5array[0]);
3635 cj5array[0]=IKcos(j5array[0]);
3636 if( j5array[0] > IKPI )
3637 {
3638  j5array[0]-=IK2PI;
3639 }
3640 else if( j5array[0] < -IKPI )
3641 { j5array[0]+=IK2PI;
3642 }
3643 j5valid[0] = true;
3644 for(int ij5 = 0; ij5 < 1; ++ij5)
3645 {
3646 if( !j5valid[ij5] )
3647 {
3648  continue;
3649 }
3650 _ij5[0] = ij5; _ij5[1] = -1;
3651 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3652 {
3653 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3654 {
3655  j5valid[iij5]=false; _ij5[1] = iij5; break;
3656 }
3657 }
3658 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3659 {
3660 IkReal evalcond[4];
3661 IkReal x278=IKsin(j5);
3662 IkReal x279=IKcos(j5);
3663 evalcond[0]=(x278+new_r20);
3664 evalcond[1]=(x279+new_r21);
3665 evalcond[2]=(x278+(((-1.0)*new_r11)));
3666 evalcond[3]=((((-1.0)*x279))+(((-1.0)*new_r10)));
3667 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
3668 {
3669 continue;
3670 }
3671 }
3672 
3673 {
3674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3675 vinfos[0].jointtype = 1;
3676 vinfos[0].foffset = j0;
3677 vinfos[0].indices[0] = _ij0[0];
3678 vinfos[0].indices[1] = _ij0[1];
3679 vinfos[0].maxsolutions = _nj0;
3680 vinfos[1].jointtype = 1;
3681 vinfos[1].foffset = j1;
3682 vinfos[1].indices[0] = _ij1[0];
3683 vinfos[1].indices[1] = _ij1[1];
3684 vinfos[1].maxsolutions = _nj1;
3685 vinfos[2].jointtype = 1;
3686 vinfos[2].foffset = j2;
3687 vinfos[2].indices[0] = _ij2[0];
3688 vinfos[2].indices[1] = _ij2[1];
3689 vinfos[2].maxsolutions = _nj2;
3690 vinfos[3].jointtype = 1;
3691 vinfos[3].foffset = j3;
3692 vinfos[3].indices[0] = _ij3[0];
3693 vinfos[3].indices[1] = _ij3[1];
3694 vinfos[3].maxsolutions = _nj3;
3695 vinfos[4].jointtype = 1;
3696 vinfos[4].foffset = j4;
3697 vinfos[4].indices[0] = _ij4[0];
3698 vinfos[4].indices[1] = _ij4[1];
3699 vinfos[4].maxsolutions = _nj4;
3700 vinfos[5].jointtype = 1;
3701 vinfos[5].foffset = j5;
3702 vinfos[5].indices[0] = _ij5[0];
3703 vinfos[5].indices[1] = _ij5[1];
3704 vinfos[5].maxsolutions = _nj5;
3705 std::vector<int> vfree(0);
3706 solutions.AddSolution(vinfos,vfree);
3707 }
3708 }
3709 }
3710 
3711 }
3712 } while(0);
3713 if( bgotonextstatement )
3714 {
3715 bool bgotonextstatement = true;
3716 do
3717 {
3718 evalcond[0]=IKabs(new_r02);
3719 evalcond[1]=new_r20;
3720 evalcond[2]=new_r21;
3721 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
3722 {
3723 bgotonextstatement=false;
3724 {
3725 IkReal j5array[1], cj5array[1], sj5array[1];
3726 bool j5valid[1]={false};
3727 _nj5 = 1;
3728 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
3729  continue;
3730 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
3731 sj5array[0]=IKsin(j5array[0]);
3732 cj5array[0]=IKcos(j5array[0]);
3733 if( j5array[0] > IKPI )
3734 {
3735  j5array[0]-=IK2PI;
3736 }
3737 else if( j5array[0] < -IKPI )
3738 { j5array[0]+=IK2PI;
3739 }
3740 j5valid[0] = true;
3741 for(int ij5 = 0; ij5 < 1; ++ij5)
3742 {
3743 if( !j5valid[ij5] )
3744 {
3745  continue;
3746 }
3747 _ij5[0] = ij5; _ij5[1] = -1;
3748 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3749 {
3750 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3751 {
3752  j5valid[iij5]=false; _ij5[1] = iij5; break;
3753 }
3754 }
3755 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3756 {
3757 IkReal evalcond[6];
3758 IkReal x280=IKsin(j5);
3759 IkReal x281=IKcos(j5);
3760 IkReal x282=((1.0)*cj4);
3761 IkReal x283=((1.0)*x281);
3762 evalcond[0]=(x280+(((-1.0)*new_r11)));
3763 evalcond[1]=((((-1.0)*x281*x282))+new_r01);
3764 evalcond[2]=((((-1.0)*x280*x282))+new_r00);
3765 evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x283)));
3766 evalcond[4]=(((cj4*new_r00))+(((-1.0)*x280)));
3767 evalcond[5]=(((cj4*new_r01))+(((-1.0)*x283)));
3768 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
3769 {
3770 continue;
3771 }
3772 }
3773 
3774 {
3775 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3776 vinfos[0].jointtype = 1;
3777 vinfos[0].foffset = j0;
3778 vinfos[0].indices[0] = _ij0[0];
3779 vinfos[0].indices[1] = _ij0[1];
3780 vinfos[0].maxsolutions = _nj0;
3781 vinfos[1].jointtype = 1;
3782 vinfos[1].foffset = j1;
3783 vinfos[1].indices[0] = _ij1[0];
3784 vinfos[1].indices[1] = _ij1[1];
3785 vinfos[1].maxsolutions = _nj1;
3786 vinfos[2].jointtype = 1;
3787 vinfos[2].foffset = j2;
3788 vinfos[2].indices[0] = _ij2[0];
3789 vinfos[2].indices[1] = _ij2[1];
3790 vinfos[2].maxsolutions = _nj2;
3791 vinfos[3].jointtype = 1;
3792 vinfos[3].foffset = j3;
3793 vinfos[3].indices[0] = _ij3[0];
3794 vinfos[3].indices[1] = _ij3[1];
3795 vinfos[3].maxsolutions = _nj3;
3796 vinfos[4].jointtype = 1;
3797 vinfos[4].foffset = j4;
3798 vinfos[4].indices[0] = _ij4[0];
3799 vinfos[4].indices[1] = _ij4[1];
3800 vinfos[4].maxsolutions = _nj4;
3801 vinfos[5].jointtype = 1;
3802 vinfos[5].foffset = j5;
3803 vinfos[5].indices[0] = _ij5[0];
3804 vinfos[5].indices[1] = _ij5[1];
3805 vinfos[5].maxsolutions = _nj5;
3806 std::vector<int> vfree(0);
3807 solutions.AddSolution(vinfos,vfree);
3808 }
3809 }
3810 }
3811 
3812 }
3813 } while(0);
3814 if( bgotonextstatement )
3815 {
3816 bool bgotonextstatement = true;
3817 do
3818 {
3819 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3820 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3821 {
3822 bgotonextstatement=false;
3823 {
3824 IkReal j5array[1], cj5array[1], sj5array[1];
3825 bool j5valid[1]={false};
3826 _nj5 = 1;
3827 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
3828  continue;
3829 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
3830 sj5array[0]=IKsin(j5array[0]);
3831 cj5array[0]=IKcos(j5array[0]);
3832 if( j5array[0] > IKPI )
3833 {
3834  j5array[0]-=IK2PI;
3835 }
3836 else if( j5array[0] < -IKPI )
3837 { j5array[0]+=IK2PI;
3838 }
3839 j5valid[0] = true;
3840 for(int ij5 = 0; ij5 < 1; ++ij5)
3841 {
3842 if( !j5valid[ij5] )
3843 {
3844  continue;
3845 }
3846 _ij5[0] = ij5; _ij5[1] = -1;
3847 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3848 {
3849 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3850 {
3851  j5valid[iij5]=false; _ij5[1] = iij5; break;
3852 }
3853 }
3854 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3855 {
3856 IkReal evalcond[6];
3857 IkReal x284=IKcos(j5);
3858 IkReal x285=IKsin(j5);
3859 IkReal x286=((-1.0)*x285);
3860 IkReal x287=((-1.0)*x284);
3861 evalcond[0]=x286;
3862 evalcond[1]=x287;
3863 evalcond[2]=(new_r22*x287);
3864 evalcond[3]=(new_r22*x286);
3865 evalcond[4]=(x285+(((-1.0)*new_r11)));
3866 evalcond[5]=((((-1.0)*x284))+(((-1.0)*new_r10)));
3867 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
3868 {
3869 continue;
3870 }
3871 }
3872 
3873 {
3874 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3875 vinfos[0].jointtype = 1;
3876 vinfos[0].foffset = j0;
3877 vinfos[0].indices[0] = _ij0[0];
3878 vinfos[0].indices[1] = _ij0[1];
3879 vinfos[0].maxsolutions = _nj0;
3880 vinfos[1].jointtype = 1;
3881 vinfos[1].foffset = j1;
3882 vinfos[1].indices[0] = _ij1[0];
3883 vinfos[1].indices[1] = _ij1[1];
3884 vinfos[1].maxsolutions = _nj1;
3885 vinfos[2].jointtype = 1;
3886 vinfos[2].foffset = j2;
3887 vinfos[2].indices[0] = _ij2[0];
3888 vinfos[2].indices[1] = _ij2[1];
3889 vinfos[2].maxsolutions = _nj2;
3890 vinfos[3].jointtype = 1;
3891 vinfos[3].foffset = j3;
3892 vinfos[3].indices[0] = _ij3[0];
3893 vinfos[3].indices[1] = _ij3[1];
3894 vinfos[3].maxsolutions = _nj3;
3895 vinfos[4].jointtype = 1;
3896 vinfos[4].foffset = j4;
3897 vinfos[4].indices[0] = _ij4[0];
3898 vinfos[4].indices[1] = _ij4[1];
3899 vinfos[4].maxsolutions = _nj4;
3900 vinfos[5].jointtype = 1;
3901 vinfos[5].foffset = j5;
3902 vinfos[5].indices[0] = _ij5[0];
3903 vinfos[5].indices[1] = _ij5[1];
3904 vinfos[5].maxsolutions = _nj5;
3905 std::vector<int> vfree(0);
3906 solutions.AddSolution(vinfos,vfree);
3907 }
3908 }
3909 }
3910 
3911 }
3912 } while(0);
3913 if( bgotonextstatement )
3914 {
3915 bool bgotonextstatement = true;
3916 do
3917 {
3918 if( 1 )
3919 {
3920 bgotonextstatement=false;
3921 continue; // branch miss [j5]
3922 
3923 }
3924 } while(0);
3925 if( bgotonextstatement )
3926 {
3927 }
3928 }
3929 }
3930 }
3931 }
3932 }
3933 
3934 } else
3935 {
3936 {
3937 IkReal j5array[1], cj5array[1], sj5array[1];
3938 bool j5valid[1]={false};
3939 _nj5 = 1;
3940 CheckValue<IkReal> x288=IKPowWithIntegerCheck(new_r02,-1);
3941 if(!x288.valid){
3942 continue;
3943 }
3944 CheckValue<IkReal> x289=IKPowWithIntegerCheck(cj4,-1);
3945 if(!x289.valid){
3946 continue;
3947 }
3948 if( IKabs(((-1.0)*new_r20*(x288.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x289.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x288.value)))+IKsqr((new_r01*(x289.value)))-1) <= IKFAST_SINCOS_THRESH )
3949  continue;
3950 j5array[0]=IKatan2(((-1.0)*new_r20*(x288.value)), (new_r01*(x289.value)));
3951 sj5array[0]=IKsin(j5array[0]);
3952 cj5array[0]=IKcos(j5array[0]);
3953 if( j5array[0] > IKPI )
3954 {
3955  j5array[0]-=IK2PI;
3956 }
3957 else if( j5array[0] < -IKPI )
3958 { j5array[0]+=IK2PI;
3959 }
3960 j5valid[0] = true;
3961 for(int ij5 = 0; ij5 < 1; ++ij5)
3962 {
3963 if( !j5valid[ij5] )
3964 {
3965  continue;
3966 }
3967 _ij5[0] = ij5; _ij5[1] = -1;
3968 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3969 {
3970 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3971 {
3972  j5valid[iij5]=false; _ij5[1] = iij5; break;
3973 }
3974 }
3975 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3976 {
3977 IkReal evalcond[8];
3978 IkReal x290=IKsin(j5);
3979 IkReal x291=IKcos(j5);
3980 IkReal x292=((1.0)*cj4);
3981 IkReal x293=((1.0)*x291);
3982 evalcond[0]=(((new_r02*x290))+new_r20);
3983 evalcond[1]=(((new_r02*x291))+new_r21);
3984 evalcond[2]=(x290+(((-1.0)*new_r11)));
3985 evalcond[3]=((((-1.0)*x291*x292))+new_r01);
3986 evalcond[4]=((((-1.0)*x290*x292))+new_r00);
3987 evalcond[5]=((((-1.0)*x293))+(((-1.0)*new_r10)));
3988 evalcond[6]=((((-1.0)*x290))+((new_r20*sj4))+((cj4*new_r00)));
3989 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x293))+((new_r21*sj4)));
3990 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3991 {
3992 continue;
3993 }
3994 }
3995 
3996 {
3997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3998 vinfos[0].jointtype = 1;
3999 vinfos[0].foffset = j0;
4000 vinfos[0].indices[0] = _ij0[0];
4001 vinfos[0].indices[1] = _ij0[1];
4002 vinfos[0].maxsolutions = _nj0;
4003 vinfos[1].jointtype = 1;
4004 vinfos[1].foffset = j1;
4005 vinfos[1].indices[0] = _ij1[0];
4006 vinfos[1].indices[1] = _ij1[1];
4007 vinfos[1].maxsolutions = _nj1;
4008 vinfos[2].jointtype = 1;
4009 vinfos[2].foffset = j2;
4010 vinfos[2].indices[0] = _ij2[0];
4011 vinfos[2].indices[1] = _ij2[1];
4012 vinfos[2].maxsolutions = _nj2;
4013 vinfos[3].jointtype = 1;
4014 vinfos[3].foffset = j3;
4015 vinfos[3].indices[0] = _ij3[0];
4016 vinfos[3].indices[1] = _ij3[1];
4017 vinfos[3].maxsolutions = _nj3;
4018 vinfos[4].jointtype = 1;
4019 vinfos[4].foffset = j4;
4020 vinfos[4].indices[0] = _ij4[0];
4021 vinfos[4].indices[1] = _ij4[1];
4022 vinfos[4].maxsolutions = _nj4;
4023 vinfos[5].jointtype = 1;
4024 vinfos[5].foffset = j5;
4025 vinfos[5].indices[0] = _ij5[0];
4026 vinfos[5].indices[1] = _ij5[1];
4027 vinfos[5].maxsolutions = _nj5;
4028 std::vector<int> vfree(0);
4029 solutions.AddSolution(vinfos,vfree);
4030 }
4031 }
4032 }
4033 
4034 }
4035 
4036 }
4037 
4038 } else
4039 {
4040 {
4041 IkReal j5array[1], cj5array[1], sj5array[1];
4042 bool j5valid[1]={false};
4043 _nj5 = 1;
4044 CheckValue<IkReal> x294=IKPowWithIntegerCheck(new_r02,-1);
4045 if(!x294.valid){
4046 continue;
4047 }
4048 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x294.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x294.value)))-1) <= IKFAST_SINCOS_THRESH )
4049  continue;
4050 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x294.value)));
4051 sj5array[0]=IKsin(j5array[0]);
4052 cj5array[0]=IKcos(j5array[0]);
4053 if( j5array[0] > IKPI )
4054 {
4055  j5array[0]-=IK2PI;
4056 }
4057 else if( j5array[0] < -IKPI )
4058 { j5array[0]+=IK2PI;
4059 }
4060 j5valid[0] = true;
4061 for(int ij5 = 0; ij5 < 1; ++ij5)
4062 {
4063 if( !j5valid[ij5] )
4064 {
4065  continue;
4066 }
4067 _ij5[0] = ij5; _ij5[1] = -1;
4068 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4069 {
4070 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4071 {
4072  j5valid[iij5]=false; _ij5[1] = iij5; break;
4073 }
4074 }
4075 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4076 {
4077 IkReal evalcond[8];
4078 IkReal x295=IKsin(j5);
4079 IkReal x296=IKcos(j5);
4080 IkReal x297=((1.0)*cj4);
4081 IkReal x298=((1.0)*x296);
4082 evalcond[0]=(((new_r02*x295))+new_r20);
4083 evalcond[1]=(((new_r02*x296))+new_r21);
4084 evalcond[2]=(x295+(((-1.0)*new_r11)));
4085 evalcond[3]=((((-1.0)*x296*x297))+new_r01);
4086 evalcond[4]=((((-1.0)*x295*x297))+new_r00);
4087 evalcond[5]=((((-1.0)*x298))+(((-1.0)*new_r10)));
4088 evalcond[6]=((((-1.0)*x295))+((new_r20*sj4))+((cj4*new_r00)));
4089 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x298))+((new_r21*sj4)));
4090 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4091 {
4092 continue;
4093 }
4094 }
4095 
4096 {
4097 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4098 vinfos[0].jointtype = 1;
4099 vinfos[0].foffset = j0;
4100 vinfos[0].indices[0] = _ij0[0];
4101 vinfos[0].indices[1] = _ij0[1];
4102 vinfos[0].maxsolutions = _nj0;
4103 vinfos[1].jointtype = 1;
4104 vinfos[1].foffset = j1;
4105 vinfos[1].indices[0] = _ij1[0];
4106 vinfos[1].indices[1] = _ij1[1];
4107 vinfos[1].maxsolutions = _nj1;
4108 vinfos[2].jointtype = 1;
4109 vinfos[2].foffset = j2;
4110 vinfos[2].indices[0] = _ij2[0];
4111 vinfos[2].indices[1] = _ij2[1];
4112 vinfos[2].maxsolutions = _nj2;
4113 vinfos[3].jointtype = 1;
4114 vinfos[3].foffset = j3;
4115 vinfos[3].indices[0] = _ij3[0];
4116 vinfos[3].indices[1] = _ij3[1];
4117 vinfos[3].maxsolutions = _nj3;
4118 vinfos[4].jointtype = 1;
4119 vinfos[4].foffset = j4;
4120 vinfos[4].indices[0] = _ij4[0];
4121 vinfos[4].indices[1] = _ij4[1];
4122 vinfos[4].maxsolutions = _nj4;
4123 vinfos[5].jointtype = 1;
4124 vinfos[5].foffset = j5;
4125 vinfos[5].indices[0] = _ij5[0];
4126 vinfos[5].indices[1] = _ij5[1];
4127 vinfos[5].maxsolutions = _nj5;
4128 std::vector<int> vfree(0);
4129 solutions.AddSolution(vinfos,vfree);
4130 }
4131 }
4132 }
4133 
4134 }
4135 
4136 }
4137 
4138 } else
4139 {
4140 {
4141 IkReal j5array[1], cj5array[1], sj5array[1];
4142 bool j5valid[1]={false};
4143 _nj5 = 1;
4144 CheckValue<IkReal> x299=IKPowWithIntegerCheck(IKsign(new_r02),-1);
4145 if(!x299.valid){
4146 continue;
4147 }
4148 CheckValue<IkReal> x300 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH);
4149 if(!x300.valid){
4150 continue;
4151 }
4152 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x299.value)))+(x300.value));
4153 sj5array[0]=IKsin(j5array[0]);
4154 cj5array[0]=IKcos(j5array[0]);
4155 if( j5array[0] > IKPI )
4156 {
4157  j5array[0]-=IK2PI;
4158 }
4159 else if( j5array[0] < -IKPI )
4160 { j5array[0]+=IK2PI;
4161 }
4162 j5valid[0] = true;
4163 for(int ij5 = 0; ij5 < 1; ++ij5)
4164 {
4165 if( !j5valid[ij5] )
4166 {
4167  continue;
4168 }
4169 _ij5[0] = ij5; _ij5[1] = -1;
4170 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4171 {
4172 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4173 {
4174  j5valid[iij5]=false; _ij5[1] = iij5; break;
4175 }
4176 }
4177 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4178 {
4179 IkReal evalcond[8];
4180 IkReal x301=IKsin(j5);
4181 IkReal x302=IKcos(j5);
4182 IkReal x303=((1.0)*cj4);
4183 IkReal x304=((1.0)*x302);
4184 evalcond[0]=(((new_r02*x301))+new_r20);
4185 evalcond[1]=(((new_r02*x302))+new_r21);
4186 evalcond[2]=(x301+(((-1.0)*new_r11)));
4187 evalcond[3]=((((-1.0)*x302*x303))+new_r01);
4188 evalcond[4]=((((-1.0)*x301*x303))+new_r00);
4189 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x304)));
4190 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x301)));
4191 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x304)));
4192 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4193 {
4194 continue;
4195 }
4196 }
4197 
4198 {
4199 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4200 vinfos[0].jointtype = 1;
4201 vinfos[0].foffset = j0;
4202 vinfos[0].indices[0] = _ij0[0];
4203 vinfos[0].indices[1] = _ij0[1];
4204 vinfos[0].maxsolutions = _nj0;
4205 vinfos[1].jointtype = 1;
4206 vinfos[1].foffset = j1;
4207 vinfos[1].indices[0] = _ij1[0];
4208 vinfos[1].indices[1] = _ij1[1];
4209 vinfos[1].maxsolutions = _nj1;
4210 vinfos[2].jointtype = 1;
4211 vinfos[2].foffset = j2;
4212 vinfos[2].indices[0] = _ij2[0];
4213 vinfos[2].indices[1] = _ij2[1];
4214 vinfos[2].maxsolutions = _nj2;
4215 vinfos[3].jointtype = 1;
4216 vinfos[3].foffset = j3;
4217 vinfos[3].indices[0] = _ij3[0];
4218 vinfos[3].indices[1] = _ij3[1];
4219 vinfos[3].maxsolutions = _nj3;
4220 vinfos[4].jointtype = 1;
4221 vinfos[4].foffset = j4;
4222 vinfos[4].indices[0] = _ij4[0];
4223 vinfos[4].indices[1] = _ij4[1];
4224 vinfos[4].maxsolutions = _nj4;
4225 vinfos[5].jointtype = 1;
4226 vinfos[5].foffset = j5;
4227 vinfos[5].indices[0] = _ij5[0];
4228 vinfos[5].indices[1] = _ij5[1];
4229 vinfos[5].maxsolutions = _nj5;
4230 std::vector<int> vfree(0);
4231 solutions.AddSolution(vinfos,vfree);
4232 }
4233 }
4234 }
4235 
4236 }
4237 
4238 }
4239 
4240 }
4241 } while(0);
4242 if( bgotonextstatement )
4243 {
4244 bool bgotonextstatement = true;
4245 do
4246 {
4247 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4248 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4249 {
4250 bgotonextstatement=false;
4251 {
4252 IkReal j5eval[1];
4253 new_r21=0;
4254 new_r20=0;
4255 new_r02=0;
4256 new_r12=0;
4257 j5eval[0]=1.0;
4258 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4259 {
4260 continue; // no branches [j5]
4261 
4262 } else
4263 {
4264 IkReal op[2+1], zeror[2];
4265 int numroots;
4266 op[0]=1.0;
4267 op[1]=0;
4268 op[2]=-1.0;
4269 polyroots2(op,zeror,numroots);
4270 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4271 int numsolutions = 0;
4272 for(int ij5 = 0; ij5 < numroots; ++ij5)
4273 {
4274 IkReal htj5 = zeror[ij5];
4275 tempj5array[0]=((2.0)*(atan(htj5)));
4276 for(int kj5 = 0; kj5 < 1; ++kj5)
4277 {
4278 j5array[numsolutions] = tempj5array[kj5];
4279 if( j5array[numsolutions] > IKPI )
4280 {
4281  j5array[numsolutions]-=IK2PI;
4282 }
4283 else if( j5array[numsolutions] < -IKPI )
4284 {
4285  j5array[numsolutions]+=IK2PI;
4286 }
4287 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4288 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4289 numsolutions++;
4290 }
4291 }
4292 bool j5valid[2]={true,true};
4293 _nj5 = 2;
4294 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4295  {
4296 if( !j5valid[ij5] )
4297 {
4298  continue;
4299 }
4300  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4301 htj5 = IKtan(j5/2);
4302 
4303 _ij5[0] = ij5; _ij5[1] = -1;
4304 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4305 {
4306 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4307 {
4308  j5valid[iij5]=false; _ij5[1] = iij5; break;
4309 }
4310 }
4311 {
4312 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4313 vinfos[0].jointtype = 1;
4314 vinfos[0].foffset = j0;
4315 vinfos[0].indices[0] = _ij0[0];
4316 vinfos[0].indices[1] = _ij0[1];
4317 vinfos[0].maxsolutions = _nj0;
4318 vinfos[1].jointtype = 1;
4319 vinfos[1].foffset = j1;
4320 vinfos[1].indices[0] = _ij1[0];
4321 vinfos[1].indices[1] = _ij1[1];
4322 vinfos[1].maxsolutions = _nj1;
4323 vinfos[2].jointtype = 1;
4324 vinfos[2].foffset = j2;
4325 vinfos[2].indices[0] = _ij2[0];
4326 vinfos[2].indices[1] = _ij2[1];
4327 vinfos[2].maxsolutions = _nj2;
4328 vinfos[3].jointtype = 1;
4329 vinfos[3].foffset = j3;
4330 vinfos[3].indices[0] = _ij3[0];
4331 vinfos[3].indices[1] = _ij3[1];
4332 vinfos[3].maxsolutions = _nj3;
4333 vinfos[4].jointtype = 1;
4334 vinfos[4].foffset = j4;
4335 vinfos[4].indices[0] = _ij4[0];
4336 vinfos[4].indices[1] = _ij4[1];
4337 vinfos[4].maxsolutions = _nj4;
4338 vinfos[5].jointtype = 1;
4339 vinfos[5].foffset = j5;
4340 vinfos[5].indices[0] = _ij5[0];
4341 vinfos[5].indices[1] = _ij5[1];
4342 vinfos[5].maxsolutions = _nj5;
4343 std::vector<int> vfree(0);
4344 solutions.AddSolution(vinfos,vfree);
4345 }
4346  }
4347 
4348 }
4349 
4350 }
4351 
4352 }
4353 } while(0);
4354 if( bgotonextstatement )
4355 {
4356 bool bgotonextstatement = true;
4357 do
4358 {
4359 if( 1 )
4360 {
4361 bgotonextstatement=false;
4362 continue; // branch miss [j5]
4363 
4364 }
4365 } while(0);
4366 if( bgotonextstatement )
4367 {
4368 }
4369 }
4370 }
4371 }
4372 }
4373 }
4374 }
4375 }
4376 }
4377 }
4378 }
4379 
4380 } else
4381 {
4382 {
4383 IkReal j5array[1], cj5array[1], sj5array[1];
4384 bool j5valid[1]={false};
4385 _nj5 = 1;
4386 CheckValue<IkReal> x306=IKPowWithIntegerCheck(sj4,-1);
4387 if(!x306.valid){
4388 continue;
4389 }
4390 IkReal x305=x306.value;
4391 CheckValue<IkReal> x307=IKPowWithIntegerCheck(cj4,-1);
4392 if(!x307.valid){
4393 continue;
4394 }
4395 CheckValue<IkReal> x308=IKPowWithIntegerCheck(sj3,-1);
4396 if(!x308.valid){
4397 continue;
4398 }
4399 if( IKabs((new_r20*x305)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x305))+IKsqr((x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))))-1) <= IKFAST_SINCOS_THRESH )
4400  continue;
4401 j5array[0]=IKatan2((new_r20*x305), (x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))));
4402 sj5array[0]=IKsin(j5array[0]);
4403 cj5array[0]=IKcos(j5array[0]);
4404 if( j5array[0] > IKPI )
4405 {
4406  j5array[0]-=IK2PI;
4407 }
4408 else if( j5array[0] < -IKPI )
4409 { j5array[0]+=IK2PI;
4410 }
4411 j5valid[0] = true;
4412 for(int ij5 = 0; ij5 < 1; ++ij5)
4413 {
4414 if( !j5valid[ij5] )
4415 {
4416  continue;
4417 }
4418 _ij5[0] = ij5; _ij5[1] = -1;
4419 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4420 {
4421 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4422 {
4423  j5valid[iij5]=false; _ij5[1] = iij5; break;
4424 }
4425 }
4426 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4427 {
4428 IkReal evalcond[12];
4429 IkReal x309=IKsin(j5);
4430 IkReal x310=IKcos(j5);
4431 IkReal x311=(cj4*sj3);
4432 IkReal x312=(cj3*new_r10);
4433 IkReal x313=((1.0)*new_r01);
4434 IkReal x314=(cj3*new_r11);
4435 IkReal x315=((1.0)*new_r00);
4436 IkReal x316=((1.0)*x310);
4437 IkReal x317=(cj4*x309);
4438 IkReal x318=((1.0)*x309);
4439 evalcond[0]=((((-1.0)*sj4*x318))+new_r20);
4440 evalcond[1]=((((-1.0)*sj4*x316))+new_r21);
4441 evalcond[2]=(((new_r11*sj3))+x309+((cj3*new_r01)));
4442 evalcond[3]=(((new_r10*sj3))+(((-1.0)*x316))+((cj3*new_r00)));
4443 evalcond[4]=(((x310*x311))+((cj3*x309))+new_r01);
4444 evalcond[5]=(((x309*x311))+new_r00+(((-1.0)*cj3*x316)));
4445 evalcond[6]=(((sj3*x309))+(((-1.0)*cj3*cj4*x316))+new_r11);
4446 evalcond[7]=((((-1.0)*sj3*x315))+x312+(((-1.0)*x317)));
4447 evalcond[8]=((((-1.0)*sj3*x313))+(((-1.0)*cj4*x316))+x314);
4448 evalcond[9]=((((-1.0)*cj3*x317))+(((-1.0)*sj3*x316))+new_r10);
4449 evalcond[10]=(((new_r20*sj4))+((cj4*x312))+(((-1.0)*x318))+(((-1.0)*x311*x315)));
4450 evalcond[11]=(((cj4*x314))+(((-1.0)*x316))+((new_r21*sj4))+(((-1.0)*x311*x313)));
4451 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4452 {
4453 continue;
4454 }
4455 }
4456 
4457 {
4458 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4459 vinfos[0].jointtype = 1;
4460 vinfos[0].foffset = j0;
4461 vinfos[0].indices[0] = _ij0[0];
4462 vinfos[0].indices[1] = _ij0[1];
4463 vinfos[0].maxsolutions = _nj0;
4464 vinfos[1].jointtype = 1;
4465 vinfos[1].foffset = j1;
4466 vinfos[1].indices[0] = _ij1[0];
4467 vinfos[1].indices[1] = _ij1[1];
4468 vinfos[1].maxsolutions = _nj1;
4469 vinfos[2].jointtype = 1;
4470 vinfos[2].foffset = j2;
4471 vinfos[2].indices[0] = _ij2[0];
4472 vinfos[2].indices[1] = _ij2[1];
4473 vinfos[2].maxsolutions = _nj2;
4474 vinfos[3].jointtype = 1;
4475 vinfos[3].foffset = j3;
4476 vinfos[3].indices[0] = _ij3[0];
4477 vinfos[3].indices[1] = _ij3[1];
4478 vinfos[3].maxsolutions = _nj3;
4479 vinfos[4].jointtype = 1;
4480 vinfos[4].foffset = j4;
4481 vinfos[4].indices[0] = _ij4[0];
4482 vinfos[4].indices[1] = _ij4[1];
4483 vinfos[4].maxsolutions = _nj4;
4484 vinfos[5].jointtype = 1;
4485 vinfos[5].foffset = j5;
4486 vinfos[5].indices[0] = _ij5[0];
4487 vinfos[5].indices[1] = _ij5[1];
4488 vinfos[5].maxsolutions = _nj5;
4489 std::vector<int> vfree(0);
4490 solutions.AddSolution(vinfos,vfree);
4491 }
4492 }
4493 }
4494 
4495 }
4496 
4497 }
4498 
4499 } else
4500 {
4501 {
4502 IkReal j5array[1], cj5array[1], sj5array[1];
4503 bool j5valid[1]={false};
4504 _nj5 = 1;
4505 CheckValue<IkReal> x320=IKPowWithIntegerCheck(sj4,-1);
4506 if(!x320.valid){
4507 continue;
4508 }
4509 IkReal x319=x320.value;
4510 CheckValue<IkReal> x321=IKPowWithIntegerCheck(cj3,-1);
4511 if(!x321.valid){
4512 continue;
4513 }
4514 if( IKabs((new_r20*x319)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x319))+IKsqr((x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4515  continue;
4516 j5array[0]=IKatan2((new_r20*x319), (x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))));
4517 sj5array[0]=IKsin(j5array[0]);
4518 cj5array[0]=IKcos(j5array[0]);
4519 if( j5array[0] > IKPI )
4520 {
4521  j5array[0]-=IK2PI;
4522 }
4523 else if( j5array[0] < -IKPI )
4524 { j5array[0]+=IK2PI;
4525 }
4526 j5valid[0] = true;
4527 for(int ij5 = 0; ij5 < 1; ++ij5)
4528 {
4529 if( !j5valid[ij5] )
4530 {
4531  continue;
4532 }
4533 _ij5[0] = ij5; _ij5[1] = -1;
4534 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4535 {
4536 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4537 {
4538  j5valid[iij5]=false; _ij5[1] = iij5; break;
4539 }
4540 }
4541 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4542 {
4543 IkReal evalcond[12];
4544 IkReal x322=IKsin(j5);
4545 IkReal x323=IKcos(j5);
4546 IkReal x324=(cj4*sj3);
4547 IkReal x325=(cj3*new_r10);
4548 IkReal x326=((1.0)*new_r01);
4549 IkReal x327=(cj3*new_r11);
4550 IkReal x328=((1.0)*new_r00);
4551 IkReal x329=((1.0)*x323);
4552 IkReal x330=(cj4*x322);
4553 IkReal x331=((1.0)*x322);
4554 evalcond[0]=((((-1.0)*sj4*x331))+new_r20);
4555 evalcond[1]=(new_r21+(((-1.0)*sj4*x329)));
4556 evalcond[2]=(((new_r11*sj3))+x322+((cj3*new_r01)));
4557 evalcond[3]=(((new_r10*sj3))+(((-1.0)*x329))+((cj3*new_r00)));
4558 evalcond[4]=(((x323*x324))+new_r01+((cj3*x322)));
4559 evalcond[5]=(((x322*x324))+(((-1.0)*cj3*x329))+new_r00);
4560 evalcond[6]=(((sj3*x322))+new_r11+(((-1.0)*cj3*cj4*x329)));
4561 evalcond[7]=(x325+(((-1.0)*x330))+(((-1.0)*sj3*x328)));
4562 evalcond[8]=(x327+(((-1.0)*cj4*x329))+(((-1.0)*sj3*x326)));
4563 evalcond[9]=((((-1.0)*cj3*x330))+new_r10+(((-1.0)*sj3*x329)));
4564 evalcond[10]=(((new_r20*sj4))+((cj4*x325))+(((-1.0)*x331))+(((-1.0)*x324*x328)));
4565 evalcond[11]=((((-1.0)*x329))+((cj4*x327))+((new_r21*sj4))+(((-1.0)*x324*x326)));
4566 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4567 {
4568 continue;
4569 }
4570 }
4571 
4572 {
4573 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4574 vinfos[0].jointtype = 1;
4575 vinfos[0].foffset = j0;
4576 vinfos[0].indices[0] = _ij0[0];
4577 vinfos[0].indices[1] = _ij0[1];
4578 vinfos[0].maxsolutions = _nj0;
4579 vinfos[1].jointtype = 1;
4580 vinfos[1].foffset = j1;
4581 vinfos[1].indices[0] = _ij1[0];
4582 vinfos[1].indices[1] = _ij1[1];
4583 vinfos[1].maxsolutions = _nj1;
4584 vinfos[2].jointtype = 1;
4585 vinfos[2].foffset = j2;
4586 vinfos[2].indices[0] = _ij2[0];
4587 vinfos[2].indices[1] = _ij2[1];
4588 vinfos[2].maxsolutions = _nj2;
4589 vinfos[3].jointtype = 1;
4590 vinfos[3].foffset = j3;
4591 vinfos[3].indices[0] = _ij3[0];
4592 vinfos[3].indices[1] = _ij3[1];
4593 vinfos[3].maxsolutions = _nj3;
4594 vinfos[4].jointtype = 1;
4595 vinfos[4].foffset = j4;
4596 vinfos[4].indices[0] = _ij4[0];
4597 vinfos[4].indices[1] = _ij4[1];
4598 vinfos[4].maxsolutions = _nj4;
4599 vinfos[5].jointtype = 1;
4600 vinfos[5].foffset = j5;
4601 vinfos[5].indices[0] = _ij5[0];
4602 vinfos[5].indices[1] = _ij5[1];
4603 vinfos[5].maxsolutions = _nj5;
4604 std::vector<int> vfree(0);
4605 solutions.AddSolution(vinfos,vfree);
4606 }
4607 }
4608 }
4609 
4610 }
4611 
4612 }
4613 
4614 } else
4615 {
4616 {
4617 IkReal j5array[1], cj5array[1], sj5array[1];
4618 bool j5valid[1]={false};
4619 _nj5 = 1;
4620 CheckValue<IkReal> x332=IKPowWithIntegerCheck(IKsign(sj4),-1);
4621 if(!x332.valid){
4622 continue;
4623 }
4624 CheckValue<IkReal> x333 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
4625 if(!x333.valid){
4626 continue;
4627 }
4628 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x332.value)))+(x333.value));
4629 sj5array[0]=IKsin(j5array[0]);
4630 cj5array[0]=IKcos(j5array[0]);
4631 if( j5array[0] > IKPI )
4632 {
4633  j5array[0]-=IK2PI;
4634 }
4635 else if( j5array[0] < -IKPI )
4636 { j5array[0]+=IK2PI;
4637 }
4638 j5valid[0] = true;
4639 for(int ij5 = 0; ij5 < 1; ++ij5)
4640 {
4641 if( !j5valid[ij5] )
4642 {
4643  continue;
4644 }
4645 _ij5[0] = ij5; _ij5[1] = -1;
4646 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4647 {
4648 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4649 {
4650  j5valid[iij5]=false; _ij5[1] = iij5; break;
4651 }
4652 }
4653 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4654 {
4655 IkReal evalcond[12];
4656 IkReal x334=IKsin(j5);
4657 IkReal x335=IKcos(j5);
4658 IkReal x336=(cj4*sj3);
4659 IkReal x337=(cj3*new_r10);
4660 IkReal x338=((1.0)*new_r01);
4661 IkReal x339=(cj3*new_r11);
4662 IkReal x340=((1.0)*new_r00);
4663 IkReal x341=((1.0)*x335);
4664 IkReal x342=(cj4*x334);
4665 IkReal x343=((1.0)*x334);
4666 evalcond[0]=((((-1.0)*sj4*x343))+new_r20);
4667 evalcond[1]=((((-1.0)*sj4*x341))+new_r21);
4668 evalcond[2]=(((new_r11*sj3))+x334+((cj3*new_r01)));
4669 evalcond[3]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x341)));
4670 evalcond[4]=(((x335*x336))+((cj3*x334))+new_r01);
4671 evalcond[5]=((((-1.0)*cj3*x341))+((x334*x336))+new_r00);
4672 evalcond[6]=(((sj3*x334))+(((-1.0)*cj3*cj4*x341))+new_r11);
4673 evalcond[7]=(x337+(((-1.0)*sj3*x340))+(((-1.0)*x342)));
4674 evalcond[8]=((((-1.0)*sj3*x338))+(((-1.0)*cj4*x341))+x339);
4675 evalcond[9]=((((-1.0)*cj3*x342))+new_r10+(((-1.0)*sj3*x341)));
4676 evalcond[10]=(((new_r20*sj4))+(((-1.0)*x336*x340))+((cj4*x337))+(((-1.0)*x343)));
4677 evalcond[11]=(((cj4*x339))+((new_r21*sj4))+(((-1.0)*x341))+(((-1.0)*x336*x338)));
4678 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4679 {
4680 continue;
4681 }
4682 }
4683 
4684 {
4685 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4686 vinfos[0].jointtype = 1;
4687 vinfos[0].foffset = j0;
4688 vinfos[0].indices[0] = _ij0[0];
4689 vinfos[0].indices[1] = _ij0[1];
4690 vinfos[0].maxsolutions = _nj0;
4691 vinfos[1].jointtype = 1;
4692 vinfos[1].foffset = j1;
4693 vinfos[1].indices[0] = _ij1[0];
4694 vinfos[1].indices[1] = _ij1[1];
4695 vinfos[1].maxsolutions = _nj1;
4696 vinfos[2].jointtype = 1;
4697 vinfos[2].foffset = j2;
4698 vinfos[2].indices[0] = _ij2[0];
4699 vinfos[2].indices[1] = _ij2[1];
4700 vinfos[2].maxsolutions = _nj2;
4701 vinfos[3].jointtype = 1;
4702 vinfos[3].foffset = j3;
4703 vinfos[3].indices[0] = _ij3[0];
4704 vinfos[3].indices[1] = _ij3[1];
4705 vinfos[3].maxsolutions = _nj3;
4706 vinfos[4].jointtype = 1;
4707 vinfos[4].foffset = j4;
4708 vinfos[4].indices[0] = _ij4[0];
4709 vinfos[4].indices[1] = _ij4[1];
4710 vinfos[4].maxsolutions = _nj4;
4711 vinfos[5].jointtype = 1;
4712 vinfos[5].foffset = j5;
4713 vinfos[5].indices[0] = _ij5[0];
4714 vinfos[5].indices[1] = _ij5[1];
4715 vinfos[5].maxsolutions = _nj5;
4716 std::vector<int> vfree(0);
4717 solutions.AddSolution(vinfos,vfree);
4718 }
4719 }
4720 }
4721 
4722 }
4723 
4724 }
4725 }
4726 }
4727 
4728 }
4729 
4730 }
4731 
4732 } else
4733 {
4734 {
4735 IkReal j3array[1], cj3array[1], sj3array[1];
4736 bool j3valid[1]={false};
4737 _nj3 = 1;
4738 CheckValue<IkReal> x344=IKPowWithIntegerCheck(IKsign(sj4),-1);
4739 if(!x344.valid){
4740 continue;
4741 }
4742 CheckValue<IkReal> x345 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH);
4743 if(!x345.valid){
4744 continue;
4745 }
4746 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x344.value)))+(x345.value));
4747 sj3array[0]=IKsin(j3array[0]);
4748 cj3array[0]=IKcos(j3array[0]);
4749 if( j3array[0] > IKPI )
4750 {
4751  j3array[0]-=IK2PI;
4752 }
4753 else if( j3array[0] < -IKPI )
4754 { j3array[0]+=IK2PI;
4755 }
4756 j3valid[0] = true;
4757 for(int ij3 = 0; ij3 < 1; ++ij3)
4758 {
4759 if( !j3valid[ij3] )
4760 {
4761  continue;
4762 }
4763 _ij3[0] = ij3; _ij3[1] = -1;
4764 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
4765 {
4766 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
4767 {
4768  j3valid[iij3]=false; _ij3[1] = iij3; break;
4769 }
4770 }
4771 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
4772 {
4773 IkReal evalcond[8];
4774 IkReal x346=IKsin(j3);
4775 IkReal x347=IKcos(j3);
4776 IkReal x348=((1.0)*cj4);
4777 IkReal x349=((1.0)*sj4);
4778 IkReal x350=(new_r02*x346);
4779 IkReal x351=(new_r12*x347);
4780 IkReal x352=(sj4*x347);
4781 evalcond[0]=(x352+new_r12);
4782 evalcond[1]=((((-1.0)*x346*x349))+new_r02);
4783 evalcond[2]=(((new_r02*x347))+((new_r12*x346)));
4784 evalcond[3]=(sj4+(((-1.0)*x350))+x351);
4785 evalcond[4]=((((-1.0)*x348*x350))+((cj4*x351))+((new_r22*sj4)));
4786 evalcond[5]=((((-1.0)*new_r00*x346*x349))+(((-1.0)*new_r20*x348))+((new_r10*x352)));
4787 evalcond[6]=((((-1.0)*new_r01*x346*x349))+(((-1.0)*new_r21*x348))+((new_r11*x352)));
4788 evalcond[7]=((1.0)+((sj4*x351))+(((-1.0)*x349*x350))+(((-1.0)*new_r22*x348)));
4789 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4790 {
4791 continue;
4792 }
4793 }
4794 
4795 {
4796 IkReal j5eval[3];
4797 j5eval[0]=sj4;
4798 j5eval[1]=IKsign(sj4);
4799 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4800 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4801 {
4802 {
4803 IkReal j5eval[2];
4804 j5eval[0]=sj4;
4805 j5eval[1]=cj3;
4806 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4807 {
4808 {
4809 IkReal j5eval[3];
4810 j5eval[0]=sj4;
4811 j5eval[1]=cj4;
4812 j5eval[2]=sj3;
4813 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4814 {
4815 {
4816 IkReal evalcond[5];
4817 bool bgotonextstatement = true;
4818 do
4819 {
4820 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4821 evalcond[1]=new_r12;
4822 evalcond[2]=new_r02;
4823 evalcond[3]=new_r20;
4824 evalcond[4]=new_r21;
4825 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 )
4826 {
4827 bgotonextstatement=false;
4828 {
4829 IkReal j5array[1], cj5array[1], sj5array[1];
4830 bool j5valid[1]={false};
4831 _nj5 = 1;
4832 IkReal x353=((1.0)*new_r01);
4833 if( IKabs(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r00))+(((-1.0)*sj3*x353)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3))))+IKsqr((((cj3*new_r00))+(((-1.0)*sj3*x353))))-1) <= IKFAST_SINCOS_THRESH )
4834  continue;
4835 j5array[0]=IKatan2(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3))), (((cj3*new_r00))+(((-1.0)*sj3*x353))));
4836 sj5array[0]=IKsin(j5array[0]);
4837 cj5array[0]=IKcos(j5array[0]);
4838 if( j5array[0] > IKPI )
4839 {
4840  j5array[0]-=IK2PI;
4841 }
4842 else if( j5array[0] < -IKPI )
4843 { j5array[0]+=IK2PI;
4844 }
4845 j5valid[0] = true;
4846 for(int ij5 = 0; ij5 < 1; ++ij5)
4847 {
4848 if( !j5valid[ij5] )
4849 {
4850  continue;
4851 }
4852 _ij5[0] = ij5; _ij5[1] = -1;
4853 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4854 {
4855 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4856 {
4857  j5valid[iij5]=false; _ij5[1] = iij5; break;
4858 }
4859 }
4860 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4861 {
4862 IkReal evalcond[8];
4863 IkReal x354=IKsin(j5);
4864 IkReal x355=IKcos(j5);
4865 IkReal x356=((1.0)*sj3);
4866 IkReal x357=(sj3*x354);
4867 IkReal x358=((1.0)*x355);
4868 IkReal x359=((1.0)*x354);
4869 IkReal x360=(cj3*x358);
4870 evalcond[0]=(((new_r11*sj3))+x354+((cj3*new_r01)));
4871 evalcond[1]=(((new_r10*sj3))+(((-1.0)*x358))+((cj3*new_r00)));
4872 evalcond[2]=(((cj3*x354))+((sj3*x355))+new_r01);
4873 evalcond[3]=((((-1.0)*x360))+x357+new_r00);
4874 evalcond[4]=((((-1.0)*x360))+x357+new_r11);
4875 evalcond[5]=((((-1.0)*x359))+((cj3*new_r10))+(((-1.0)*new_r00*x356)));
4876 evalcond[6]=((((-1.0)*x358))+(((-1.0)*new_r01*x356))+((cj3*new_r11)));
4877 evalcond[7]=((((-1.0)*cj3*x359))+new_r10+(((-1.0)*x355*x356)));
4878 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4879 {
4880 continue;
4881 }
4882 }
4883 
4884 {
4885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4886 vinfos[0].jointtype = 1;
4887 vinfos[0].foffset = j0;
4888 vinfos[0].indices[0] = _ij0[0];
4889 vinfos[0].indices[1] = _ij0[1];
4890 vinfos[0].maxsolutions = _nj0;
4891 vinfos[1].jointtype = 1;
4892 vinfos[1].foffset = j1;
4893 vinfos[1].indices[0] = _ij1[0];
4894 vinfos[1].indices[1] = _ij1[1];
4895 vinfos[1].maxsolutions = _nj1;
4896 vinfos[2].jointtype = 1;
4897 vinfos[2].foffset = j2;
4898 vinfos[2].indices[0] = _ij2[0];
4899 vinfos[2].indices[1] = _ij2[1];
4900 vinfos[2].maxsolutions = _nj2;
4901 vinfos[3].jointtype = 1;
4902 vinfos[3].foffset = j3;
4903 vinfos[3].indices[0] = _ij3[0];
4904 vinfos[3].indices[1] = _ij3[1];
4905 vinfos[3].maxsolutions = _nj3;
4906 vinfos[4].jointtype = 1;
4907 vinfos[4].foffset = j4;
4908 vinfos[4].indices[0] = _ij4[0];
4909 vinfos[4].indices[1] = _ij4[1];
4910 vinfos[4].maxsolutions = _nj4;
4911 vinfos[5].jointtype = 1;
4912 vinfos[5].foffset = j5;
4913 vinfos[5].indices[0] = _ij5[0];
4914 vinfos[5].indices[1] = _ij5[1];
4915 vinfos[5].maxsolutions = _nj5;
4916 std::vector<int> vfree(0);
4917 solutions.AddSolution(vinfos,vfree);
4918 }
4919 }
4920 }
4921 
4922 }
4923 } while(0);
4924 if( bgotonextstatement )
4925 {
4926 bool bgotonextstatement = true;
4927 do
4928 {
4929 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4930 evalcond[1]=new_r12;
4931 evalcond[2]=new_r02;
4932 evalcond[3]=new_r20;
4933 evalcond[4]=new_r21;
4934 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 )
4935 {
4936 bgotonextstatement=false;
4937 {
4938 IkReal j5array[1], cj5array[1], sj5array[1];
4939 bool j5valid[1]={false};
4940 _nj5 = 1;
4941 IkReal x361=((1.0)*cj3);
4942 if( IKabs(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj3))+(((-1.0)*new_r11*x361)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361))))+IKsqr((((new_r01*sj3))+(((-1.0)*new_r11*x361))))-1) <= IKFAST_SINCOS_THRESH )
4943  continue;
4944 j5array[0]=IKatan2(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361))), (((new_r01*sj3))+(((-1.0)*new_r11*x361))));
4945 sj5array[0]=IKsin(j5array[0]);
4946 cj5array[0]=IKcos(j5array[0]);
4947 if( j5array[0] > IKPI )
4948 {
4949  j5array[0]-=IK2PI;
4950 }
4951 else if( j5array[0] < -IKPI )
4952 { j5array[0]+=IK2PI;
4953 }
4954 j5valid[0] = true;
4955 for(int ij5 = 0; ij5 < 1; ++ij5)
4956 {
4957 if( !j5valid[ij5] )
4958 {
4959  continue;
4960 }
4961 _ij5[0] = ij5; _ij5[1] = -1;
4962 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4963 {
4964 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4965 {
4966  j5valid[iij5]=false; _ij5[1] = iij5; break;
4967 }
4968 }
4969 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4970 {
4971 IkReal evalcond[8];
4972 IkReal x362=IKsin(j5);
4973 IkReal x363=IKcos(j5);
4974 IkReal x364=((1.0)*sj3);
4975 IkReal x365=(cj3*x362);
4976 IkReal x366=((1.0)*x363);
4977 IkReal x367=(x363*x364);
4978 evalcond[0]=(((new_r11*sj3))+x362+((cj3*new_r01)));
4979 evalcond[1]=((((-1.0)*new_r00*x364))+x362+((cj3*new_r10)));
4980 evalcond[2]=(x363+((cj3*new_r11))+(((-1.0)*new_r01*x364)));
4981 evalcond[3]=(((new_r10*sj3))+(((-1.0)*x366))+((cj3*new_r00)));
4982 evalcond[4]=(((sj3*x362))+new_r11+((cj3*x363)));
4983 evalcond[5]=((((-1.0)*x367))+x365+new_r01);
4984 evalcond[6]=((((-1.0)*x367))+x365+new_r10);
4985 evalcond[7]=((((-1.0)*x362*x364))+new_r00+(((-1.0)*cj3*x366)));
4986 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4987 {
4988 continue;
4989 }
4990 }
4991 
4992 {
4993 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4994 vinfos[0].jointtype = 1;
4995 vinfos[0].foffset = j0;
4996 vinfos[0].indices[0] = _ij0[0];
4997 vinfos[0].indices[1] = _ij0[1];
4998 vinfos[0].maxsolutions = _nj0;
4999 vinfos[1].jointtype = 1;
5000 vinfos[1].foffset = j1;
5001 vinfos[1].indices[0] = _ij1[0];
5002 vinfos[1].indices[1] = _ij1[1];
5003 vinfos[1].maxsolutions = _nj1;
5004 vinfos[2].jointtype = 1;
5005 vinfos[2].foffset = j2;
5006 vinfos[2].indices[0] = _ij2[0];
5007 vinfos[2].indices[1] = _ij2[1];
5008 vinfos[2].maxsolutions = _nj2;
5009 vinfos[3].jointtype = 1;
5010 vinfos[3].foffset = j3;
5011 vinfos[3].indices[0] = _ij3[0];
5012 vinfos[3].indices[1] = _ij3[1];
5013 vinfos[3].maxsolutions = _nj3;
5014 vinfos[4].jointtype = 1;
5015 vinfos[4].foffset = j4;
5016 vinfos[4].indices[0] = _ij4[0];
5017 vinfos[4].indices[1] = _ij4[1];
5018 vinfos[4].maxsolutions = _nj4;
5019 vinfos[5].jointtype = 1;
5020 vinfos[5].foffset = j5;
5021 vinfos[5].indices[0] = _ij5[0];
5022 vinfos[5].indices[1] = _ij5[1];
5023 vinfos[5].maxsolutions = _nj5;
5024 std::vector<int> vfree(0);
5025 solutions.AddSolution(vinfos,vfree);
5026 }
5027 }
5028 }
5029 
5030 }
5031 } while(0);
5032 if( bgotonextstatement )
5033 {
5034 bool bgotonextstatement = true;
5035 do
5036 {
5037 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
5038 evalcond[1]=new_r22;
5039 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5040 {
5041 bgotonextstatement=false;
5042 {
5043 IkReal j5array[1], cj5array[1], sj5array[1];
5044 bool j5valid[1]={false};
5045 _nj5 = 1;
5046 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
5047  continue;
5048 j5array[0]=IKatan2(new_r20, new_r21);
5049 sj5array[0]=IKsin(j5array[0]);
5050 cj5array[0]=IKcos(j5array[0]);
5051 if( j5array[0] > IKPI )
5052 {
5053  j5array[0]-=IK2PI;
5054 }
5055 else if( j5array[0] < -IKPI )
5056 { j5array[0]+=IK2PI;
5057 }
5058 j5valid[0] = true;
5059 for(int ij5 = 0; ij5 < 1; ++ij5)
5060 {
5061 if( !j5valid[ij5] )
5062 {
5063  continue;
5064 }
5065 _ij5[0] = ij5; _ij5[1] = -1;
5066 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5067 {
5068 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5069 {
5070  j5valid[iij5]=false; _ij5[1] = iij5; break;
5071 }
5072 }
5073 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5074 {
5075 IkReal evalcond[8];
5076 IkReal x368=IKsin(j5);
5077 IkReal x369=IKcos(j5);
5078 IkReal x370=((1.0)*x369);
5079 IkReal x371=((1.0)*x368);
5080 evalcond[0]=((((-1.0)*x371))+new_r20);
5081 evalcond[1]=((((-1.0)*x370))+new_r21);
5082 evalcond[2]=(((sj3*x368))+new_r11);
5083 evalcond[3]=((((-1.0)*new_r12*x371))+new_r01);
5084 evalcond[4]=((((-1.0)*cj3*x370))+new_r00);
5085 evalcond[5]=((((-1.0)*sj3*x370))+new_r10);
5086 evalcond[6]=(((new_r11*sj3))+x368+((cj3*new_r01)));
5087 evalcond[7]=(((new_r10*sj3))+(((-1.0)*x370))+((cj3*new_r00)));
5088 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5089 {
5090 continue;
5091 }
5092 }
5093 
5094 {
5095 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5096 vinfos[0].jointtype = 1;
5097 vinfos[0].foffset = j0;
5098 vinfos[0].indices[0] = _ij0[0];
5099 vinfos[0].indices[1] = _ij0[1];
5100 vinfos[0].maxsolutions = _nj0;
5101 vinfos[1].jointtype = 1;
5102 vinfos[1].foffset = j1;
5103 vinfos[1].indices[0] = _ij1[0];
5104 vinfos[1].indices[1] = _ij1[1];
5105 vinfos[1].maxsolutions = _nj1;
5106 vinfos[2].jointtype = 1;
5107 vinfos[2].foffset = j2;
5108 vinfos[2].indices[0] = _ij2[0];
5109 vinfos[2].indices[1] = _ij2[1];
5110 vinfos[2].maxsolutions = _nj2;
5111 vinfos[3].jointtype = 1;
5112 vinfos[3].foffset = j3;
5113 vinfos[3].indices[0] = _ij3[0];
5114 vinfos[3].indices[1] = _ij3[1];
5115 vinfos[3].maxsolutions = _nj3;
5116 vinfos[4].jointtype = 1;
5117 vinfos[4].foffset = j4;
5118 vinfos[4].indices[0] = _ij4[0];
5119 vinfos[4].indices[1] = _ij4[1];
5120 vinfos[4].maxsolutions = _nj4;
5121 vinfos[5].jointtype = 1;
5122 vinfos[5].foffset = j5;
5123 vinfos[5].indices[0] = _ij5[0];
5124 vinfos[5].indices[1] = _ij5[1];
5125 vinfos[5].maxsolutions = _nj5;
5126 std::vector<int> vfree(0);
5127 solutions.AddSolution(vinfos,vfree);
5128 }
5129 }
5130 }
5131 
5132 }
5133 } while(0);
5134 if( bgotonextstatement )
5135 {
5136 bool bgotonextstatement = true;
5137 do
5138 {
5139 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
5140 evalcond[1]=new_r22;
5141 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5142 {
5143 bgotonextstatement=false;
5144 {
5145 IkReal j5array[1], cj5array[1], sj5array[1];
5146 bool j5valid[1]={false};
5147 _nj5 = 1;
5148 if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
5149  continue;
5150 j5array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21));
5151 sj5array[0]=IKsin(j5array[0]);
5152 cj5array[0]=IKcos(j5array[0]);
5153 if( j5array[0] > IKPI )
5154 {
5155  j5array[0]-=IK2PI;
5156 }
5157 else if( j5array[0] < -IKPI )
5158 { j5array[0]+=IK2PI;
5159 }
5160 j5valid[0] = true;
5161 for(int ij5 = 0; ij5 < 1; ++ij5)
5162 {
5163 if( !j5valid[ij5] )
5164 {
5165  continue;
5166 }
5167 _ij5[0] = ij5; _ij5[1] = -1;
5168 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5169 {
5170 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5171 {
5172  j5valid[iij5]=false; _ij5[1] = iij5; break;
5173 }
5174 }
5175 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5176 {
5177 IkReal evalcond[8];
5178 IkReal x372=IKsin(j5);
5179 IkReal x373=IKcos(j5);
5180 IkReal x374=((1.0)*x373);
5181 evalcond[0]=(x372+new_r20);
5182 evalcond[1]=(x373+new_r21);
5183 evalcond[2]=(((new_r12*x372))+new_r01);
5184 evalcond[3]=(((sj3*x372))+new_r11);
5185 evalcond[4]=((((-1.0)*cj3*x374))+new_r00);
5186 evalcond[5]=((((-1.0)*sj3*x374))+new_r10);
5187 evalcond[6]=(((new_r11*sj3))+x372+((cj3*new_r01)));
5188 evalcond[7]=(((new_r10*sj3))+(((-1.0)*x374))+((cj3*new_r00)));
5189 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5190 {
5191 continue;
5192 }
5193 }
5194 
5195 {
5196 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5197 vinfos[0].jointtype = 1;
5198 vinfos[0].foffset = j0;
5199 vinfos[0].indices[0] = _ij0[0];
5200 vinfos[0].indices[1] = _ij0[1];
5201 vinfos[0].maxsolutions = _nj0;
5202 vinfos[1].jointtype = 1;
5203 vinfos[1].foffset = j1;
5204 vinfos[1].indices[0] = _ij1[0];
5205 vinfos[1].indices[1] = _ij1[1];
5206 vinfos[1].maxsolutions = _nj1;
5207 vinfos[2].jointtype = 1;
5208 vinfos[2].foffset = j2;
5209 vinfos[2].indices[0] = _ij2[0];
5210 vinfos[2].indices[1] = _ij2[1];
5211 vinfos[2].maxsolutions = _nj2;
5212 vinfos[3].jointtype = 1;
5213 vinfos[3].foffset = j3;
5214 vinfos[3].indices[0] = _ij3[0];
5215 vinfos[3].indices[1] = _ij3[1];
5216 vinfos[3].maxsolutions = _nj3;
5217 vinfos[4].jointtype = 1;
5218 vinfos[4].foffset = j4;
5219 vinfos[4].indices[0] = _ij4[0];
5220 vinfos[4].indices[1] = _ij4[1];
5221 vinfos[4].maxsolutions = _nj4;
5222 vinfos[5].jointtype = 1;
5223 vinfos[5].foffset = j5;
5224 vinfos[5].indices[0] = _ij5[0];
5225 vinfos[5].indices[1] = _ij5[1];
5226 vinfos[5].maxsolutions = _nj5;
5227 std::vector<int> vfree(0);
5228 solutions.AddSolution(vinfos,vfree);
5229 }
5230 }
5231 }
5232 
5233 }
5234 } while(0);
5235 if( bgotonextstatement )
5236 {
5237 bool bgotonextstatement = true;
5238 do
5239 {
5240 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
5241 evalcond[1]=new_r02;
5242 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5243 {
5244 bgotonextstatement=false;
5245 {
5246 IkReal j5array[1], cj5array[1], sj5array[1];
5247 bool j5valid[1]={false};
5248 _nj5 = 1;
5249 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 )
5250  continue;
5251 j5array[0]=IKatan2(((-1.0)*new_r01), new_r00);
5252 sj5array[0]=IKsin(j5array[0]);
5253 cj5array[0]=IKcos(j5array[0]);
5254 if( j5array[0] > IKPI )
5255 {
5256  j5array[0]-=IK2PI;
5257 }
5258 else if( j5array[0] < -IKPI )
5259 { j5array[0]+=IK2PI;
5260 }
5261 j5valid[0] = true;
5262 for(int ij5 = 0; ij5 < 1; ++ij5)
5263 {
5264 if( !j5valid[ij5] )
5265 {
5266  continue;
5267 }
5268 _ij5[0] = ij5; _ij5[1] = -1;
5269 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5270 {
5271 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5272 {
5273  j5valid[iij5]=false; _ij5[1] = iij5; break;
5274 }
5275 }
5276 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5277 {
5278 IkReal evalcond[8];
5279 IkReal x375=IKsin(j5);
5280 IkReal x376=IKcos(j5);
5281 IkReal x377=((1.0)*cj4);
5282 IkReal x378=((1.0)*x376);
5283 IkReal x379=((1.0)*x375);
5284 evalcond[0]=(x375+new_r01);
5285 evalcond[1]=((((-1.0)*x378))+new_r00);
5286 evalcond[2]=((((-1.0)*sj4*x379))+new_r20);
5287 evalcond[3]=((((-1.0)*sj4*x378))+new_r21);
5288 evalcond[4]=((((-1.0)*x376*x377))+new_r11);
5289 evalcond[5]=(new_r10+(((-1.0)*x375*x377)));
5290 evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x379)));
5291 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x378))+((new_r21*sj4)));
5292 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5293 {
5294 continue;
5295 }
5296 }
5297 
5298 {
5299 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5300 vinfos[0].jointtype = 1;
5301 vinfos[0].foffset = j0;
5302 vinfos[0].indices[0] = _ij0[0];
5303 vinfos[0].indices[1] = _ij0[1];
5304 vinfos[0].maxsolutions = _nj0;
5305 vinfos[1].jointtype = 1;
5306 vinfos[1].foffset = j1;
5307 vinfos[1].indices[0] = _ij1[0];
5308 vinfos[1].indices[1] = _ij1[1];
5309 vinfos[1].maxsolutions = _nj1;
5310 vinfos[2].jointtype = 1;
5311 vinfos[2].foffset = j2;
5312 vinfos[2].indices[0] = _ij2[0];
5313 vinfos[2].indices[1] = _ij2[1];
5314 vinfos[2].maxsolutions = _nj2;
5315 vinfos[3].jointtype = 1;
5316 vinfos[3].foffset = j3;
5317 vinfos[3].indices[0] = _ij3[0];
5318 vinfos[3].indices[1] = _ij3[1];
5319 vinfos[3].maxsolutions = _nj3;
5320 vinfos[4].jointtype = 1;
5321 vinfos[4].foffset = j4;
5322 vinfos[4].indices[0] = _ij4[0];
5323 vinfos[4].indices[1] = _ij4[1];
5324 vinfos[4].maxsolutions = _nj4;
5325 vinfos[5].jointtype = 1;
5326 vinfos[5].foffset = j5;
5327 vinfos[5].indices[0] = _ij5[0];
5328 vinfos[5].indices[1] = _ij5[1];
5329 vinfos[5].maxsolutions = _nj5;
5330 std::vector<int> vfree(0);
5331 solutions.AddSolution(vinfos,vfree);
5332 }
5333 }
5334 }
5335 
5336 }
5337 } while(0);
5338 if( bgotonextstatement )
5339 {
5340 bool bgotonextstatement = true;
5341 do
5342 {
5343 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
5344 evalcond[1]=new_r02;
5345 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5346 {
5347 bgotonextstatement=false;
5348 {
5349 IkReal j5array[1], cj5array[1], sj5array[1];
5350 bool j5valid[1]={false};
5351 _nj5 = 1;
5352 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
5353  continue;
5354 j5array[0]=IKatan2(new_r01, ((-1.0)*new_r00));
5355 sj5array[0]=IKsin(j5array[0]);
5356 cj5array[0]=IKcos(j5array[0]);
5357 if( j5array[0] > IKPI )
5358 {
5359  j5array[0]-=IK2PI;
5360 }
5361 else if( j5array[0] < -IKPI )
5362 { j5array[0]+=IK2PI;
5363 }
5364 j5valid[0] = true;
5365 for(int ij5 = 0; ij5 < 1; ++ij5)
5366 {
5367 if( !j5valid[ij5] )
5368 {
5369  continue;
5370 }
5371 _ij5[0] = ij5; _ij5[1] = -1;
5372 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5373 {
5374 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5375 {
5376  j5valid[iij5]=false; _ij5[1] = iij5; break;
5377 }
5378 }
5379 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5380 {
5381 IkReal evalcond[8];
5382 IkReal x380=IKsin(j5);
5383 IkReal x381=IKcos(j5);
5384 IkReal x382=((1.0)*cj4);
5385 IkReal x383=((1.0)*x381);
5386 IkReal x384=((1.0)*x380);
5387 evalcond[0]=(x380+(((-1.0)*new_r01)));
5388 evalcond[1]=(new_r11+((cj4*x381)));
5389 evalcond[2]=((((-1.0)*sj4*x384))+new_r20);
5390 evalcond[3]=((((-1.0)*sj4*x383))+new_r21);
5391 evalcond[4]=((((-1.0)*x383))+(((-1.0)*new_r00)));
5392 evalcond[5]=((((-1.0)*x380*x382))+(((-1.0)*new_r10)));
5393 evalcond[6]=((((-1.0)*new_r10*x382))+((new_r20*sj4))+(((-1.0)*x384)));
5394 evalcond[7]=((((-1.0)*new_r11*x382))+(((-1.0)*x383))+((new_r21*sj4)));
5395 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5396 {
5397 continue;
5398 }
5399 }
5400 
5401 {
5402 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5403 vinfos[0].jointtype = 1;
5404 vinfos[0].foffset = j0;
5405 vinfos[0].indices[0] = _ij0[0];
5406 vinfos[0].indices[1] = _ij0[1];
5407 vinfos[0].maxsolutions = _nj0;
5408 vinfos[1].jointtype = 1;
5409 vinfos[1].foffset = j1;
5410 vinfos[1].indices[0] = _ij1[0];
5411 vinfos[1].indices[1] = _ij1[1];
5412 vinfos[1].maxsolutions = _nj1;
5413 vinfos[2].jointtype = 1;
5414 vinfos[2].foffset = j2;
5415 vinfos[2].indices[0] = _ij2[0];
5416 vinfos[2].indices[1] = _ij2[1];
5417 vinfos[2].maxsolutions = _nj2;
5418 vinfos[3].jointtype = 1;
5419 vinfos[3].foffset = j3;
5420 vinfos[3].indices[0] = _ij3[0];
5421 vinfos[3].indices[1] = _ij3[1];
5422 vinfos[3].maxsolutions = _nj3;
5423 vinfos[4].jointtype = 1;
5424 vinfos[4].foffset = j4;
5425 vinfos[4].indices[0] = _ij4[0];
5426 vinfos[4].indices[1] = _ij4[1];
5427 vinfos[4].maxsolutions = _nj4;
5428 vinfos[5].jointtype = 1;
5429 vinfos[5].foffset = j5;
5430 vinfos[5].indices[0] = _ij5[0];
5431 vinfos[5].indices[1] = _ij5[1];
5432 vinfos[5].maxsolutions = _nj5;
5433 std::vector<int> vfree(0);
5434 solutions.AddSolution(vinfos,vfree);
5435 }
5436 }
5437 }
5438 
5439 }
5440 } while(0);
5441 if( bgotonextstatement )
5442 {
5443 bool bgotonextstatement = true;
5444 do
5445 {
5446 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
5447 evalcond[1]=new_r12;
5448 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5449 {
5450 bgotonextstatement=false;
5451 {
5452 IkReal j5array[1], cj5array[1], sj5array[1];
5453 bool j5valid[1]={false};
5454 _nj5 = 1;
5455 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH )
5456  continue;
5457 j5array[0]=IKatan2(((-1.0)*new_r11), new_r10);
5458 sj5array[0]=IKsin(j5array[0]);
5459 cj5array[0]=IKcos(j5array[0]);
5460 if( j5array[0] > IKPI )
5461 {
5462  j5array[0]-=IK2PI;
5463 }
5464 else if( j5array[0] < -IKPI )
5465 { j5array[0]+=IK2PI;
5466 }
5467 j5valid[0] = true;
5468 for(int ij5 = 0; ij5 < 1; ++ij5)
5469 {
5470 if( !j5valid[ij5] )
5471 {
5472  continue;
5473 }
5474 _ij5[0] = ij5; _ij5[1] = -1;
5475 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5476 {
5477 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5478 {
5479  j5valid[iij5]=false; _ij5[1] = iij5; break;
5480 }
5481 }
5482 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5483 {
5484 IkReal evalcond[8];
5485 IkReal x385=IKcos(j5);
5486 IkReal x386=IKsin(j5);
5487 IkReal x387=((1.0)*cj4);
5488 IkReal x388=((1.0)*x385);
5489 IkReal x389=((1.0)*x386);
5490 evalcond[0]=(x386+new_r11);
5491 evalcond[1]=((((-1.0)*x388))+new_r10);
5492 evalcond[2]=(new_r01+((cj4*x385)));
5493 evalcond[3]=(new_r00+((cj4*x386)));
5494 evalcond[4]=((((-1.0)*new_r02*x389))+new_r20);
5495 evalcond[5]=((((-1.0)*new_r02*x388))+new_r21);
5496 evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r00*x387))+(((-1.0)*x389)));
5497 evalcond[7]=((((-1.0)*new_r01*x387))+(((-1.0)*x388))+((new_r21*sj4)));
5498 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
5499 {
5500 continue;
5501 }
5502 }
5503 
5504 {
5505 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5506 vinfos[0].jointtype = 1;
5507 vinfos[0].foffset = j0;
5508 vinfos[0].indices[0] = _ij0[0];
5509 vinfos[0].indices[1] = _ij0[1];
5510 vinfos[0].maxsolutions = _nj0;
5511 vinfos[1].jointtype = 1;
5512 vinfos[1].foffset = j1;
5513 vinfos[1].indices[0] = _ij1[0];
5514 vinfos[1].indices[1] = _ij1[1];
5515 vinfos[1].maxsolutions = _nj1;
5516 vinfos[2].jointtype = 1;
5517 vinfos[2].foffset = j2;
5518 vinfos[2].indices[0] = _ij2[0];
5519 vinfos[2].indices[1] = _ij2[1];
5520 vinfos[2].maxsolutions = _nj2;
5521 vinfos[3].jointtype = 1;
5522 vinfos[3].foffset = j3;
5523 vinfos[3].indices[0] = _ij3[0];
5524 vinfos[3].indices[1] = _ij3[1];
5525 vinfos[3].maxsolutions = _nj3;
5526 vinfos[4].jointtype = 1;
5527 vinfos[4].foffset = j4;
5528 vinfos[4].indices[0] = _ij4[0];
5529 vinfos[4].indices[1] = _ij4[1];
5530 vinfos[4].maxsolutions = _nj4;
5531 vinfos[5].jointtype = 1;
5532 vinfos[5].foffset = j5;
5533 vinfos[5].indices[0] = _ij5[0];
5534 vinfos[5].indices[1] = _ij5[1];
5535 vinfos[5].maxsolutions = _nj5;
5536 std::vector<int> vfree(0);
5537 solutions.AddSolution(vinfos,vfree);
5538 }
5539 }
5540 }
5541 
5542 }
5543 } while(0);
5544 if( bgotonextstatement )
5545 {
5546 bool bgotonextstatement = true;
5547 do
5548 {
5549 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
5550 evalcond[1]=new_r12;
5551 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5552 {
5553 bgotonextstatement=false;
5554 {
5555 IkReal j5eval[3];
5556 sj3=-1.0;
5557 cj3=0;
5558 j3=-1.5707963267949;
5559 j5eval[0]=new_r02;
5560 j5eval[1]=IKsign(new_r02);
5561 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
5562 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
5563 {
5564 {
5565 IkReal j5eval[1];
5566 sj3=-1.0;
5567 cj3=0;
5568 j3=-1.5707963267949;
5569 j5eval[0]=new_r02;
5570 if( IKabs(j5eval[0]) < 0.0000010000000000 )
5571 {
5572 {
5573 IkReal j5eval[2];
5574 sj3=-1.0;
5575 cj3=0;
5576 j3=-1.5707963267949;
5577 j5eval[0]=new_r02;
5578 j5eval[1]=cj4;
5579 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
5580 {
5581 {
5582 IkReal evalcond[4];
5583 bool bgotonextstatement = true;
5584 do
5585 {
5586 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
5587 evalcond[1]=new_r22;
5588 evalcond[2]=new_r01;
5589 evalcond[3]=new_r00;
5590 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
5591 {
5592 bgotonextstatement=false;
5593 {
5594 IkReal j5array[1], cj5array[1], sj5array[1];
5595 bool j5valid[1]={false};
5596 _nj5 = 1;
5597 if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH )
5598  continue;
5599 j5array[0]=IKatan2(new_r20, new_r21);
5600 sj5array[0]=IKsin(j5array[0]);
5601 cj5array[0]=IKcos(j5array[0]);
5602 if( j5array[0] > IKPI )
5603 {
5604  j5array[0]-=IK2PI;
5605 }
5606 else if( j5array[0] < -IKPI )
5607 { j5array[0]+=IK2PI;
5608 }
5609 j5valid[0] = true;
5610 for(int ij5 = 0; ij5 < 1; ++ij5)
5611 {
5612 if( !j5valid[ij5] )
5613 {
5614  continue;
5615 }
5616 _ij5[0] = ij5; _ij5[1] = -1;
5617 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5618 {
5619 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5620 {
5621  j5valid[iij5]=false; _ij5[1] = iij5; break;
5622 }
5623 }
5624 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5625 {
5626 IkReal evalcond[4];
5627 IkReal x390=IKsin(j5);
5628 IkReal x391=((1.0)*(IKcos(j5)));
5629 evalcond[0]=((((-1.0)*x390))+new_r20);
5630 evalcond[1]=((((-1.0)*x391))+new_r21);
5631 evalcond[2]=(x390+(((-1.0)*new_r11)));
5632 evalcond[3]=((((-1.0)*x391))+(((-1.0)*new_r10)));
5633 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
5634 {
5635 continue;
5636 }
5637 }
5638 
5639 {
5640 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5641 vinfos[0].jointtype = 1;
5642 vinfos[0].foffset = j0;
5643 vinfos[0].indices[0] = _ij0[0];
5644 vinfos[0].indices[1] = _ij0[1];
5645 vinfos[0].maxsolutions = _nj0;
5646 vinfos[1].jointtype = 1;
5647 vinfos[1].foffset = j1;
5648 vinfos[1].indices[0] = _ij1[0];
5649 vinfos[1].indices[1] = _ij1[1];
5650 vinfos[1].maxsolutions = _nj1;
5651 vinfos[2].jointtype = 1;
5652 vinfos[2].foffset = j2;
5653 vinfos[2].indices[0] = _ij2[0];
5654 vinfos[2].indices[1] = _ij2[1];
5655 vinfos[2].maxsolutions = _nj2;
5656 vinfos[3].jointtype = 1;
5657 vinfos[3].foffset = j3;
5658 vinfos[3].indices[0] = _ij3[0];
5659 vinfos[3].indices[1] = _ij3[1];
5660 vinfos[3].maxsolutions = _nj3;
5661 vinfos[4].jointtype = 1;
5662 vinfos[4].foffset = j4;
5663 vinfos[4].indices[0] = _ij4[0];
5664 vinfos[4].indices[1] = _ij4[1];
5665 vinfos[4].maxsolutions = _nj4;
5666 vinfos[5].jointtype = 1;
5667 vinfos[5].foffset = j5;
5668 vinfos[5].indices[0] = _ij5[0];
5669 vinfos[5].indices[1] = _ij5[1];
5670 vinfos[5].maxsolutions = _nj5;
5671 std::vector<int> vfree(0);
5672 solutions.AddSolution(vinfos,vfree);
5673 }
5674 }
5675 }
5676 
5677 }
5678 } while(0);
5679 if( bgotonextstatement )
5680 {
5681 bool bgotonextstatement = true;
5682 do
5683 {
5684 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
5685 evalcond[1]=new_r22;
5686 evalcond[2]=new_r01;
5687 evalcond[3]=new_r00;
5688 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 )
5689 {
5690 bgotonextstatement=false;
5691 {
5692 IkReal j5array[1], cj5array[1], sj5array[1];
5693 bool j5valid[1]={false};
5694 _nj5 = 1;
5695 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH )
5696  continue;
5697 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21));
5698 sj5array[0]=IKsin(j5array[0]);
5699 cj5array[0]=IKcos(j5array[0]);
5700 if( j5array[0] > IKPI )
5701 {
5702  j5array[0]-=IK2PI;
5703 }
5704 else if( j5array[0] < -IKPI )
5705 { j5array[0]+=IK2PI;
5706 }
5707 j5valid[0] = true;
5708 for(int ij5 = 0; ij5 < 1; ++ij5)
5709 {
5710 if( !j5valid[ij5] )
5711 {
5712  continue;
5713 }
5714 _ij5[0] = ij5; _ij5[1] = -1;
5715 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5716 {
5717 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5718 {
5719  j5valid[iij5]=false; _ij5[1] = iij5; break;
5720 }
5721 }
5722 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5723 {
5724 IkReal evalcond[4];
5725 IkReal x392=IKsin(j5);
5726 IkReal x393=IKcos(j5);
5727 evalcond[0]=(x392+new_r20);
5728 evalcond[1]=(x393+new_r21);
5729 evalcond[2]=(x392+(((-1.0)*new_r11)));
5730 evalcond[3]=((((-1.0)*x393))+(((-1.0)*new_r10)));
5731 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
5732 {
5733 continue;
5734 }
5735 }
5736 
5737 {
5738 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5739 vinfos[0].jointtype = 1;
5740 vinfos[0].foffset = j0;
5741 vinfos[0].indices[0] = _ij0[0];
5742 vinfos[0].indices[1] = _ij0[1];
5743 vinfos[0].maxsolutions = _nj0;
5744 vinfos[1].jointtype = 1;
5745 vinfos[1].foffset = j1;
5746 vinfos[1].indices[0] = _ij1[0];
5747 vinfos[1].indices[1] = _ij1[1];
5748 vinfos[1].maxsolutions = _nj1;
5749 vinfos[2].jointtype = 1;
5750 vinfos[2].foffset = j2;
5751 vinfos[2].indices[0] = _ij2[0];
5752 vinfos[2].indices[1] = _ij2[1];
5753 vinfos[2].maxsolutions = _nj2;
5754 vinfos[3].jointtype = 1;
5755 vinfos[3].foffset = j3;
5756 vinfos[3].indices[0] = _ij3[0];
5757 vinfos[3].indices[1] = _ij3[1];
5758 vinfos[3].maxsolutions = _nj3;
5759 vinfos[4].jointtype = 1;
5760 vinfos[4].foffset = j4;
5761 vinfos[4].indices[0] = _ij4[0];
5762 vinfos[4].indices[1] = _ij4[1];
5763 vinfos[4].maxsolutions = _nj4;
5764 vinfos[5].jointtype = 1;
5765 vinfos[5].foffset = j5;
5766 vinfos[5].indices[0] = _ij5[0];
5767 vinfos[5].indices[1] = _ij5[1];
5768 vinfos[5].maxsolutions = _nj5;
5769 std::vector<int> vfree(0);
5770 solutions.AddSolution(vinfos,vfree);
5771 }
5772 }
5773 }
5774 
5775 }
5776 } while(0);
5777 if( bgotonextstatement )
5778 {
5779 bool bgotonextstatement = true;
5780 do
5781 {
5782 evalcond[0]=IKabs(new_r02);
5783 evalcond[1]=new_r20;
5784 evalcond[2]=new_r21;
5785 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
5786 {
5787 bgotonextstatement=false;
5788 {
5789 IkReal j5array[1], cj5array[1], sj5array[1];
5790 bool j5valid[1]={false};
5791 _nj5 = 1;
5792 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
5793  continue;
5794 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
5795 sj5array[0]=IKsin(j5array[0]);
5796 cj5array[0]=IKcos(j5array[0]);
5797 if( j5array[0] > IKPI )
5798 {
5799  j5array[0]-=IK2PI;
5800 }
5801 else if( j5array[0] < -IKPI )
5802 { j5array[0]+=IK2PI;
5803 }
5804 j5valid[0] = true;
5805 for(int ij5 = 0; ij5 < 1; ++ij5)
5806 {
5807 if( !j5valid[ij5] )
5808 {
5809  continue;
5810 }
5811 _ij5[0] = ij5; _ij5[1] = -1;
5812 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5813 {
5814 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5815 {
5816  j5valid[iij5]=false; _ij5[1] = iij5; break;
5817 }
5818 }
5819 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5820 {
5821 IkReal evalcond[6];
5822 IkReal x394=IKsin(j5);
5823 IkReal x395=IKcos(j5);
5824 IkReal x396=((1.0)*cj4);
5825 IkReal x397=((1.0)*x395);
5826 evalcond[0]=(x394+(((-1.0)*new_r11)));
5827 evalcond[1]=((((-1.0)*x395*x396))+new_r01);
5828 evalcond[2]=((((-1.0)*x394*x396))+new_r00);
5829 evalcond[3]=((((-1.0)*x397))+(((-1.0)*new_r10)));
5830 evalcond[4]=((((-1.0)*x394))+((cj4*new_r00)));
5831 evalcond[5]=((((-1.0)*x397))+((cj4*new_r01)));
5832 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5833 {
5834 continue;
5835 }
5836 }
5837 
5838 {
5839 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5840 vinfos[0].jointtype = 1;
5841 vinfos[0].foffset = j0;
5842 vinfos[0].indices[0] = _ij0[0];
5843 vinfos[0].indices[1] = _ij0[1];
5844 vinfos[0].maxsolutions = _nj0;
5845 vinfos[1].jointtype = 1;
5846 vinfos[1].foffset = j1;
5847 vinfos[1].indices[0] = _ij1[0];
5848 vinfos[1].indices[1] = _ij1[1];
5849 vinfos[1].maxsolutions = _nj1;
5850 vinfos[2].jointtype = 1;
5851 vinfos[2].foffset = j2;
5852 vinfos[2].indices[0] = _ij2[0];
5853 vinfos[2].indices[1] = _ij2[1];
5854 vinfos[2].maxsolutions = _nj2;
5855 vinfos[3].jointtype = 1;
5856 vinfos[3].foffset = j3;
5857 vinfos[3].indices[0] = _ij3[0];
5858 vinfos[3].indices[1] = _ij3[1];
5859 vinfos[3].maxsolutions = _nj3;
5860 vinfos[4].jointtype = 1;
5861 vinfos[4].foffset = j4;
5862 vinfos[4].indices[0] = _ij4[0];
5863 vinfos[4].indices[1] = _ij4[1];
5864 vinfos[4].maxsolutions = _nj4;
5865 vinfos[5].jointtype = 1;
5866 vinfos[5].foffset = j5;
5867 vinfos[5].indices[0] = _ij5[0];
5868 vinfos[5].indices[1] = _ij5[1];
5869 vinfos[5].maxsolutions = _nj5;
5870 std::vector<int> vfree(0);
5871 solutions.AddSolution(vinfos,vfree);
5872 }
5873 }
5874 }
5875 
5876 }
5877 } while(0);
5878 if( bgotonextstatement )
5879 {
5880 bool bgotonextstatement = true;
5881 do
5882 {
5883 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
5884 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5885 {
5886 bgotonextstatement=false;
5887 {
5888 IkReal j5array[1], cj5array[1], sj5array[1];
5889 bool j5valid[1]={false};
5890 _nj5 = 1;
5891 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH )
5892  continue;
5893 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10));
5894 sj5array[0]=IKsin(j5array[0]);
5895 cj5array[0]=IKcos(j5array[0]);
5896 if( j5array[0] > IKPI )
5897 {
5898  j5array[0]-=IK2PI;
5899 }
5900 else if( j5array[0] < -IKPI )
5901 { j5array[0]+=IK2PI;
5902 }
5903 j5valid[0] = true;
5904 for(int ij5 = 0; ij5 < 1; ++ij5)
5905 {
5906 if( !j5valid[ij5] )
5907 {
5908  continue;
5909 }
5910 _ij5[0] = ij5; _ij5[1] = -1;
5911 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5912 {
5913 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5914 {
5915  j5valid[iij5]=false; _ij5[1] = iij5; break;
5916 }
5917 }
5918 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5919 {
5920 IkReal evalcond[6];
5921 IkReal x398=IKcos(j5);
5922 IkReal x399=IKsin(j5);
5923 IkReal x400=((-1.0)*x399);
5924 IkReal x401=((-1.0)*x398);
5925 evalcond[0]=x400;
5926 evalcond[1]=x401;
5927 evalcond[2]=(new_r22*x401);
5928 evalcond[3]=(new_r22*x400);
5929 evalcond[4]=(x399+(((-1.0)*new_r11)));
5930 evalcond[5]=((((-1.0)*x398))+(((-1.0)*new_r10)));
5931 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5932 {
5933 continue;
5934 }
5935 }
5936 
5937 {
5938 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5939 vinfos[0].jointtype = 1;
5940 vinfos[0].foffset = j0;
5941 vinfos[0].indices[0] = _ij0[0];
5942 vinfos[0].indices[1] = _ij0[1];
5943 vinfos[0].maxsolutions = _nj0;
5944 vinfos[1].jointtype = 1;
5945 vinfos[1].foffset = j1;
5946 vinfos[1].indices[0] = _ij1[0];
5947 vinfos[1].indices[1] = _ij1[1];
5948 vinfos[1].maxsolutions = _nj1;
5949 vinfos[2].jointtype = 1;
5950 vinfos[2].foffset = j2;
5951 vinfos[2].indices[0] = _ij2[0];
5952 vinfos[2].indices[1] = _ij2[1];
5953 vinfos[2].maxsolutions = _nj2;
5954 vinfos[3].jointtype = 1;
5955 vinfos[3].foffset = j3;
5956 vinfos[3].indices[0] = _ij3[0];
5957 vinfos[3].indices[1] = _ij3[1];
5958 vinfos[3].maxsolutions = _nj3;
5959 vinfos[4].jointtype = 1;
5960 vinfos[4].foffset = j4;
5961 vinfos[4].indices[0] = _ij4[0];
5962 vinfos[4].indices[1] = _ij4[1];
5963 vinfos[4].maxsolutions = _nj4;
5964 vinfos[5].jointtype = 1;
5965 vinfos[5].foffset = j5;
5966 vinfos[5].indices[0] = _ij5[0];
5967 vinfos[5].indices[1] = _ij5[1];
5968 vinfos[5].maxsolutions = _nj5;
5969 std::vector<int> vfree(0);
5970 solutions.AddSolution(vinfos,vfree);
5971 }
5972 }
5973 }
5974 
5975 }
5976 } while(0);
5977 if( bgotonextstatement )
5978 {
5979 bool bgotonextstatement = true;
5980 do
5981 {
5982 if( 1 )
5983 {
5984 bgotonextstatement=false;
5985 continue; // branch miss [j5]
5986 
5987 }
5988 } while(0);
5989 if( bgotonextstatement )
5990 {
5991 }
5992 }
5993 }
5994 }
5995 }
5996 }
5997 
5998 } else
5999 {
6000 {
6001 IkReal j5array[1], cj5array[1], sj5array[1];
6002 bool j5valid[1]={false};
6003 _nj5 = 1;
6004 CheckValue<IkReal> x402=IKPowWithIntegerCheck(new_r02,-1);
6005 if(!x402.valid){
6006 continue;
6007 }
6008 CheckValue<IkReal> x403=IKPowWithIntegerCheck(cj4,-1);
6009 if(!x403.valid){
6010 continue;
6011 }
6012 if( IKabs(((-1.0)*new_r20*(x402.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x403.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x402.value)))+IKsqr((new_r01*(x403.value)))-1) <= IKFAST_SINCOS_THRESH )
6013  continue;
6014 j5array[0]=IKatan2(((-1.0)*new_r20*(x402.value)), (new_r01*(x403.value)));
6015 sj5array[0]=IKsin(j5array[0]);
6016 cj5array[0]=IKcos(j5array[0]);
6017 if( j5array[0] > IKPI )
6018 {
6019  j5array[0]-=IK2PI;
6020 }
6021 else if( j5array[0] < -IKPI )
6022 { j5array[0]+=IK2PI;
6023 }
6024 j5valid[0] = true;
6025 for(int ij5 = 0; ij5 < 1; ++ij5)
6026 {
6027 if( !j5valid[ij5] )
6028 {
6029  continue;
6030 }
6031 _ij5[0] = ij5; _ij5[1] = -1;
6032 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6033 {
6034 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6035 {
6036  j5valid[iij5]=false; _ij5[1] = iij5; break;
6037 }
6038 }
6039 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6040 {
6041 IkReal evalcond[8];
6042 IkReal x404=IKsin(j5);
6043 IkReal x405=IKcos(j5);
6044 IkReal x406=((1.0)*cj4);
6045 IkReal x407=((1.0)*x405);
6046 evalcond[0]=(new_r20+((new_r02*x404)));
6047 evalcond[1]=(new_r21+((new_r02*x405)));
6048 evalcond[2]=(x404+(((-1.0)*new_r11)));
6049 evalcond[3]=(new_r01+(((-1.0)*x405*x406)));
6050 evalcond[4]=((((-1.0)*x404*x406))+new_r00);
6051 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x407)));
6052 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x404)));
6053 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x407)));
6054 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6055 {
6056 continue;
6057 }
6058 }
6059 
6060 {
6061 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6062 vinfos[0].jointtype = 1;
6063 vinfos[0].foffset = j0;
6064 vinfos[0].indices[0] = _ij0[0];
6065 vinfos[0].indices[1] = _ij0[1];
6066 vinfos[0].maxsolutions = _nj0;
6067 vinfos[1].jointtype = 1;
6068 vinfos[1].foffset = j1;
6069 vinfos[1].indices[0] = _ij1[0];
6070 vinfos[1].indices[1] = _ij1[1];
6071 vinfos[1].maxsolutions = _nj1;
6072 vinfos[2].jointtype = 1;
6073 vinfos[2].foffset = j2;
6074 vinfos[2].indices[0] = _ij2[0];
6075 vinfos[2].indices[1] = _ij2[1];
6076 vinfos[2].maxsolutions = _nj2;
6077 vinfos[3].jointtype = 1;
6078 vinfos[3].foffset = j3;
6079 vinfos[3].indices[0] = _ij3[0];
6080 vinfos[3].indices[1] = _ij3[1];
6081 vinfos[3].maxsolutions = _nj3;
6082 vinfos[4].jointtype = 1;
6083 vinfos[4].foffset = j4;
6084 vinfos[4].indices[0] = _ij4[0];
6085 vinfos[4].indices[1] = _ij4[1];
6086 vinfos[4].maxsolutions = _nj4;
6087 vinfos[5].jointtype = 1;
6088 vinfos[5].foffset = j5;
6089 vinfos[5].indices[0] = _ij5[0];
6090 vinfos[5].indices[1] = _ij5[1];
6091 vinfos[5].maxsolutions = _nj5;
6092 std::vector<int> vfree(0);
6093 solutions.AddSolution(vinfos,vfree);
6094 }
6095 }
6096 }
6097 
6098 }
6099 
6100 }
6101 
6102 } else
6103 {
6104 {
6105 IkReal j5array[1], cj5array[1], sj5array[1];
6106 bool j5valid[1]={false};
6107 _nj5 = 1;
6108 CheckValue<IkReal> x408=IKPowWithIntegerCheck(new_r02,-1);
6109 if(!x408.valid){
6110 continue;
6111 }
6112 if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x408.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x408.value)))-1) <= IKFAST_SINCOS_THRESH )
6113  continue;
6114 j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x408.value)));
6115 sj5array[0]=IKsin(j5array[0]);
6116 cj5array[0]=IKcos(j5array[0]);
6117 if( j5array[0] > IKPI )
6118 {
6119  j5array[0]-=IK2PI;
6120 }
6121 else if( j5array[0] < -IKPI )
6122 { j5array[0]+=IK2PI;
6123 }
6124 j5valid[0] = true;
6125 for(int ij5 = 0; ij5 < 1; ++ij5)
6126 {
6127 if( !j5valid[ij5] )
6128 {
6129  continue;
6130 }
6131 _ij5[0] = ij5; _ij5[1] = -1;
6132 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6133 {
6134 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6135 {
6136  j5valid[iij5]=false; _ij5[1] = iij5; break;
6137 }
6138 }
6139 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6140 {
6141 IkReal evalcond[8];
6142 IkReal x409=IKsin(j5);
6143 IkReal x410=IKcos(j5);
6144 IkReal x411=((1.0)*cj4);
6145 IkReal x412=((1.0)*x410);
6146 evalcond[0]=(new_r20+((new_r02*x409)));
6147 evalcond[1]=(new_r21+((new_r02*x410)));
6148 evalcond[2]=(x409+(((-1.0)*new_r11)));
6149 evalcond[3]=((((-1.0)*x410*x411))+new_r01);
6150 evalcond[4]=(new_r00+(((-1.0)*x409*x411)));
6151 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x412)));
6152 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x409)));
6153 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x412)));
6154 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6155 {
6156 continue;
6157 }
6158 }
6159 
6160 {
6161 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6162 vinfos[0].jointtype = 1;
6163 vinfos[0].foffset = j0;
6164 vinfos[0].indices[0] = _ij0[0];
6165 vinfos[0].indices[1] = _ij0[1];
6166 vinfos[0].maxsolutions = _nj0;
6167 vinfos[1].jointtype = 1;
6168 vinfos[1].foffset = j1;
6169 vinfos[1].indices[0] = _ij1[0];
6170 vinfos[1].indices[1] = _ij1[1];
6171 vinfos[1].maxsolutions = _nj1;
6172 vinfos[2].jointtype = 1;
6173 vinfos[2].foffset = j2;
6174 vinfos[2].indices[0] = _ij2[0];
6175 vinfos[2].indices[1] = _ij2[1];
6176 vinfos[2].maxsolutions = _nj2;
6177 vinfos[3].jointtype = 1;
6178 vinfos[3].foffset = j3;
6179 vinfos[3].indices[0] = _ij3[0];
6180 vinfos[3].indices[1] = _ij3[1];
6181 vinfos[3].maxsolutions = _nj3;
6182 vinfos[4].jointtype = 1;
6183 vinfos[4].foffset = j4;
6184 vinfos[4].indices[0] = _ij4[0];
6185 vinfos[4].indices[1] = _ij4[1];
6186 vinfos[4].maxsolutions = _nj4;
6187 vinfos[5].jointtype = 1;
6188 vinfos[5].foffset = j5;
6189 vinfos[5].indices[0] = _ij5[0];
6190 vinfos[5].indices[1] = _ij5[1];
6191 vinfos[5].maxsolutions = _nj5;
6192 std::vector<int> vfree(0);
6193 solutions.AddSolution(vinfos,vfree);
6194 }
6195 }
6196 }
6197 
6198 }
6199 
6200 }
6201 
6202 } else
6203 {
6204 {
6205 IkReal j5array[1], cj5array[1], sj5array[1];
6206 bool j5valid[1]={false};
6207 _nj5 = 1;
6208 CheckValue<IkReal> x413=IKPowWithIntegerCheck(IKsign(new_r02),-1);
6209 if(!x413.valid){
6210 continue;
6211 }
6212 CheckValue<IkReal> x414 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH);
6213 if(!x414.valid){
6214 continue;
6215 }
6216 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x413.value)))+(x414.value));
6217 sj5array[0]=IKsin(j5array[0]);
6218 cj5array[0]=IKcos(j5array[0]);
6219 if( j5array[0] > IKPI )
6220 {
6221  j5array[0]-=IK2PI;
6222 }
6223 else if( j5array[0] < -IKPI )
6224 { j5array[0]+=IK2PI;
6225 }
6226 j5valid[0] = true;
6227 for(int ij5 = 0; ij5 < 1; ++ij5)
6228 {
6229 if( !j5valid[ij5] )
6230 {
6231  continue;
6232 }
6233 _ij5[0] = ij5; _ij5[1] = -1;
6234 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6235 {
6236 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6237 {
6238  j5valid[iij5]=false; _ij5[1] = iij5; break;
6239 }
6240 }
6241 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6242 {
6243 IkReal evalcond[8];
6244 IkReal x415=IKsin(j5);
6245 IkReal x416=IKcos(j5);
6246 IkReal x417=((1.0)*cj4);
6247 IkReal x418=((1.0)*x416);
6248 evalcond[0]=(new_r20+((new_r02*x415)));
6249 evalcond[1]=(new_r21+((new_r02*x416)));
6250 evalcond[2]=(x415+(((-1.0)*new_r11)));
6251 evalcond[3]=((((-1.0)*x416*x417))+new_r01);
6252 evalcond[4]=((((-1.0)*x415*x417))+new_r00);
6253 evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x418)));
6254 evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x415)));
6255 evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x418)));
6256 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6257 {
6258 continue;
6259 }
6260 }
6261 
6262 {
6263 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6264 vinfos[0].jointtype = 1;
6265 vinfos[0].foffset = j0;
6266 vinfos[0].indices[0] = _ij0[0];
6267 vinfos[0].indices[1] = _ij0[1];
6268 vinfos[0].maxsolutions = _nj0;
6269 vinfos[1].jointtype = 1;
6270 vinfos[1].foffset = j1;
6271 vinfos[1].indices[0] = _ij1[0];
6272 vinfos[1].indices[1] = _ij1[1];
6273 vinfos[1].maxsolutions = _nj1;
6274 vinfos[2].jointtype = 1;
6275 vinfos[2].foffset = j2;
6276 vinfos[2].indices[0] = _ij2[0];
6277 vinfos[2].indices[1] = _ij2[1];
6278 vinfos[2].maxsolutions = _nj2;
6279 vinfos[3].jointtype = 1;
6280 vinfos[3].foffset = j3;
6281 vinfos[3].indices[0] = _ij3[0];
6282 vinfos[3].indices[1] = _ij3[1];
6283 vinfos[3].maxsolutions = _nj3;
6284 vinfos[4].jointtype = 1;
6285 vinfos[4].foffset = j4;
6286 vinfos[4].indices[0] = _ij4[0];
6287 vinfos[4].indices[1] = _ij4[1];
6288 vinfos[4].maxsolutions = _nj4;
6289 vinfos[5].jointtype = 1;
6290 vinfos[5].foffset = j5;
6291 vinfos[5].indices[0] = _ij5[0];
6292 vinfos[5].indices[1] = _ij5[1];
6293 vinfos[5].maxsolutions = _nj5;
6294 std::vector<int> vfree(0);
6295 solutions.AddSolution(vinfos,vfree);
6296 }
6297 }
6298 }
6299 
6300 }
6301 
6302 }
6303 
6304 }
6305 } while(0);
6306 if( bgotonextstatement )
6307 {
6308 bool bgotonextstatement = true;
6309 do
6310 {
6311 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
6312 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6313 {
6314 bgotonextstatement=false;
6315 {
6316 IkReal j5eval[1];
6317 new_r21=0;
6318 new_r20=0;
6319 new_r02=0;
6320 new_r12=0;
6321 j5eval[0]=1.0;
6322 if( IKabs(j5eval[0]) < 0.0000000100000000 )
6323 {
6324 continue; // no branches [j5]
6325 
6326 } else
6327 {
6328 IkReal op[2+1], zeror[2];
6329 int numroots;
6330 op[0]=1.0;
6331 op[1]=0;
6332 op[2]=-1.0;
6333 polyroots2(op,zeror,numroots);
6334 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
6335 int numsolutions = 0;
6336 for(int ij5 = 0; ij5 < numroots; ++ij5)
6337 {
6338 IkReal htj5 = zeror[ij5];
6339 tempj5array[0]=((2.0)*(atan(htj5)));
6340 for(int kj5 = 0; kj5 < 1; ++kj5)
6341 {
6342 j5array[numsolutions] = tempj5array[kj5];
6343 if( j5array[numsolutions] > IKPI )
6344 {
6345  j5array[numsolutions]-=IK2PI;
6346 }
6347 else if( j5array[numsolutions] < -IKPI )
6348 {
6349  j5array[numsolutions]+=IK2PI;
6350 }
6351 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
6352 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
6353 numsolutions++;
6354 }
6355 }
6356 bool j5valid[2]={true,true};
6357 _nj5 = 2;
6358 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
6359  {
6360 if( !j5valid[ij5] )
6361 {
6362  continue;
6363 }
6364  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6365 htj5 = IKtan(j5/2);
6366 
6367 _ij5[0] = ij5; _ij5[1] = -1;
6368 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
6369 {
6370 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6371 {
6372  j5valid[iij5]=false; _ij5[1] = iij5; break;
6373 }
6374 }
6375 {
6376 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6377 vinfos[0].jointtype = 1;
6378 vinfos[0].foffset = j0;
6379 vinfos[0].indices[0] = _ij0[0];
6380 vinfos[0].indices[1] = _ij0[1];
6381 vinfos[0].maxsolutions = _nj0;
6382 vinfos[1].jointtype = 1;
6383 vinfos[1].foffset = j1;
6384 vinfos[1].indices[0] = _ij1[0];
6385 vinfos[1].indices[1] = _ij1[1];
6386 vinfos[1].maxsolutions = _nj1;
6387 vinfos[2].jointtype = 1;
6388 vinfos[2].foffset = j2;
6389 vinfos[2].indices[0] = _ij2[0];
6390 vinfos[2].indices[1] = _ij2[1];
6391 vinfos[2].maxsolutions = _nj2;
6392 vinfos[3].jointtype = 1;
6393 vinfos[3].foffset = j3;
6394 vinfos[3].indices[0] = _ij3[0];
6395 vinfos[3].indices[1] = _ij3[1];
6396 vinfos[3].maxsolutions = _nj3;
6397 vinfos[4].jointtype = 1;
6398 vinfos[4].foffset = j4;
6399 vinfos[4].indices[0] = _ij4[0];
6400 vinfos[4].indices[1] = _ij4[1];
6401 vinfos[4].maxsolutions = _nj4;
6402 vinfos[5].jointtype = 1;
6403 vinfos[5].foffset = j5;
6404 vinfos[5].indices[0] = _ij5[0];
6405 vinfos[5].indices[1] = _ij5[1];
6406 vinfos[5].maxsolutions = _nj5;
6407 std::vector<int> vfree(0);
6408 solutions.AddSolution(vinfos,vfree);
6409 }
6410  }
6411 
6412 }
6413 
6414 }
6415 
6416 }
6417 } while(0);
6418 if( bgotonextstatement )
6419 {
6420 bool bgotonextstatement = true;
6421 do
6422 {
6423 if( 1 )
6424 {
6425 bgotonextstatement=false;
6426 continue; // branch miss [j5]
6427 
6428 }
6429 } while(0);
6430 if( bgotonextstatement )
6431 {
6432 }
6433 }
6434 }
6435 }
6436 }
6437 }
6438 }
6439 }
6440 }
6441 }
6442 }
6443 
6444 } else
6445 {
6446 {
6447 IkReal j5array[1], cj5array[1], sj5array[1];
6448 bool j5valid[1]={false};
6449 _nj5 = 1;
6450 CheckValue<IkReal> x420=IKPowWithIntegerCheck(sj4,-1);
6451 if(!x420.valid){
6452 continue;
6453 }
6454 IkReal x419=x420.value;
6455 CheckValue<IkReal> x421=IKPowWithIntegerCheck(cj4,-1);
6456 if(!x421.valid){
6457 continue;
6458 }
6459 CheckValue<IkReal> x422=IKPowWithIntegerCheck(sj3,-1);
6460 if(!x422.valid){
6461 continue;
6462 }
6463 if( IKabs((new_r20*x419)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x419))+IKsqr((x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))))-1) <= IKFAST_SINCOS_THRESH )
6464  continue;
6465 j5array[0]=IKatan2((new_r20*x419), (x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))));
6466 sj5array[0]=IKsin(j5array[0]);
6467 cj5array[0]=IKcos(j5array[0]);
6468 if( j5array[0] > IKPI )
6469 {
6470  j5array[0]-=IK2PI;
6471 }
6472 else if( j5array[0] < -IKPI )
6473 { j5array[0]+=IK2PI;
6474 }
6475 j5valid[0] = true;
6476 for(int ij5 = 0; ij5 < 1; ++ij5)
6477 {
6478 if( !j5valid[ij5] )
6479 {
6480  continue;
6481 }
6482 _ij5[0] = ij5; _ij5[1] = -1;
6483 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6484 {
6485 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6486 {
6487  j5valid[iij5]=false; _ij5[1] = iij5; break;
6488 }
6489 }
6490 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6491 {
6492 IkReal evalcond[12];
6493 IkReal x423=IKsin(j5);
6494 IkReal x424=IKcos(j5);
6495 IkReal x425=(cj4*sj3);
6496 IkReal x426=(cj3*new_r10);
6497 IkReal x427=((1.0)*new_r01);
6498 IkReal x428=(cj3*new_r11);
6499 IkReal x429=((1.0)*new_r00);
6500 IkReal x430=((1.0)*x424);
6501 IkReal x431=(cj4*x423);
6502 IkReal x432=((1.0)*x423);
6503 evalcond[0]=(new_r20+(((-1.0)*sj4*x432)));
6504 evalcond[1]=(new_r21+(((-1.0)*sj4*x430)));
6505 evalcond[2]=(((new_r11*sj3))+x423+((cj3*new_r01)));
6506 evalcond[3]=((((-1.0)*x430))+((new_r10*sj3))+((cj3*new_r00)));
6507 evalcond[4]=(((x424*x425))+((cj3*x423))+new_r01);
6508 evalcond[5]=((((-1.0)*cj3*x430))+((x423*x425))+new_r00);
6509 evalcond[6]=((((-1.0)*cj3*cj4*x430))+((sj3*x423))+new_r11);
6510 evalcond[7]=((((-1.0)*sj3*x429))+(((-1.0)*x431))+x426);
6511 evalcond[8]=((((-1.0)*sj3*x427))+(((-1.0)*cj4*x430))+x428);
6512 evalcond[9]=((((-1.0)*sj3*x430))+(((-1.0)*cj3*x431))+new_r10);
6513 evalcond[10]=((((-1.0)*x432))+((new_r20*sj4))+((cj4*x426))+(((-1.0)*x425*x429)));
6514 evalcond[11]=((((-1.0)*x430))+((cj4*x428))+(((-1.0)*x425*x427))+((new_r21*sj4)));
6515 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
6516 {
6517 continue;
6518 }
6519 }
6520 
6521 {
6522 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6523 vinfos[0].jointtype = 1;
6524 vinfos[0].foffset = j0;
6525 vinfos[0].indices[0] = _ij0[0];
6526 vinfos[0].indices[1] = _ij0[1];
6527 vinfos[0].maxsolutions = _nj0;
6528 vinfos[1].jointtype = 1;
6529 vinfos[1].foffset = j1;
6530 vinfos[1].indices[0] = _ij1[0];
6531 vinfos[1].indices[1] = _ij1[1];
6532 vinfos[1].maxsolutions = _nj1;
6533 vinfos[2].jointtype = 1;
6534 vinfos[2].foffset = j2;
6535 vinfos[2].indices[0] = _ij2[0];
6536 vinfos[2].indices[1] = _ij2[1];
6537 vinfos[2].maxsolutions = _nj2;
6538 vinfos[3].jointtype = 1;
6539 vinfos[3].foffset = j3;
6540 vinfos[3].indices[0] = _ij3[0];
6541 vinfos[3].indices[1] = _ij3[1];
6542 vinfos[3].maxsolutions = _nj3;
6543 vinfos[4].jointtype = 1;
6544 vinfos[4].foffset = j4;
6545 vinfos[4].indices[0] = _ij4[0];
6546 vinfos[4].indices[1] = _ij4[1];
6547 vinfos[4].maxsolutions = _nj4;
6548 vinfos[5].jointtype = 1;
6549 vinfos[5].foffset = j5;
6550 vinfos[5].indices[0] = _ij5[0];
6551 vinfos[5].indices[1] = _ij5[1];
6552 vinfos[5].maxsolutions = _nj5;
6553 std::vector<int> vfree(0);
6554 solutions.AddSolution(vinfos,vfree);
6555 }
6556 }
6557 }
6558 
6559 }
6560 
6561 }
6562 
6563 } else
6564 {
6565 {
6566 IkReal j5array[1], cj5array[1], sj5array[1];
6567 bool j5valid[1]={false};
6568 _nj5 = 1;
6569 CheckValue<IkReal> x434=IKPowWithIntegerCheck(sj4,-1);
6570 if(!x434.valid){
6571 continue;
6572 }
6573 IkReal x433=x434.value;
6574 CheckValue<IkReal> x435=IKPowWithIntegerCheck(cj3,-1);
6575 if(!x435.valid){
6576 continue;
6577 }
6578 if( IKabs((new_r20*x433)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x433))+IKsqr((x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
6579  continue;
6580 j5array[0]=IKatan2((new_r20*x433), (x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))));
6581 sj5array[0]=IKsin(j5array[0]);
6582 cj5array[0]=IKcos(j5array[0]);
6583 if( j5array[0] > IKPI )
6584 {
6585  j5array[0]-=IK2PI;
6586 }
6587 else if( j5array[0] < -IKPI )
6588 { j5array[0]+=IK2PI;
6589 }
6590 j5valid[0] = true;
6591 for(int ij5 = 0; ij5 < 1; ++ij5)
6592 {
6593 if( !j5valid[ij5] )
6594 {
6595  continue;
6596 }
6597 _ij5[0] = ij5; _ij5[1] = -1;
6598 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6599 {
6600 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6601 {
6602  j5valid[iij5]=false; _ij5[1] = iij5; break;
6603 }
6604 }
6605 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6606 {
6607 IkReal evalcond[12];
6608 IkReal x436=IKsin(j5);
6609 IkReal x437=IKcos(j5);
6610 IkReal x438=(cj4*sj3);
6611 IkReal x439=(cj3*new_r10);
6612 IkReal x440=((1.0)*new_r01);
6613 IkReal x441=(cj3*new_r11);
6614 IkReal x442=((1.0)*new_r00);
6615 IkReal x443=((1.0)*x437);
6616 IkReal x444=(cj4*x436);
6617 IkReal x445=((1.0)*x436);
6618 evalcond[0]=((((-1.0)*sj4*x445))+new_r20);
6619 evalcond[1]=((((-1.0)*sj4*x443))+new_r21);
6620 evalcond[2]=(((new_r11*sj3))+x436+((cj3*new_r01)));
6621 evalcond[3]=((((-1.0)*x443))+((new_r10*sj3))+((cj3*new_r00)));
6622 evalcond[4]=(((cj3*x436))+new_r01+((x437*x438)));
6623 evalcond[5]=(((x436*x438))+(((-1.0)*cj3*x443))+new_r00);
6624 evalcond[6]=(((sj3*x436))+(((-1.0)*cj3*cj4*x443))+new_r11);
6625 evalcond[7]=((((-1.0)*sj3*x442))+(((-1.0)*x444))+x439);
6626 evalcond[8]=((((-1.0)*sj3*x440))+(((-1.0)*cj4*x443))+x441);
6627 evalcond[9]=((((-1.0)*sj3*x443))+(((-1.0)*cj3*x444))+new_r10);
6628 evalcond[10]=((((-1.0)*x445))+(((-1.0)*x438*x442))+((new_r20*sj4))+((cj4*x439)));
6629 evalcond[11]=((((-1.0)*x443))+(((-1.0)*x438*x440))+((cj4*x441))+((new_r21*sj4)));
6630 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
6631 {
6632 continue;
6633 }
6634 }
6635 
6636 {
6637 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6638 vinfos[0].jointtype = 1;
6639 vinfos[0].foffset = j0;
6640 vinfos[0].indices[0] = _ij0[0];
6641 vinfos[0].indices[1] = _ij0[1];
6642 vinfos[0].maxsolutions = _nj0;
6643 vinfos[1].jointtype = 1;
6644 vinfos[1].foffset = j1;
6645 vinfos[1].indices[0] = _ij1[0];
6646 vinfos[1].indices[1] = _ij1[1];
6647 vinfos[1].maxsolutions = _nj1;
6648 vinfos[2].jointtype = 1;
6649 vinfos[2].foffset = j2;
6650 vinfos[2].indices[0] = _ij2[0];
6651 vinfos[2].indices[1] = _ij2[1];
6652 vinfos[2].maxsolutions = _nj2;
6653 vinfos[3].jointtype = 1;
6654 vinfos[3].foffset = j3;
6655 vinfos[3].indices[0] = _ij3[0];
6656 vinfos[3].indices[1] = _ij3[1];
6657 vinfos[3].maxsolutions = _nj3;
6658 vinfos[4].jointtype = 1;
6659 vinfos[4].foffset = j4;
6660 vinfos[4].indices[0] = _ij4[0];
6661 vinfos[4].indices[1] = _ij4[1];
6662 vinfos[4].maxsolutions = _nj4;
6663 vinfos[5].jointtype = 1;
6664 vinfos[5].foffset = j5;
6665 vinfos[5].indices[0] = _ij5[0];
6666 vinfos[5].indices[1] = _ij5[1];
6667 vinfos[5].maxsolutions = _nj5;
6668 std::vector<int> vfree(0);
6669 solutions.AddSolution(vinfos,vfree);
6670 }
6671 }
6672 }
6673 
6674 }
6675 
6676 }
6677 
6678 } else
6679 {
6680 {
6681 IkReal j5array[1], cj5array[1], sj5array[1];
6682 bool j5valid[1]={false};
6683 _nj5 = 1;
6684 CheckValue<IkReal> x446=IKPowWithIntegerCheck(IKsign(sj4),-1);
6685 if(!x446.valid){
6686 continue;
6687 }
6688 CheckValue<IkReal> x447 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
6689 if(!x447.valid){
6690 continue;
6691 }
6692 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x446.value)))+(x447.value));
6693 sj5array[0]=IKsin(j5array[0]);
6694 cj5array[0]=IKcos(j5array[0]);
6695 if( j5array[0] > IKPI )
6696 {
6697  j5array[0]-=IK2PI;
6698 }
6699 else if( j5array[0] < -IKPI )
6700 { j5array[0]+=IK2PI;
6701 }
6702 j5valid[0] = true;
6703 for(int ij5 = 0; ij5 < 1; ++ij5)
6704 {
6705 if( !j5valid[ij5] )
6706 {
6707  continue;
6708 }
6709 _ij5[0] = ij5; _ij5[1] = -1;
6710 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6711 {
6712 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6713 {
6714  j5valid[iij5]=false; _ij5[1] = iij5; break;
6715 }
6716 }
6717 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6718 {
6719 IkReal evalcond[12];
6720 IkReal x448=IKsin(j5);
6721 IkReal x449=IKcos(j5);
6722 IkReal x450=(cj4*sj3);
6723 IkReal x451=(cj3*new_r10);
6724 IkReal x452=((1.0)*new_r01);
6725 IkReal x453=(cj3*new_r11);
6726 IkReal x454=((1.0)*new_r00);
6727 IkReal x455=((1.0)*x449);
6728 IkReal x456=(cj4*x448);
6729 IkReal x457=((1.0)*x448);
6730 evalcond[0]=((((-1.0)*sj4*x457))+new_r20);
6731 evalcond[1]=((((-1.0)*sj4*x455))+new_r21);
6732 evalcond[2]=(((new_r11*sj3))+x448+((cj3*new_r01)));
6733 evalcond[3]=(((new_r10*sj3))+(((-1.0)*x455))+((cj3*new_r00)));
6734 evalcond[4]=(((x449*x450))+((cj3*x448))+new_r01);
6735 evalcond[5]=(((x448*x450))+new_r00+(((-1.0)*cj3*x455)));
6736 evalcond[6]=((((-1.0)*cj3*cj4*x455))+((sj3*x448))+new_r11);
6737 evalcond[7]=((((-1.0)*x456))+(((-1.0)*sj3*x454))+x451);
6738 evalcond[8]=((((-1.0)*cj4*x455))+(((-1.0)*sj3*x452))+x453);
6739 evalcond[9]=((((-1.0)*sj3*x455))+new_r10+(((-1.0)*cj3*x456)));
6740 evalcond[10]=(((new_r20*sj4))+((cj4*x451))+(((-1.0)*x457))+(((-1.0)*x450*x454)));
6741 evalcond[11]=(((cj4*x453))+(((-1.0)*x455))+(((-1.0)*x450*x452))+((new_r21*sj4)));
6742 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
6743 {
6744 continue;
6745 }
6746 }
6747 
6748 {
6749 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6750 vinfos[0].jointtype = 1;
6751 vinfos[0].foffset = j0;
6752 vinfos[0].indices[0] = _ij0[0];
6753 vinfos[0].indices[1] = _ij0[1];
6754 vinfos[0].maxsolutions = _nj0;
6755 vinfos[1].jointtype = 1;
6756 vinfos[1].foffset = j1;
6757 vinfos[1].indices[0] = _ij1[0];
6758 vinfos[1].indices[1] = _ij1[1];
6759 vinfos[1].maxsolutions = _nj1;
6760 vinfos[2].jointtype = 1;
6761 vinfos[2].foffset = j2;
6762 vinfos[2].indices[0] = _ij2[0];
6763 vinfos[2].indices[1] = _ij2[1];
6764 vinfos[2].maxsolutions = _nj2;
6765 vinfos[3].jointtype = 1;
6766 vinfos[3].foffset = j3;
6767 vinfos[3].indices[0] = _ij3[0];
6768 vinfos[3].indices[1] = _ij3[1];
6769 vinfos[3].maxsolutions = _nj3;
6770 vinfos[4].jointtype = 1;
6771 vinfos[4].foffset = j4;
6772 vinfos[4].indices[0] = _ij4[0];
6773 vinfos[4].indices[1] = _ij4[1];
6774 vinfos[4].maxsolutions = _nj4;
6775 vinfos[5].jointtype = 1;
6776 vinfos[5].foffset = j5;
6777 vinfos[5].indices[0] = _ij5[0];
6778 vinfos[5].indices[1] = _ij5[1];
6779 vinfos[5].maxsolutions = _nj5;
6780 std::vector<int> vfree(0);
6781 solutions.AddSolution(vinfos,vfree);
6782 }
6783 }
6784 }
6785 
6786 }
6787 
6788 }
6789 }
6790 }
6791 
6792 }
6793 
6794 }
6795 
6796 } else
6797 {
6798 {
6799 IkReal j5array[1], cj5array[1], sj5array[1];
6800 bool j5valid[1]={false};
6801 _nj5 = 1;
6802 CheckValue<IkReal> x458=IKPowWithIntegerCheck(IKsign(sj4),-1);
6803 if(!x458.valid){
6804 continue;
6805 }
6806 CheckValue<IkReal> x459 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH);
6807 if(!x459.valid){
6808 continue;
6809 }
6810 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x458.value)))+(x459.value));
6811 sj5array[0]=IKsin(j5array[0]);
6812 cj5array[0]=IKcos(j5array[0]);
6813 if( j5array[0] > IKPI )
6814 {
6815  j5array[0]-=IK2PI;
6816 }
6817 else if( j5array[0] < -IKPI )
6818 { j5array[0]+=IK2PI;
6819 }
6820 j5valid[0] = true;
6821 for(int ij5 = 0; ij5 < 1; ++ij5)
6822 {
6823 if( !j5valid[ij5] )
6824 {
6825  continue;
6826 }
6827 _ij5[0] = ij5; _ij5[1] = -1;
6828 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
6829 {
6830 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
6831 {
6832  j5valid[iij5]=false; _ij5[1] = iij5; break;
6833 }
6834 }
6835 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
6836 {
6837 IkReal evalcond[2];
6838 IkReal x460=((1.0)*sj4);
6839 evalcond[0]=((((-1.0)*x460*(IKsin(j5))))+new_r20);
6840 evalcond[1]=((((-1.0)*x460*(IKcos(j5))))+new_r21);
6841 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
6842 {
6843 continue;
6844 }
6845 }
6846 
6847 {
6848 IkReal j3eval[3];
6849 j3eval[0]=sj4;
6850 j3eval[1]=IKsign(sj4);
6851 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
6852 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
6853 {
6854 {
6855 IkReal j3eval[2];
6856 j3eval[0]=new_r11;
6857 j3eval[1]=sj4;
6858 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
6859 {
6860 {
6861 IkReal evalcond[5];
6862 bool bgotonextstatement = true;
6863 do
6864 {
6865 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
6866 evalcond[1]=new_r12;
6867 evalcond[2]=new_r02;
6868 evalcond[3]=new_r20;
6869 evalcond[4]=new_r21;
6870 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 )
6871 {
6872 bgotonextstatement=false;
6873 {
6874 IkReal j3array[1], cj3array[1], sj3array[1];
6875 bool j3valid[1]={false};
6876 _nj3 = 1;
6877 IkReal x461=((1.0)*new_r01);
6878 if( IKabs(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj5*x461))+((cj5*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5))))+IKsqr(((((-1.0)*sj5*x461))+((cj5*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
6879  continue;
6880 j3array[0]=IKatan2(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5))), ((((-1.0)*sj5*x461))+((cj5*new_r00))));
6881 sj3array[0]=IKsin(j3array[0]);
6882 cj3array[0]=IKcos(j3array[0]);
6883 if( j3array[0] > IKPI )
6884 {
6885  j3array[0]-=IK2PI;
6886 }
6887 else if( j3array[0] < -IKPI )
6888 { j3array[0]+=IK2PI;
6889 }
6890 j3valid[0] = true;
6891 for(int ij3 = 0; ij3 < 1; ++ij3)
6892 {
6893 if( !j3valid[ij3] )
6894 {
6895  continue;
6896 }
6897 _ij3[0] = ij3; _ij3[1] = -1;
6898 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6899 {
6900 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6901 {
6902  j3valid[iij3]=false; _ij3[1] = iij3; break;
6903 }
6904 }
6905 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6906 {
6907 IkReal evalcond[8];
6908 IkReal x462=IKcos(j3);
6909 IkReal x463=IKsin(j3);
6910 IkReal x464=((1.0)*cj5);
6911 IkReal x465=(sj5*x463);
6912 IkReal x466=(sj5*x462);
6913 IkReal x467=((1.0)*x463);
6914 IkReal x468=(x462*x464);
6915 evalcond[0]=(((new_r11*x463))+sj5+((new_r01*x462)));
6916 evalcond[1]=(((cj5*x463))+x466+new_r01);
6917 evalcond[2]=((((-1.0)*x468))+x465+new_r00);
6918 evalcond[3]=((((-1.0)*x468))+x465+new_r11);
6919 evalcond[4]=(((new_r10*x463))+((new_r00*x462))+(((-1.0)*x464)));
6920 evalcond[5]=((((-1.0)*x463*x464))+(((-1.0)*x466))+new_r10);
6921 evalcond[6]=((((-1.0)*sj5))+((new_r10*x462))+(((-1.0)*new_r00*x467)));
6922 evalcond[7]=(((new_r11*x462))+(((-1.0)*new_r01*x467))+(((-1.0)*x464)));
6923 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6924 {
6925 continue;
6926 }
6927 }
6928 
6929 {
6930 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6931 vinfos[0].jointtype = 1;
6932 vinfos[0].foffset = j0;
6933 vinfos[0].indices[0] = _ij0[0];
6934 vinfos[0].indices[1] = _ij0[1];
6935 vinfos[0].maxsolutions = _nj0;
6936 vinfos[1].jointtype = 1;
6937 vinfos[1].foffset = j1;
6938 vinfos[1].indices[0] = _ij1[0];
6939 vinfos[1].indices[1] = _ij1[1];
6940 vinfos[1].maxsolutions = _nj1;
6941 vinfos[2].jointtype = 1;
6942 vinfos[2].foffset = j2;
6943 vinfos[2].indices[0] = _ij2[0];
6944 vinfos[2].indices[1] = _ij2[1];
6945 vinfos[2].maxsolutions = _nj2;
6946 vinfos[3].jointtype = 1;
6947 vinfos[3].foffset = j3;
6948 vinfos[3].indices[0] = _ij3[0];
6949 vinfos[3].indices[1] = _ij3[1];
6950 vinfos[3].maxsolutions = _nj3;
6951 vinfos[4].jointtype = 1;
6952 vinfos[4].foffset = j4;
6953 vinfos[4].indices[0] = _ij4[0];
6954 vinfos[4].indices[1] = _ij4[1];
6955 vinfos[4].maxsolutions = _nj4;
6956 vinfos[5].jointtype = 1;
6957 vinfos[5].foffset = j5;
6958 vinfos[5].indices[0] = _ij5[0];
6959 vinfos[5].indices[1] = _ij5[1];
6960 vinfos[5].maxsolutions = _nj5;
6961 std::vector<int> vfree(0);
6962 solutions.AddSolution(vinfos,vfree);
6963 }
6964 }
6965 }
6966 
6967 }
6968 } while(0);
6969 if( bgotonextstatement )
6970 {
6971 bool bgotonextstatement = true;
6972 do
6973 {
6974 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
6975 evalcond[1]=new_r12;
6976 evalcond[2]=new_r02;
6977 evalcond[3]=new_r20;
6978 evalcond[4]=new_r21;
6979 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 )
6980 {
6981 bgotonextstatement=false;
6982 {
6983 IkReal j3eval[3];
6984 sj4=0;
6985 cj4=-1.0;
6986 j4=3.14159265358979;
6987 IkReal x469=(((new_r11*sj5))+((cj5*new_r01)));
6988 j3eval[0]=x469;
6989 j3eval[1]=IKsign(x469);
6990 j3eval[2]=((IKabs(((((-1.0)*cj5*sj5))+(((-1.0)*new_r01*new_r11)))))+(IKabs(((-1.0)+(new_r01*new_r01)+(cj5*cj5)))));
6991 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
6992 {
6993 {
6994 IkReal j3eval[3];
6995 sj4=0;
6996 cj4=-1.0;
6997 j4=3.14159265358979;
6998 IkReal x470=((1.0)*sj5);
6999 IkReal x471=(((new_r10*new_r11))+((new_r00*new_r01)));
7000 j3eval[0]=x471;
7001 j3eval[1]=IKsign(x471);
7002 j3eval[2]=((IKabs(((((-1.0)*new_r00*x470))+(((-1.0)*new_r11*x470)))))+(IKabs((((new_r01*sj5))+(((-1.0)*new_r10*x470))))));
7003 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7004 {
7005 {
7006 IkReal j3eval[3];
7007 sj4=0;
7008 cj4=-1.0;
7009 j4=3.14159265358979;
7010 IkReal x472=((1.0)*sj5);
7011 IkReal x473=((new_r01*new_r01)+(new_r11*new_r11));
7012 j3eval[0]=x473;
7013 j3eval[1]=((IKabs((((cj5*new_r01))+(((-1.0)*new_r11*x472)))))+(IKabs(((((-1.0)*new_r01*x472))+(((-1.0)*cj5*new_r11))))));
7014 j3eval[2]=IKsign(x473);
7015 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7016 {
7017 {
7018 IkReal evalcond[1];
7019 bool bgotonextstatement = true;
7020 do
7021 {
7022 evalcond[0]=((new_r01*new_r01)+(new_r11*new_r11));
7023 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7024 {
7025 bgotonextstatement=false;
7026 {
7027 IkReal j3eval[1];
7028 sj4=0;
7029 cj4=-1.0;
7030 j4=3.14159265358979;
7031 new_r01=0;
7032 new_r11=0;
7033 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7034 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7035 {
7036 continue; // 3 cases reached
7037 
7038 } else
7039 {
7040 {
7041 IkReal j3array[2], cj3array[2], sj3array[2];
7042 bool j3valid[2]={false};
7043 _nj3 = 2;
7044 CheckValue<IkReal> x475 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
7045 if(!x475.valid){
7046 continue;
7047 }
7048 IkReal x474=x475.value;
7049 j3array[0]=((-1.0)*x474);
7050 sj3array[0]=IKsin(j3array[0]);
7051 cj3array[0]=IKcos(j3array[0]);
7052 j3array[1]=((3.14159265358979)+(((-1.0)*x474)));
7053 sj3array[1]=IKsin(j3array[1]);
7054 cj3array[1]=IKcos(j3array[1]);
7055 if( j3array[0] > IKPI )
7056 {
7057  j3array[0]-=IK2PI;
7058 }
7059 else if( j3array[0] < -IKPI )
7060 { j3array[0]+=IK2PI;
7061 }
7062 j3valid[0] = true;
7063 if( j3array[1] > IKPI )
7064 {
7065  j3array[1]-=IK2PI;
7066 }
7067 else if( j3array[1] < -IKPI )
7068 { j3array[1]+=IK2PI;
7069 }
7070 j3valid[1] = true;
7071 for(int ij3 = 0; ij3 < 2; ++ij3)
7072 {
7073 if( !j3valid[ij3] )
7074 {
7075  continue;
7076 }
7077 _ij3[0] = ij3; _ij3[1] = -1;
7078 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7079 {
7080 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7081 {
7082  j3valid[iij3]=false; _ij3[1] = iij3; break;
7083 }
7084 }
7085 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7086 {
7087 IkReal evalcond[1];
7088 evalcond[0]=((((-1.0)*new_r00*(IKsin(j3))))+((new_r10*(IKcos(j3)))));
7089 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
7090 {
7091 continue;
7092 }
7093 }
7094 
7095 {
7096 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7097 vinfos[0].jointtype = 1;
7098 vinfos[0].foffset = j0;
7099 vinfos[0].indices[0] = _ij0[0];
7100 vinfos[0].indices[1] = _ij0[1];
7101 vinfos[0].maxsolutions = _nj0;
7102 vinfos[1].jointtype = 1;
7103 vinfos[1].foffset = j1;
7104 vinfos[1].indices[0] = _ij1[0];
7105 vinfos[1].indices[1] = _ij1[1];
7106 vinfos[1].maxsolutions = _nj1;
7107 vinfos[2].jointtype = 1;
7108 vinfos[2].foffset = j2;
7109 vinfos[2].indices[0] = _ij2[0];
7110 vinfos[2].indices[1] = _ij2[1];
7111 vinfos[2].maxsolutions = _nj2;
7112 vinfos[3].jointtype = 1;
7113 vinfos[3].foffset = j3;
7114 vinfos[3].indices[0] = _ij3[0];
7115 vinfos[3].indices[1] = _ij3[1];
7116 vinfos[3].maxsolutions = _nj3;
7117 vinfos[4].jointtype = 1;
7118 vinfos[4].foffset = j4;
7119 vinfos[4].indices[0] = _ij4[0];
7120 vinfos[4].indices[1] = _ij4[1];
7121 vinfos[4].maxsolutions = _nj4;
7122 vinfos[5].jointtype = 1;
7123 vinfos[5].foffset = j5;
7124 vinfos[5].indices[0] = _ij5[0];
7125 vinfos[5].indices[1] = _ij5[1];
7126 vinfos[5].maxsolutions = _nj5;
7127 std::vector<int> vfree(0);
7128 solutions.AddSolution(vinfos,vfree);
7129 }
7130 }
7131 }
7132 
7133 }
7134 
7135 }
7136 
7137 }
7138 } while(0);
7139 if( bgotonextstatement )
7140 {
7141 bool bgotonextstatement = true;
7142 do
7143 {
7144 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
7145 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7146 {
7147 bgotonextstatement=false;
7148 {
7149 IkReal j3array[1], cj3array[1], sj3array[1];
7150 bool j3valid[1]={false};
7151 _nj3 = 1;
7152 if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
7153  continue;
7154 j3array[0]=IKatan2(new_r01, ((-1.0)*new_r11));
7155 sj3array[0]=IKsin(j3array[0]);
7156 cj3array[0]=IKcos(j3array[0]);
7157 if( j3array[0] > IKPI )
7158 {
7159  j3array[0]-=IK2PI;
7160 }
7161 else if( j3array[0] < -IKPI )
7162 { j3array[0]+=IK2PI;
7163 }
7164 j3valid[0] = true;
7165 for(int ij3 = 0; ij3 < 1; ++ij3)
7166 {
7167 if( !j3valid[ij3] )
7168 {
7169  continue;
7170 }
7171 _ij3[0] = ij3; _ij3[1] = -1;
7172 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7173 {
7174 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7175 {
7176  j3valid[iij3]=false; _ij3[1] = iij3; break;
7177 }
7178 }
7179 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7180 {
7181 IkReal evalcond[8];
7182 IkReal x476=IKcos(j3);
7183 IkReal x477=IKsin(j3);
7184 IkReal x478=((1.0)*x477);
7185 evalcond[0]=(x476+new_r11);
7186 evalcond[1]=((((-1.0)*x478))+new_r01);
7187 evalcond[2]=((((-1.0)*x476))+new_r00);
7188 evalcond[3]=((((-1.0)*x478))+new_r10);
7189 evalcond[4]=(((new_r01*x476))+((new_r11*x477)));
7190 evalcond[5]=((-1.0)+((new_r10*x477))+((new_r00*x476)));
7191 evalcond[6]=(((new_r10*x476))+(((-1.0)*new_r00*x478)));
7192 evalcond[7]=((1.0)+((new_r11*x476))+(((-1.0)*new_r01*x478)));
7193 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7194 {
7195 continue;
7196 }
7197 }
7198 
7199 {
7200 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7201 vinfos[0].jointtype = 1;
7202 vinfos[0].foffset = j0;
7203 vinfos[0].indices[0] = _ij0[0];
7204 vinfos[0].indices[1] = _ij0[1];
7205 vinfos[0].maxsolutions = _nj0;
7206 vinfos[1].jointtype = 1;
7207 vinfos[1].foffset = j1;
7208 vinfos[1].indices[0] = _ij1[0];
7209 vinfos[1].indices[1] = _ij1[1];
7210 vinfos[1].maxsolutions = _nj1;
7211 vinfos[2].jointtype = 1;
7212 vinfos[2].foffset = j2;
7213 vinfos[2].indices[0] = _ij2[0];
7214 vinfos[2].indices[1] = _ij2[1];
7215 vinfos[2].maxsolutions = _nj2;
7216 vinfos[3].jointtype = 1;
7217 vinfos[3].foffset = j3;
7218 vinfos[3].indices[0] = _ij3[0];
7219 vinfos[3].indices[1] = _ij3[1];
7220 vinfos[3].maxsolutions = _nj3;
7221 vinfos[4].jointtype = 1;
7222 vinfos[4].foffset = j4;
7223 vinfos[4].indices[0] = _ij4[0];
7224 vinfos[4].indices[1] = _ij4[1];
7225 vinfos[4].maxsolutions = _nj4;
7226 vinfos[5].jointtype = 1;
7227 vinfos[5].foffset = j5;
7228 vinfos[5].indices[0] = _ij5[0];
7229 vinfos[5].indices[1] = _ij5[1];
7230 vinfos[5].maxsolutions = _nj5;
7231 std::vector<int> vfree(0);
7232 solutions.AddSolution(vinfos,vfree);
7233 }
7234 }
7235 }
7236 
7237 }
7238 } while(0);
7239 if( bgotonextstatement )
7240 {
7241 bool bgotonextstatement = true;
7242 do
7243 {
7244 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
7245 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7246 {
7247 bgotonextstatement=false;
7248 {
7249 IkReal j3array[1], cj3array[1], sj3array[1];
7250 bool j3valid[1]={false};
7251 _nj3 = 1;
7252 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 )
7253  continue;
7254 j3array[0]=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
7255 sj3array[0]=IKsin(j3array[0]);
7256 cj3array[0]=IKcos(j3array[0]);
7257 if( j3array[0] > IKPI )
7258 {
7259  j3array[0]-=IK2PI;
7260 }
7261 else if( j3array[0] < -IKPI )
7262 { j3array[0]+=IK2PI;
7263 }
7264 j3valid[0] = true;
7265 for(int ij3 = 0; ij3 < 1; ++ij3)
7266 {
7267 if( !j3valid[ij3] )
7268 {
7269  continue;
7270 }
7271 _ij3[0] = ij3; _ij3[1] = -1;
7272 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7273 {
7274 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7275 {
7276  j3valid[iij3]=false; _ij3[1] = iij3; break;
7277 }
7278 }
7279 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7280 {
7281 IkReal evalcond[8];
7282 IkReal x479=IKsin(j3);
7283 IkReal x480=IKcos(j3);
7284 IkReal x481=((1.0)*x479);
7285 evalcond[0]=(x479+new_r01);
7286 evalcond[1]=(x480+new_r00);
7287 evalcond[2]=(x479+new_r10);
7288 evalcond[3]=((((-1.0)*x480))+new_r11);
7289 evalcond[4]=(((new_r11*x479))+((new_r01*x480)));
7290 evalcond[5]=((1.0)+((new_r10*x479))+((new_r00*x480)));
7291 evalcond[6]=((((-1.0)*new_r00*x481))+((new_r10*x480)));
7292 evalcond[7]=((-1.0)+(((-1.0)*new_r01*x481))+((new_r11*x480)));
7293 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7294 {
7295 continue;
7296 }
7297 }
7298 
7299 {
7300 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7301 vinfos[0].jointtype = 1;
7302 vinfos[0].foffset = j0;
7303 vinfos[0].indices[0] = _ij0[0];
7304 vinfos[0].indices[1] = _ij0[1];
7305 vinfos[0].maxsolutions = _nj0;
7306 vinfos[1].jointtype = 1;
7307 vinfos[1].foffset = j1;
7308 vinfos[1].indices[0] = _ij1[0];
7309 vinfos[1].indices[1] = _ij1[1];
7310 vinfos[1].maxsolutions = _nj1;
7311 vinfos[2].jointtype = 1;
7312 vinfos[2].foffset = j2;
7313 vinfos[2].indices[0] = _ij2[0];
7314 vinfos[2].indices[1] = _ij2[1];
7315 vinfos[2].maxsolutions = _nj2;
7316 vinfos[3].jointtype = 1;
7317 vinfos[3].foffset = j3;
7318 vinfos[3].indices[0] = _ij3[0];
7319 vinfos[3].indices[1] = _ij3[1];
7320 vinfos[3].maxsolutions = _nj3;
7321 vinfos[4].jointtype = 1;
7322 vinfos[4].foffset = j4;
7323 vinfos[4].indices[0] = _ij4[0];
7324 vinfos[4].indices[1] = _ij4[1];
7325 vinfos[4].maxsolutions = _nj4;
7326 vinfos[5].jointtype = 1;
7327 vinfos[5].foffset = j5;
7328 vinfos[5].indices[0] = _ij5[0];
7329 vinfos[5].indices[1] = _ij5[1];
7330 vinfos[5].maxsolutions = _nj5;
7331 std::vector<int> vfree(0);
7332 solutions.AddSolution(vinfos,vfree);
7333 }
7334 }
7335 }
7336 
7337 }
7338 } while(0);
7339 if( bgotonextstatement )
7340 {
7341 bool bgotonextstatement = true;
7342 do
7343 {
7344 IkReal x483 = ((new_r01*new_r01)+(new_r11*new_r11));
7345 if(IKabs(x483)==0){
7346 continue;
7347 }
7348 IkReal x482=pow(x483,-0.5);
7349 CheckValue<IkReal> x484 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7350 if(!x484.valid){
7351 continue;
7352 }
7353 IkReal gconst6=((-1.0)*(x484.value));
7354 IkReal gconst7=((-1.0)*new_r01*x482);
7355 IkReal gconst8=(new_r11*x482);
7356 CheckValue<IkReal> x485 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7357 if(!x485.valid){
7358 continue;
7359 }
7360 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x485.value)+j5)))), 6.28318530717959)));
7361 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7362 {
7363 bgotonextstatement=false;
7364 {
7365 IkReal j3eval[3];
7366 CheckValue<IkReal> x488 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7367 if(!x488.valid){
7368 continue;
7369 }
7370 IkReal x486=((-1.0)*(x488.value));
7371 IkReal x487=x482;
7372 sj4=0;
7373 cj4=-1.0;
7374 j4=3.14159265358979;
7375 sj5=gconst7;
7376 cj5=gconst8;
7377 j5=x486;
7378 IkReal gconst6=x486;
7379 IkReal gconst7=((-1.0)*new_r01*x487);
7380 IkReal gconst8=(new_r11*x487);
7381 IkReal x489=new_r01*new_r01;
7382 IkReal x490=(new_r00*new_r01);
7383 IkReal x491=(((new_r10*new_r11))+x490);
7384 IkReal x492=x482;
7385 IkReal x493=(new_r01*x492);
7386 j3eval[0]=x491;
7387 j3eval[1]=IKsign(x491);
7388 j3eval[2]=((IKabs(((((-1.0)*x489*x492))+((new_r10*x493)))))+(IKabs((((x490*x492))+((new_r11*x493))))));
7389 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7390 {
7391 {
7392 IkReal j3eval[2];
7393 CheckValue<IkReal> x496 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7394 if(!x496.valid){
7395 continue;
7396 }
7397 IkReal x494=((-1.0)*(x496.value));
7398 IkReal x495=x482;
7399 sj4=0;
7400 cj4=-1.0;
7401 j4=3.14159265358979;
7402 sj5=gconst7;
7403 cj5=gconst8;
7404 j5=x494;
7405 IkReal gconst6=x494;
7406 IkReal gconst7=((-1.0)*new_r01*x495);
7407 IkReal gconst8=(new_r11*x495);
7408 IkReal x497=((new_r01*new_r01)+(new_r11*new_r11));
7409 j3eval[0]=x497;
7410 j3eval[1]=IKsign(x497);
7411 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
7412 {
7413 {
7414 IkReal j3eval[1];
7415 CheckValue<IkReal> x500 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7416 if(!x500.valid){
7417 continue;
7418 }
7419 IkReal x498=((-1.0)*(x500.value));
7420 IkReal x499=x482;
7421 sj4=0;
7422 cj4=-1.0;
7423 j4=3.14159265358979;
7424 sj5=gconst7;
7425 cj5=gconst8;
7426 j5=x498;
7427 IkReal gconst6=x498;
7428 IkReal gconst7=((-1.0)*new_r01*x499);
7429 IkReal gconst8=(new_r11*x499);
7430 j3eval[0]=((new_r01*new_r01)+(new_r11*new_r11));
7431 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7432 {
7433 {
7434 IkReal evalcond[5];
7435 bool bgotonextstatement = true;
7436 do
7437 {
7438 evalcond[0]=((gconst7*gconst7)+(gconst8*gconst8));
7439 evalcond[1]=new_r01;
7440 evalcond[2]=new_r00;
7441 evalcond[3]=new_r11;
7442 evalcond[4]=new_r10;
7443 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 )
7444 {
7445 bgotonextstatement=false;
7446 {
7447 IkReal j3array[4], cj3array[4], sj3array[4];
7448 bool j3valid[4]={false};
7449 _nj3 = 4;
7450 j3array[0]=0;
7451 sj3array[0]=IKsin(j3array[0]);
7452 cj3array[0]=IKcos(j3array[0]);
7453 j3array[1]=1.5707963267949;
7454 sj3array[1]=IKsin(j3array[1]);
7455 cj3array[1]=IKcos(j3array[1]);
7456 j3array[2]=3.14159265358979;
7457 sj3array[2]=IKsin(j3array[2]);
7458 cj3array[2]=IKcos(j3array[2]);
7459 j3array[3]=-1.5707963267949;
7460 sj3array[3]=IKsin(j3array[3]);
7461 cj3array[3]=IKcos(j3array[3]);
7462 if( j3array[0] > IKPI )
7463 {
7464  j3array[0]-=IK2PI;
7465 }
7466 else if( j3array[0] < -IKPI )
7467 { j3array[0]+=IK2PI;
7468 }
7469 j3valid[0] = true;
7470 if( j3array[1] > IKPI )
7471 {
7472  j3array[1]-=IK2PI;
7473 }
7474 else if( j3array[1] < -IKPI )
7475 { j3array[1]+=IK2PI;
7476 }
7477 j3valid[1] = true;
7478 if( j3array[2] > IKPI )
7479 {
7480  j3array[2]-=IK2PI;
7481 }
7482 else if( j3array[2] < -IKPI )
7483 { j3array[2]+=IK2PI;
7484 }
7485 j3valid[2] = true;
7486 if( j3array[3] > IKPI )
7487 {
7488  j3array[3]-=IK2PI;
7489 }
7490 else if( j3array[3] < -IKPI )
7491 { j3array[3]+=IK2PI;
7492 }
7493 j3valid[3] = true;
7494 for(int ij3 = 0; ij3 < 4; ++ij3)
7495 {
7496 if( !j3valid[ij3] )
7497 {
7498  continue;
7499 }
7500 _ij3[0] = ij3; _ij3[1] = -1;
7501 for(int iij3 = ij3+1; iij3 < 4; ++iij3)
7502 {
7503 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7504 {
7505  j3valid[iij3]=false; _ij3[1] = iij3; break;
7506 }
7507 }
7508 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7509 
7510 {
7511 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7512 vinfos[0].jointtype = 1;
7513 vinfos[0].foffset = j0;
7514 vinfos[0].indices[0] = _ij0[0];
7515 vinfos[0].indices[1] = _ij0[1];
7516 vinfos[0].maxsolutions = _nj0;
7517 vinfos[1].jointtype = 1;
7518 vinfos[1].foffset = j1;
7519 vinfos[1].indices[0] = _ij1[0];
7520 vinfos[1].indices[1] = _ij1[1];
7521 vinfos[1].maxsolutions = _nj1;
7522 vinfos[2].jointtype = 1;
7523 vinfos[2].foffset = j2;
7524 vinfos[2].indices[0] = _ij2[0];
7525 vinfos[2].indices[1] = _ij2[1];
7526 vinfos[2].maxsolutions = _nj2;
7527 vinfos[3].jointtype = 1;
7528 vinfos[3].foffset = j3;
7529 vinfos[3].indices[0] = _ij3[0];
7530 vinfos[3].indices[1] = _ij3[1];
7531 vinfos[3].maxsolutions = _nj3;
7532 vinfos[4].jointtype = 1;
7533 vinfos[4].foffset = j4;
7534 vinfos[4].indices[0] = _ij4[0];
7535 vinfos[4].indices[1] = _ij4[1];
7536 vinfos[4].maxsolutions = _nj4;
7537 vinfos[5].jointtype = 1;
7538 vinfos[5].foffset = j5;
7539 vinfos[5].indices[0] = _ij5[0];
7540 vinfos[5].indices[1] = _ij5[1];
7541 vinfos[5].maxsolutions = _nj5;
7542 std::vector<int> vfree(0);
7543 solutions.AddSolution(vinfos,vfree);
7544 }
7545 }
7546 }
7547 
7548 }
7549 } while(0);
7550 if( bgotonextstatement )
7551 {
7552 bool bgotonextstatement = true;
7553 do
7554 {
7555 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7556 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7557 {
7558 bgotonextstatement=false;
7559 {
7560 IkReal j3eval[1];
7561 CheckValue<IkReal> x502 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7562 if(!x502.valid){
7563 continue;
7564 }
7565 IkReal x501=((-1.0)*(x502.value));
7566 sj4=0;
7567 cj4=-1.0;
7568 j4=3.14159265358979;
7569 sj5=gconst7;
7570 cj5=gconst8;
7571 j5=x501;
7572 new_r11=0;
7573 new_r00=0;
7574 IkReal gconst6=x501;
7575 IkReal x503 = new_r01*new_r01;
7576 if(IKabs(x503)==0){
7577 continue;
7578 }
7579 IkReal gconst7=((-1.0)*new_r01*(pow(x503,-0.5)));
7580 IkReal gconst8=0;
7581 j3eval[0]=new_r01;
7582 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7583 {
7584 {
7585 IkReal j3array[2], cj3array[2], sj3array[2];
7586 bool j3valid[2]={false};
7587 _nj3 = 2;
7588 CheckValue<IkReal> x504=IKPowWithIntegerCheck(gconst7,-1);
7589 if(!x504.valid){
7590 continue;
7591 }
7592 cj3array[0]=((-1.0)*new_r01*(x504.value));
7593 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7594 {
7595  j3valid[0] = j3valid[1] = true;
7596  j3array[0] = IKacos(cj3array[0]);
7597  sj3array[0] = IKsin(j3array[0]);
7598  cj3array[1] = cj3array[0];
7599  j3array[1] = -j3array[0];
7600  sj3array[1] = -sj3array[0];
7601 }
7602 else if( isnan(cj3array[0]) )
7603 {
7604  // probably any value will work
7605  j3valid[0] = true;
7606  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7607 }
7608 for(int ij3 = 0; ij3 < 2; ++ij3)
7609 {
7610 if( !j3valid[ij3] )
7611 {
7612  continue;
7613 }
7614 _ij3[0] = ij3; _ij3[1] = -1;
7615 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7616 {
7617 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7618 {
7619  j3valid[iij3]=false; _ij3[1] = iij3; break;
7620 }
7621 }
7622 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7623 {
7624 IkReal evalcond[6];
7625 IkReal x505=IKsin(j3);
7626 IkReal x506=IKcos(j3);
7627 IkReal x507=((-1.0)*x505);
7628 evalcond[0]=(new_r10*x505);
7629 evalcond[1]=(gconst7*x507);
7630 evalcond[2]=(new_r01*x507);
7631 evalcond[3]=(gconst7+((new_r01*x506)));
7632 evalcond[4]=(gconst7+((new_r10*x506)));
7633 evalcond[5]=(((gconst7*x506))+new_r10);
7634 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7635 {
7636 continue;
7637 }
7638 }
7639 
7640 {
7641 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7642 vinfos[0].jointtype = 1;
7643 vinfos[0].foffset = j0;
7644 vinfos[0].indices[0] = _ij0[0];
7645 vinfos[0].indices[1] = _ij0[1];
7646 vinfos[0].maxsolutions = _nj0;
7647 vinfos[1].jointtype = 1;
7648 vinfos[1].foffset = j1;
7649 vinfos[1].indices[0] = _ij1[0];
7650 vinfos[1].indices[1] = _ij1[1];
7651 vinfos[1].maxsolutions = _nj1;
7652 vinfos[2].jointtype = 1;
7653 vinfos[2].foffset = j2;
7654 vinfos[2].indices[0] = _ij2[0];
7655 vinfos[2].indices[1] = _ij2[1];
7656 vinfos[2].maxsolutions = _nj2;
7657 vinfos[3].jointtype = 1;
7658 vinfos[3].foffset = j3;
7659 vinfos[3].indices[0] = _ij3[0];
7660 vinfos[3].indices[1] = _ij3[1];
7661 vinfos[3].maxsolutions = _nj3;
7662 vinfos[4].jointtype = 1;
7663 vinfos[4].foffset = j4;
7664 vinfos[4].indices[0] = _ij4[0];
7665 vinfos[4].indices[1] = _ij4[1];
7666 vinfos[4].maxsolutions = _nj4;
7667 vinfos[5].jointtype = 1;
7668 vinfos[5].foffset = j5;
7669 vinfos[5].indices[0] = _ij5[0];
7670 vinfos[5].indices[1] = _ij5[1];
7671 vinfos[5].maxsolutions = _nj5;
7672 std::vector<int> vfree(0);
7673 solutions.AddSolution(vinfos,vfree);
7674 }
7675 }
7676 }
7677 
7678 } else
7679 {
7680 {
7681 IkReal j3array[2], cj3array[2], sj3array[2];
7682 bool j3valid[2]={false};
7683 _nj3 = 2;
7684 CheckValue<IkReal> x508=IKPowWithIntegerCheck(new_r01,-1);
7685 if(!x508.valid){
7686 continue;
7687 }
7688 cj3array[0]=((-1.0)*gconst7*(x508.value));
7689 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7690 {
7691  j3valid[0] = j3valid[1] = true;
7692  j3array[0] = IKacos(cj3array[0]);
7693  sj3array[0] = IKsin(j3array[0]);
7694  cj3array[1] = cj3array[0];
7695  j3array[1] = -j3array[0];
7696  sj3array[1] = -sj3array[0];
7697 }
7698 else if( isnan(cj3array[0]) )
7699 {
7700  // probably any value will work
7701  j3valid[0] = true;
7702  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7703 }
7704 for(int ij3 = 0; ij3 < 2; ++ij3)
7705 {
7706 if( !j3valid[ij3] )
7707 {
7708  continue;
7709 }
7710 _ij3[0] = ij3; _ij3[1] = -1;
7711 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7712 {
7713 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7714 {
7715  j3valid[iij3]=false; _ij3[1] = iij3; break;
7716 }
7717 }
7718 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7719 {
7720 IkReal evalcond[6];
7721 IkReal x509=IKsin(j3);
7722 IkReal x510=IKcos(j3);
7723 IkReal x511=(gconst7*x510);
7724 IkReal x512=((-1.0)*x509);
7725 evalcond[0]=(new_r10*x509);
7726 evalcond[1]=(gconst7*x512);
7727 evalcond[2]=(new_r01*x512);
7728 evalcond[3]=(x511+new_r01);
7729 evalcond[4]=(((new_r10*x510))+gconst7);
7730 evalcond[5]=(x511+new_r10);
7731 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7732 {
7733 continue;
7734 }
7735 }
7736 
7737 {
7738 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7739 vinfos[0].jointtype = 1;
7740 vinfos[0].foffset = j0;
7741 vinfos[0].indices[0] = _ij0[0];
7742 vinfos[0].indices[1] = _ij0[1];
7743 vinfos[0].maxsolutions = _nj0;
7744 vinfos[1].jointtype = 1;
7745 vinfos[1].foffset = j1;
7746 vinfos[1].indices[0] = _ij1[0];
7747 vinfos[1].indices[1] = _ij1[1];
7748 vinfos[1].maxsolutions = _nj1;
7749 vinfos[2].jointtype = 1;
7750 vinfos[2].foffset = j2;
7751 vinfos[2].indices[0] = _ij2[0];
7752 vinfos[2].indices[1] = _ij2[1];
7753 vinfos[2].maxsolutions = _nj2;
7754 vinfos[3].jointtype = 1;
7755 vinfos[3].foffset = j3;
7756 vinfos[3].indices[0] = _ij3[0];
7757 vinfos[3].indices[1] = _ij3[1];
7758 vinfos[3].maxsolutions = _nj3;
7759 vinfos[4].jointtype = 1;
7760 vinfos[4].foffset = j4;
7761 vinfos[4].indices[0] = _ij4[0];
7762 vinfos[4].indices[1] = _ij4[1];
7763 vinfos[4].maxsolutions = _nj4;
7764 vinfos[5].jointtype = 1;
7765 vinfos[5].foffset = j5;
7766 vinfos[5].indices[0] = _ij5[0];
7767 vinfos[5].indices[1] = _ij5[1];
7768 vinfos[5].maxsolutions = _nj5;
7769 std::vector<int> vfree(0);
7770 solutions.AddSolution(vinfos,vfree);
7771 }
7772 }
7773 }
7774 
7775 }
7776 
7777 }
7778 
7779 }
7780 } while(0);
7781 if( bgotonextstatement )
7782 {
7783 bool bgotonextstatement = true;
7784 do
7785 {
7786 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7787 evalcond[1]=gconst7;
7788 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7789 {
7790 bgotonextstatement=false;
7791 {
7792 IkReal j3eval[3];
7793 CheckValue<IkReal> x514 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7794 if(!x514.valid){
7795 continue;
7796 }
7797 IkReal x513=((-1.0)*(x514.value));
7798 sj4=0;
7799 cj4=-1.0;
7800 j4=3.14159265358979;
7801 sj5=gconst7;
7802 cj5=gconst8;
7803 j5=x513;
7804 new_r00=0;
7805 new_r10=0;
7806 new_r21=0;
7807 new_r22=0;
7808 IkReal gconst6=x513;
7809 IkReal gconst7=((-1.0)*new_r01);
7810 IkReal gconst8=new_r11;
7811 j3eval[0]=-1.0;
7812 j3eval[1]=-1.0;
7813 j3eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
7814 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7815 {
7816 {
7817 IkReal j3eval[3];
7818 CheckValue<IkReal> x516 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7819 if(!x516.valid){
7820 continue;
7821 }
7822 IkReal x515=((-1.0)*(x516.value));
7823 sj4=0;
7824 cj4=-1.0;
7825 j4=3.14159265358979;
7826 sj5=gconst7;
7827 cj5=gconst8;
7828 j5=x515;
7829 new_r00=0;
7830 new_r10=0;
7831 new_r21=0;
7832 new_r22=0;
7833 IkReal gconst6=x515;
7834 IkReal gconst7=((-1.0)*new_r01);
7835 IkReal gconst8=new_r11;
7836 j3eval[0]=-1.0;
7837 j3eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
7838 j3eval[2]=-1.0;
7839 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7840 {
7841 {
7842 IkReal j3eval[3];
7843 CheckValue<IkReal> x518 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
7844 if(!x518.valid){
7845 continue;
7846 }
7847 IkReal x517=((-1.0)*(x518.value));
7848 sj4=0;
7849 cj4=-1.0;
7850 j4=3.14159265358979;
7851 sj5=gconst7;
7852 cj5=gconst8;
7853 j5=x517;
7854 new_r00=0;
7855 new_r10=0;
7856 new_r21=0;
7857 new_r22=0;
7858 IkReal gconst6=x517;
7859 IkReal gconst7=((-1.0)*new_r01);
7860 IkReal gconst8=new_r11;
7861 j3eval[0]=1.0;
7862 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
7863 j3eval[2]=1.0;
7864 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7865 {
7866 continue; // 3 cases reached
7867 
7868 } else
7869 {
7870 {
7871 IkReal j3array[1], cj3array[1], sj3array[1];
7872 bool j3valid[1]={false};
7873 _nj3 = 1;
7874 IkReal x519=((1.0)*new_r11);
7875 CheckValue<IkReal> x520 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x519)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x519)))),IKFAST_ATAN2_MAGTHRESH);
7876 if(!x520.valid){
7877 continue;
7878 }
7879 CheckValue<IkReal> x521=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
7880 if(!x521.valid){
7881 continue;
7882 }
7883 j3array[0]=((-1.5707963267949)+(x520.value)+(((1.5707963267949)*(x521.value))));
7884 sj3array[0]=IKsin(j3array[0]);
7885 cj3array[0]=IKcos(j3array[0]);
7886 if( j3array[0] > IKPI )
7887 {
7888  j3array[0]-=IK2PI;
7889 }
7890 else if( j3array[0] < -IKPI )
7891 { j3array[0]+=IK2PI;
7892 }
7893 j3valid[0] = true;
7894 for(int ij3 = 0; ij3 < 1; ++ij3)
7895 {
7896 if( !j3valid[ij3] )
7897 {
7898  continue;
7899 }
7900 _ij3[0] = ij3; _ij3[1] = -1;
7901 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7902 {
7903 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7904 {
7905  j3valid[iij3]=false; _ij3[1] = iij3; break;
7906 }
7907 }
7908 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7909 {
7910 IkReal evalcond[6];
7911 IkReal x522=IKcos(j3);
7912 IkReal x523=IKsin(j3);
7913 IkReal x524=((1.0)*gconst8);
7914 IkReal x525=(gconst7*x522);
7915 IkReal x526=(gconst7*x523);
7916 IkReal x527=(x523*x524);
7917 evalcond[0]=(((new_r01*x522))+gconst7+((new_r11*x523)));
7918 evalcond[1]=(x526+((gconst8*x522))+new_r11);
7919 evalcond[2]=((((-1.0)*x527))+x525);
7920 evalcond[3]=((((-1.0)*x527))+x525+new_r01);
7921 evalcond[4]=((((-1.0)*x526))+(((-1.0)*x522*x524)));
7922 evalcond[5]=(gconst8+((new_r11*x522))+(((-1.0)*new_r01*x523)));
7923 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7924 {
7925 continue;
7926 }
7927 }
7928 
7929 {
7930 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7931 vinfos[0].jointtype = 1;
7932 vinfos[0].foffset = j0;
7933 vinfos[0].indices[0] = _ij0[0];
7934 vinfos[0].indices[1] = _ij0[1];
7935 vinfos[0].maxsolutions = _nj0;
7936 vinfos[1].jointtype = 1;
7937 vinfos[1].foffset = j1;
7938 vinfos[1].indices[0] = _ij1[0];
7939 vinfos[1].indices[1] = _ij1[1];
7940 vinfos[1].maxsolutions = _nj1;
7941 vinfos[2].jointtype = 1;
7942 vinfos[2].foffset = j2;
7943 vinfos[2].indices[0] = _ij2[0];
7944 vinfos[2].indices[1] = _ij2[1];
7945 vinfos[2].maxsolutions = _nj2;
7946 vinfos[3].jointtype = 1;
7947 vinfos[3].foffset = j3;
7948 vinfos[3].indices[0] = _ij3[0];
7949 vinfos[3].indices[1] = _ij3[1];
7950 vinfos[3].maxsolutions = _nj3;
7951 vinfos[4].jointtype = 1;
7952 vinfos[4].foffset = j4;
7953 vinfos[4].indices[0] = _ij4[0];
7954 vinfos[4].indices[1] = _ij4[1];
7955 vinfos[4].maxsolutions = _nj4;
7956 vinfos[5].jointtype = 1;
7957 vinfos[5].foffset = j5;
7958 vinfos[5].indices[0] = _ij5[0];
7959 vinfos[5].indices[1] = _ij5[1];
7960 vinfos[5].maxsolutions = _nj5;
7961 std::vector<int> vfree(0);
7962 solutions.AddSolution(vinfos,vfree);
7963 }
7964 }
7965 }
7966 
7967 }
7968 
7969 }
7970 
7971 } else
7972 {
7973 {
7974 IkReal j3array[1], cj3array[1], sj3array[1];
7975 bool j3valid[1]={false};
7976 _nj3 = 1;
7977 CheckValue<IkReal> x528 = IKatan2WithCheck(IkReal((gconst7*new_r11)),IkReal((gconst8*new_r11)),IKFAST_ATAN2_MAGTHRESH);
7978 if(!x528.valid){
7979 continue;
7980 }
7981 CheckValue<IkReal> x529=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
7982 if(!x529.valid){
7983 continue;
7984 }
7985 j3array[0]=((-1.5707963267949)+(x528.value)+(((1.5707963267949)*(x529.value))));
7986 sj3array[0]=IKsin(j3array[0]);
7987 cj3array[0]=IKcos(j3array[0]);
7988 if( j3array[0] > IKPI )
7989 {
7990  j3array[0]-=IK2PI;
7991 }
7992 else if( j3array[0] < -IKPI )
7993 { j3array[0]+=IK2PI;
7994 }
7995 j3valid[0] = true;
7996 for(int ij3 = 0; ij3 < 1; ++ij3)
7997 {
7998 if( !j3valid[ij3] )
7999 {
8000  continue;
8001 }
8002 _ij3[0] = ij3; _ij3[1] = -1;
8003 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8004 {
8005 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8006 {
8007  j3valid[iij3]=false; _ij3[1] = iij3; break;
8008 }
8009 }
8010 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8011 {
8012 IkReal evalcond[6];
8013 IkReal x530=IKcos(j3);
8014 IkReal x531=IKsin(j3);
8015 IkReal x532=((1.0)*gconst8);
8016 IkReal x533=(gconst7*x530);
8017 IkReal x534=(gconst7*x531);
8018 IkReal x535=(x531*x532);
8019 evalcond[0]=(((new_r01*x530))+gconst7+((new_r11*x531)));
8020 evalcond[1]=(x534+((gconst8*x530))+new_r11);
8021 evalcond[2]=((((-1.0)*x535))+x533);
8022 evalcond[3]=((((-1.0)*x535))+x533+new_r01);
8023 evalcond[4]=((((-1.0)*x534))+(((-1.0)*x530*x532)));
8024 evalcond[5]=(gconst8+((new_r11*x530))+(((-1.0)*new_r01*x531)));
8025 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8026 {
8027 continue;
8028 }
8029 }
8030 
8031 {
8032 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8033 vinfos[0].jointtype = 1;
8034 vinfos[0].foffset = j0;
8035 vinfos[0].indices[0] = _ij0[0];
8036 vinfos[0].indices[1] = _ij0[1];
8037 vinfos[0].maxsolutions = _nj0;
8038 vinfos[1].jointtype = 1;
8039 vinfos[1].foffset = j1;
8040 vinfos[1].indices[0] = _ij1[0];
8041 vinfos[1].indices[1] = _ij1[1];
8042 vinfos[1].maxsolutions = _nj1;
8043 vinfos[2].jointtype = 1;
8044 vinfos[2].foffset = j2;
8045 vinfos[2].indices[0] = _ij2[0];
8046 vinfos[2].indices[1] = _ij2[1];
8047 vinfos[2].maxsolutions = _nj2;
8048 vinfos[3].jointtype = 1;
8049 vinfos[3].foffset = j3;
8050 vinfos[3].indices[0] = _ij3[0];
8051 vinfos[3].indices[1] = _ij3[1];
8052 vinfos[3].maxsolutions = _nj3;
8053 vinfos[4].jointtype = 1;
8054 vinfos[4].foffset = j4;
8055 vinfos[4].indices[0] = _ij4[0];
8056 vinfos[4].indices[1] = _ij4[1];
8057 vinfos[4].maxsolutions = _nj4;
8058 vinfos[5].jointtype = 1;
8059 vinfos[5].foffset = j5;
8060 vinfos[5].indices[0] = _ij5[0];
8061 vinfos[5].indices[1] = _ij5[1];
8062 vinfos[5].maxsolutions = _nj5;
8063 std::vector<int> vfree(0);
8064 solutions.AddSolution(vinfos,vfree);
8065 }
8066 }
8067 }
8068 
8069 }
8070 
8071 }
8072 
8073 } else
8074 {
8075 {
8076 IkReal j3array[1], cj3array[1], sj3array[1];
8077 bool j3valid[1]={false};
8078 _nj3 = 1;
8079 CheckValue<IkReal> x536 = IKatan2WithCheck(IkReal((gconst7*gconst8)),IkReal(((-1.0)*(gconst7*gconst7))),IKFAST_ATAN2_MAGTHRESH);
8080 if(!x536.valid){
8081 continue;
8082 }
8083 CheckValue<IkReal> x537=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*new_r11))+((gconst7*new_r01)))),-1);
8084 if(!x537.valid){
8085 continue;
8086 }
8087 j3array[0]=((-1.5707963267949)+(x536.value)+(((1.5707963267949)*(x537.value))));
8088 sj3array[0]=IKsin(j3array[0]);
8089 cj3array[0]=IKcos(j3array[0]);
8090 if( j3array[0] > IKPI )
8091 {
8092  j3array[0]-=IK2PI;
8093 }
8094 else if( j3array[0] < -IKPI )
8095 { j3array[0]+=IK2PI;
8096 }
8097 j3valid[0] = true;
8098 for(int ij3 = 0; ij3 < 1; ++ij3)
8099 {
8100 if( !j3valid[ij3] )
8101 {
8102  continue;
8103 }
8104 _ij3[0] = ij3; _ij3[1] = -1;
8105 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8106 {
8107 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8108 {
8109  j3valid[iij3]=false; _ij3[1] = iij3; break;
8110 }
8111 }
8112 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8113 {
8114 IkReal evalcond[6];
8115 IkReal x538=IKcos(j3);
8116 IkReal x539=IKsin(j3);
8117 IkReal x540=((1.0)*gconst8);
8118 IkReal x541=(gconst7*x538);
8119 IkReal x542=(gconst7*x539);
8120 IkReal x543=(x539*x540);
8121 evalcond[0]=(((new_r01*x538))+gconst7+((new_r11*x539)));
8122 evalcond[1]=(x542+((gconst8*x538))+new_r11);
8123 evalcond[2]=((((-1.0)*x543))+x541);
8124 evalcond[3]=((((-1.0)*x543))+x541+new_r01);
8125 evalcond[4]=((((-1.0)*x542))+(((-1.0)*x538*x540)));
8126 evalcond[5]=(gconst8+((new_r11*x538))+(((-1.0)*new_r01*x539)));
8127 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8128 {
8129 continue;
8130 }
8131 }
8132 
8133 {
8134 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8135 vinfos[0].jointtype = 1;
8136 vinfos[0].foffset = j0;
8137 vinfos[0].indices[0] = _ij0[0];
8138 vinfos[0].indices[1] = _ij0[1];
8139 vinfos[0].maxsolutions = _nj0;
8140 vinfos[1].jointtype = 1;
8141 vinfos[1].foffset = j1;
8142 vinfos[1].indices[0] = _ij1[0];
8143 vinfos[1].indices[1] = _ij1[1];
8144 vinfos[1].maxsolutions = _nj1;
8145 vinfos[2].jointtype = 1;
8146 vinfos[2].foffset = j2;
8147 vinfos[2].indices[0] = _ij2[0];
8148 vinfos[2].indices[1] = _ij2[1];
8149 vinfos[2].maxsolutions = _nj2;
8150 vinfos[3].jointtype = 1;
8151 vinfos[3].foffset = j3;
8152 vinfos[3].indices[0] = _ij3[0];
8153 vinfos[3].indices[1] = _ij3[1];
8154 vinfos[3].maxsolutions = _nj3;
8155 vinfos[4].jointtype = 1;
8156 vinfos[4].foffset = j4;
8157 vinfos[4].indices[0] = _ij4[0];
8158 vinfos[4].indices[1] = _ij4[1];
8159 vinfos[4].maxsolutions = _nj4;
8160 vinfos[5].jointtype = 1;
8161 vinfos[5].foffset = j5;
8162 vinfos[5].indices[0] = _ij5[0];
8163 vinfos[5].indices[1] = _ij5[1];
8164 vinfos[5].maxsolutions = _nj5;
8165 std::vector<int> vfree(0);
8166 solutions.AddSolution(vinfos,vfree);
8167 }
8168 }
8169 }
8170 
8171 }
8172 
8173 }
8174 
8175 }
8176 } while(0);
8177 if( bgotonextstatement )
8178 {
8179 bool bgotonextstatement = true;
8180 do
8181 {
8182 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
8183 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8184 {
8185 bgotonextstatement=false;
8186 {
8187 IkReal j3array[2], cj3array[2], sj3array[2];
8188 bool j3valid[2]={false};
8189 _nj3 = 2;
8190 CheckValue<IkReal> x544=IKPowWithIntegerCheck(gconst8,-1);
8191 if(!x544.valid){
8192 continue;
8193 }
8194 cj3array[0]=(new_r00*(x544.value));
8195 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8196 {
8197  j3valid[0] = j3valid[1] = true;
8198  j3array[0] = IKacos(cj3array[0]);
8199  sj3array[0] = IKsin(j3array[0]);
8200  cj3array[1] = cj3array[0];
8201  j3array[1] = -j3array[0];
8202  sj3array[1] = -sj3array[0];
8203 }
8204 else if( isnan(cj3array[0]) )
8205 {
8206  // probably any value will work
8207  j3valid[0] = true;
8208  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8209 }
8210 for(int ij3 = 0; ij3 < 2; ++ij3)
8211 {
8212 if( !j3valid[ij3] )
8213 {
8214  continue;
8215 }
8216 _ij3[0] = ij3; _ij3[1] = -1;
8217 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8218 {
8219 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8220 {
8221  j3valid[iij3]=false; _ij3[1] = iij3; break;
8222 }
8223 }
8224 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8225 {
8226 IkReal evalcond[6];
8227 IkReal x545=IKsin(j3);
8228 IkReal x546=IKcos(j3);
8229 IkReal x547=((-1.0)*x545);
8230 evalcond[0]=(new_r11*x545);
8231 evalcond[1]=(gconst8*x547);
8232 evalcond[2]=(new_r00*x547);
8233 evalcond[3]=(((gconst8*x546))+new_r11);
8234 evalcond[4]=(gconst8+((new_r11*x546)));
8235 evalcond[5]=(((new_r00*x546))+(((-1.0)*gconst8)));
8236 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8237 {
8238 continue;
8239 }
8240 }
8241 
8242 {
8243 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8244 vinfos[0].jointtype = 1;
8245 vinfos[0].foffset = j0;
8246 vinfos[0].indices[0] = _ij0[0];
8247 vinfos[0].indices[1] = _ij0[1];
8248 vinfos[0].maxsolutions = _nj0;
8249 vinfos[1].jointtype = 1;
8250 vinfos[1].foffset = j1;
8251 vinfos[1].indices[0] = _ij1[0];
8252 vinfos[1].indices[1] = _ij1[1];
8253 vinfos[1].maxsolutions = _nj1;
8254 vinfos[2].jointtype = 1;
8255 vinfos[2].foffset = j2;
8256 vinfos[2].indices[0] = _ij2[0];
8257 vinfos[2].indices[1] = _ij2[1];
8258 vinfos[2].maxsolutions = _nj2;
8259 vinfos[3].jointtype = 1;
8260 vinfos[3].foffset = j3;
8261 vinfos[3].indices[0] = _ij3[0];
8262 vinfos[3].indices[1] = _ij3[1];
8263 vinfos[3].maxsolutions = _nj3;
8264 vinfos[4].jointtype = 1;
8265 vinfos[4].foffset = j4;
8266 vinfos[4].indices[0] = _ij4[0];
8267 vinfos[4].indices[1] = _ij4[1];
8268 vinfos[4].maxsolutions = _nj4;
8269 vinfos[5].jointtype = 1;
8270 vinfos[5].foffset = j5;
8271 vinfos[5].indices[0] = _ij5[0];
8272 vinfos[5].indices[1] = _ij5[1];
8273 vinfos[5].maxsolutions = _nj5;
8274 std::vector<int> vfree(0);
8275 solutions.AddSolution(vinfos,vfree);
8276 }
8277 }
8278 }
8279 
8280 }
8281 } while(0);
8282 if( bgotonextstatement )
8283 {
8284 bool bgotonextstatement = true;
8285 do
8286 {
8287 evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01)));
8288 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8289 {
8290 bgotonextstatement=false;
8291 {
8292 IkReal j3eval[1];
8293 CheckValue<IkReal> x549 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8294 if(!x549.valid){
8295 continue;
8296 }
8297 IkReal x548=((-1.0)*(x549.value));
8298 sj4=0;
8299 cj4=-1.0;
8300 j4=3.14159265358979;
8301 sj5=gconst7;
8302 cj5=gconst8;
8303 j5=x548;
8304 new_r00=0;
8305 new_r01=0;
8306 new_r12=0;
8307 new_r22=0;
8308 IkReal gconst6=x548;
8309 IkReal gconst7=0;
8310 IkReal x550 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
8311 if(IKabs(x550)==0){
8312 continue;
8313 }
8314 IkReal gconst8=(new_r11*(pow(x550,-0.5)));
8315 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
8316 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8317 {
8318 {
8319 IkReal j3eval[1];
8320 CheckValue<IkReal> x552 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8321 if(!x552.valid){
8322 continue;
8323 }
8324 IkReal x551=((-1.0)*(x552.value));
8325 sj4=0;
8326 cj4=-1.0;
8327 j4=3.14159265358979;
8328 sj5=gconst7;
8329 cj5=gconst8;
8330 j5=x551;
8331 new_r00=0;
8332 new_r01=0;
8333 new_r12=0;
8334 new_r22=0;
8335 IkReal gconst6=x551;
8336 IkReal gconst7=0;
8337 IkReal x553 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
8338 if(IKabs(x553)==0){
8339 continue;
8340 }
8341 IkReal gconst8=(new_r11*(pow(x553,-0.5)));
8342 j3eval[0]=new_r11;
8343 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8344 {
8345 {
8346 IkReal j3eval[1];
8347 CheckValue<IkReal> x555 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8348 if(!x555.valid){
8349 continue;
8350 }
8351 IkReal x554=((-1.0)*(x555.value));
8352 sj4=0;
8353 cj4=-1.0;
8354 j4=3.14159265358979;
8355 sj5=gconst7;
8356 cj5=gconst8;
8357 j5=x554;
8358 new_r00=0;
8359 new_r01=0;
8360 new_r12=0;
8361 new_r22=0;
8362 IkReal gconst6=x554;
8363 IkReal gconst7=0;
8364 IkReal x556 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
8365 if(IKabs(x556)==0){
8366 continue;
8367 }
8368 IkReal gconst8=(new_r11*(pow(x556,-0.5)));
8369 j3eval[0]=new_r10;
8370 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8371 {
8372 continue; // 3 cases reached
8373 
8374 } else
8375 {
8376 {
8377 IkReal j3array[1], cj3array[1], sj3array[1];
8378 bool j3valid[1]={false};
8379 _nj3 = 1;
8380 CheckValue<IkReal> x557=IKPowWithIntegerCheck(new_r10,-1);
8381 if(!x557.valid){
8382 continue;
8383 }
8384 CheckValue<IkReal> x558=IKPowWithIntegerCheck(gconst8,-1);
8385 if(!x558.valid){
8386 continue;
8387 }
8388 if( IKabs((gconst8*(x557.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x558.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst8*(x557.value)))+IKsqr(((-1.0)*new_r11*(x558.value)))-1) <= IKFAST_SINCOS_THRESH )
8389  continue;
8390 j3array[0]=IKatan2((gconst8*(x557.value)), ((-1.0)*new_r11*(x558.value)));
8391 sj3array[0]=IKsin(j3array[0]);
8392 cj3array[0]=IKcos(j3array[0]);
8393 if( j3array[0] > IKPI )
8394 {
8395  j3array[0]-=IK2PI;
8396 }
8397 else if( j3array[0] < -IKPI )
8398 { j3array[0]+=IK2PI;
8399 }
8400 j3valid[0] = true;
8401 for(int ij3 = 0; ij3 < 1; ++ij3)
8402 {
8403 if( !j3valid[ij3] )
8404 {
8405  continue;
8406 }
8407 _ij3[0] = ij3; _ij3[1] = -1;
8408 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8409 {
8410 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8411 {
8412  j3valid[iij3]=false; _ij3[1] = iij3; break;
8413 }
8414 }
8415 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8416 {
8417 IkReal evalcond[8];
8418 IkReal x559=IKsin(j3);
8419 IkReal x560=IKcos(j3);
8420 IkReal x561=((1.0)*gconst8);
8421 IkReal x562=((-1.0)*gconst8);
8422 evalcond[0]=(new_r11*x559);
8423 evalcond[1]=(new_r10*x560);
8424 evalcond[2]=(x559*x562);
8425 evalcond[3]=(x560*x562);
8426 evalcond[4]=(((gconst8*x560))+new_r11);
8427 evalcond[5]=(gconst8+((new_r11*x560)));
8428 evalcond[6]=((((-1.0)*x559*x561))+new_r10);
8429 evalcond[7]=(((new_r10*x559))+(((-1.0)*x561)));
8430 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8431 {
8432 continue;
8433 }
8434 }
8435 
8436 {
8437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8438 vinfos[0].jointtype = 1;
8439 vinfos[0].foffset = j0;
8440 vinfos[0].indices[0] = _ij0[0];
8441 vinfos[0].indices[1] = _ij0[1];
8442 vinfos[0].maxsolutions = _nj0;
8443 vinfos[1].jointtype = 1;
8444 vinfos[1].foffset = j1;
8445 vinfos[1].indices[0] = _ij1[0];
8446 vinfos[1].indices[1] = _ij1[1];
8447 vinfos[1].maxsolutions = _nj1;
8448 vinfos[2].jointtype = 1;
8449 vinfos[2].foffset = j2;
8450 vinfos[2].indices[0] = _ij2[0];
8451 vinfos[2].indices[1] = _ij2[1];
8452 vinfos[2].maxsolutions = _nj2;
8453 vinfos[3].jointtype = 1;
8454 vinfos[3].foffset = j3;
8455 vinfos[3].indices[0] = _ij3[0];
8456 vinfos[3].indices[1] = _ij3[1];
8457 vinfos[3].maxsolutions = _nj3;
8458 vinfos[4].jointtype = 1;
8459 vinfos[4].foffset = j4;
8460 vinfos[4].indices[0] = _ij4[0];
8461 vinfos[4].indices[1] = _ij4[1];
8462 vinfos[4].maxsolutions = _nj4;
8463 vinfos[5].jointtype = 1;
8464 vinfos[5].foffset = j5;
8465 vinfos[5].indices[0] = _ij5[0];
8466 vinfos[5].indices[1] = _ij5[1];
8467 vinfos[5].maxsolutions = _nj5;
8468 std::vector<int> vfree(0);
8469 solutions.AddSolution(vinfos,vfree);
8470 }
8471 }
8472 }
8473 
8474 }
8475 
8476 }
8477 
8478 } else
8479 {
8480 {
8481 IkReal j3array[1], cj3array[1], sj3array[1];
8482 bool j3valid[1]={false};
8483 _nj3 = 1;
8484 CheckValue<IkReal> x563=IKPowWithIntegerCheck(gconst8,-1);
8485 if(!x563.valid){
8486 continue;
8487 }
8488 CheckValue<IkReal> x564=IKPowWithIntegerCheck(new_r11,-1);
8489 if(!x564.valid){
8490 continue;
8491 }
8492 if( IKabs((new_r10*(x563.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst8*(x564.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x563.value)))+IKsqr(((-1.0)*gconst8*(x564.value)))-1) <= IKFAST_SINCOS_THRESH )
8493  continue;
8494 j3array[0]=IKatan2((new_r10*(x563.value)), ((-1.0)*gconst8*(x564.value)));
8495 sj3array[0]=IKsin(j3array[0]);
8496 cj3array[0]=IKcos(j3array[0]);
8497 if( j3array[0] > IKPI )
8498 {
8499  j3array[0]-=IK2PI;
8500 }
8501 else if( j3array[0] < -IKPI )
8502 { j3array[0]+=IK2PI;
8503 }
8504 j3valid[0] = true;
8505 for(int ij3 = 0; ij3 < 1; ++ij3)
8506 {
8507 if( !j3valid[ij3] )
8508 {
8509  continue;
8510 }
8511 _ij3[0] = ij3; _ij3[1] = -1;
8512 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8513 {
8514 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8515 {
8516  j3valid[iij3]=false; _ij3[1] = iij3; break;
8517 }
8518 }
8519 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8520 {
8521 IkReal evalcond[8];
8522 IkReal x565=IKsin(j3);
8523 IkReal x566=IKcos(j3);
8524 IkReal x567=((1.0)*gconst8);
8525 IkReal x568=((-1.0)*gconst8);
8526 evalcond[0]=(new_r11*x565);
8527 evalcond[1]=(new_r10*x566);
8528 evalcond[2]=(x565*x568);
8529 evalcond[3]=(x566*x568);
8530 evalcond[4]=(((gconst8*x566))+new_r11);
8531 evalcond[5]=(gconst8+((new_r11*x566)));
8532 evalcond[6]=(new_r10+(((-1.0)*x565*x567)));
8533 evalcond[7]=(((new_r10*x565))+(((-1.0)*x567)));
8534 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8535 {
8536 continue;
8537 }
8538 }
8539 
8540 {
8541 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8542 vinfos[0].jointtype = 1;
8543 vinfos[0].foffset = j0;
8544 vinfos[0].indices[0] = _ij0[0];
8545 vinfos[0].indices[1] = _ij0[1];
8546 vinfos[0].maxsolutions = _nj0;
8547 vinfos[1].jointtype = 1;
8548 vinfos[1].foffset = j1;
8549 vinfos[1].indices[0] = _ij1[0];
8550 vinfos[1].indices[1] = _ij1[1];
8551 vinfos[1].maxsolutions = _nj1;
8552 vinfos[2].jointtype = 1;
8553 vinfos[2].foffset = j2;
8554 vinfos[2].indices[0] = _ij2[0];
8555 vinfos[2].indices[1] = _ij2[1];
8556 vinfos[2].maxsolutions = _nj2;
8557 vinfos[3].jointtype = 1;
8558 vinfos[3].foffset = j3;
8559 vinfos[3].indices[0] = _ij3[0];
8560 vinfos[3].indices[1] = _ij3[1];
8561 vinfos[3].maxsolutions = _nj3;
8562 vinfos[4].jointtype = 1;
8563 vinfos[4].foffset = j4;
8564 vinfos[4].indices[0] = _ij4[0];
8565 vinfos[4].indices[1] = _ij4[1];
8566 vinfos[4].maxsolutions = _nj4;
8567 vinfos[5].jointtype = 1;
8568 vinfos[5].foffset = j5;
8569 vinfos[5].indices[0] = _ij5[0];
8570 vinfos[5].indices[1] = _ij5[1];
8571 vinfos[5].maxsolutions = _nj5;
8572 std::vector<int> vfree(0);
8573 solutions.AddSolution(vinfos,vfree);
8574 }
8575 }
8576 }
8577 
8578 }
8579 
8580 }
8581 
8582 } else
8583 {
8584 {
8585 IkReal j3array[1], cj3array[1], sj3array[1];
8586 bool j3valid[1]={false};
8587 _nj3 = 1;
8588 CheckValue<IkReal> x569 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
8589 if(!x569.valid){
8590 continue;
8591 }
8592 CheckValue<IkReal> x570=IKPowWithIntegerCheck(IKsign(gconst8),-1);
8593 if(!x570.valid){
8594 continue;
8595 }
8596 j3array[0]=((-1.5707963267949)+(x569.value)+(((1.5707963267949)*(x570.value))));
8597 sj3array[0]=IKsin(j3array[0]);
8598 cj3array[0]=IKcos(j3array[0]);
8599 if( j3array[0] > IKPI )
8600 {
8601  j3array[0]-=IK2PI;
8602 }
8603 else if( j3array[0] < -IKPI )
8604 { j3array[0]+=IK2PI;
8605 }
8606 j3valid[0] = true;
8607 for(int ij3 = 0; ij3 < 1; ++ij3)
8608 {
8609 if( !j3valid[ij3] )
8610 {
8611  continue;
8612 }
8613 _ij3[0] = ij3; _ij3[1] = -1;
8614 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8615 {
8616 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8617 {
8618  j3valid[iij3]=false; _ij3[1] = iij3; break;
8619 }
8620 }
8621 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8622 {
8623 IkReal evalcond[8];
8624 IkReal x571=IKsin(j3);
8625 IkReal x572=IKcos(j3);
8626 IkReal x573=((1.0)*gconst8);
8627 IkReal x574=((-1.0)*gconst8);
8628 evalcond[0]=(new_r11*x571);
8629 evalcond[1]=(new_r10*x572);
8630 evalcond[2]=(x571*x574);
8631 evalcond[3]=(x572*x574);
8632 evalcond[4]=(((gconst8*x572))+new_r11);
8633 evalcond[5]=(((new_r11*x572))+gconst8);
8634 evalcond[6]=((((-1.0)*x571*x573))+new_r10);
8635 evalcond[7]=(((new_r10*x571))+(((-1.0)*x573)));
8636 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8637 {
8638 continue;
8639 }
8640 }
8641 
8642 {
8643 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8644 vinfos[0].jointtype = 1;
8645 vinfos[0].foffset = j0;
8646 vinfos[0].indices[0] = _ij0[0];
8647 vinfos[0].indices[1] = _ij0[1];
8648 vinfos[0].maxsolutions = _nj0;
8649 vinfos[1].jointtype = 1;
8650 vinfos[1].foffset = j1;
8651 vinfos[1].indices[0] = _ij1[0];
8652 vinfos[1].indices[1] = _ij1[1];
8653 vinfos[1].maxsolutions = _nj1;
8654 vinfos[2].jointtype = 1;
8655 vinfos[2].foffset = j2;
8656 vinfos[2].indices[0] = _ij2[0];
8657 vinfos[2].indices[1] = _ij2[1];
8658 vinfos[2].maxsolutions = _nj2;
8659 vinfos[3].jointtype = 1;
8660 vinfos[3].foffset = j3;
8661 vinfos[3].indices[0] = _ij3[0];
8662 vinfos[3].indices[1] = _ij3[1];
8663 vinfos[3].maxsolutions = _nj3;
8664 vinfos[4].jointtype = 1;
8665 vinfos[4].foffset = j4;
8666 vinfos[4].indices[0] = _ij4[0];
8667 vinfos[4].indices[1] = _ij4[1];
8668 vinfos[4].maxsolutions = _nj4;
8669 vinfos[5].jointtype = 1;
8670 vinfos[5].foffset = j5;
8671 vinfos[5].indices[0] = _ij5[0];
8672 vinfos[5].indices[1] = _ij5[1];
8673 vinfos[5].maxsolutions = _nj5;
8674 std::vector<int> vfree(0);
8675 solutions.AddSolution(vinfos,vfree);
8676 }
8677 }
8678 }
8679 
8680 }
8681 
8682 }
8683 
8684 }
8685 } while(0);
8686 if( bgotonextstatement )
8687 {
8688 bool bgotonextstatement = true;
8689 do
8690 {
8691 evalcond[0]=IKabs(new_r01);
8692 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8693 {
8694 bgotonextstatement=false;
8695 {
8696 IkReal j3eval[1];
8697 CheckValue<IkReal> x576 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8698 if(!x576.valid){
8699 continue;
8700 }
8701 IkReal x575=((-1.0)*(x576.value));
8702 sj4=0;
8703 cj4=-1.0;
8704 j4=3.14159265358979;
8705 sj5=gconst7;
8706 cj5=gconst8;
8707 j5=x575;
8708 new_r01=0;
8709 IkReal gconst6=x575;
8710 IkReal gconst7=0;
8711 IkReal x577 = new_r11*new_r11;
8712 if(IKabs(x577)==0){
8713 continue;
8714 }
8715 IkReal gconst8=(new_r11*(pow(x577,-0.5)));
8716 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
8717 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8718 {
8719 {
8720 IkReal j3eval[1];
8721 CheckValue<IkReal> x579 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8722 if(!x579.valid){
8723 continue;
8724 }
8725 IkReal x578=((-1.0)*(x579.value));
8726 sj4=0;
8727 cj4=-1.0;
8728 j4=3.14159265358979;
8729 sj5=gconst7;
8730 cj5=gconst8;
8731 j5=x578;
8732 new_r01=0;
8733 IkReal gconst6=x578;
8734 IkReal gconst7=0;
8735 IkReal x580 = new_r11*new_r11;
8736 if(IKabs(x580)==0){
8737 continue;
8738 }
8739 IkReal gconst8=(new_r11*(pow(x580,-0.5)));
8740 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
8741 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8742 {
8743 {
8744 IkReal j3eval[1];
8745 CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
8746 if(!x582.valid){
8747 continue;
8748 }
8749 IkReal x581=((-1.0)*(x582.value));
8750 sj4=0;
8751 cj4=-1.0;
8752 j4=3.14159265358979;
8753 sj5=gconst7;
8754 cj5=gconst8;
8755 j5=x581;
8756 new_r01=0;
8757 IkReal gconst6=x581;
8758 IkReal gconst7=0;
8759 IkReal x583 = new_r11*new_r11;
8760 if(IKabs(x583)==0){
8761 continue;
8762 }
8763 IkReal gconst8=(new_r11*(pow(x583,-0.5)));
8764 j3eval[0]=new_r11;
8765 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8766 {
8767 continue; // 3 cases reached
8768 
8769 } else
8770 {
8771 {
8772 IkReal j3array[1], cj3array[1], sj3array[1];
8773 bool j3valid[1]={false};
8774 _nj3 = 1;
8775 CheckValue<IkReal> x584=IKPowWithIntegerCheck(gconst8,-1);
8776 if(!x584.valid){
8777 continue;
8778 }
8779 CheckValue<IkReal> x585=IKPowWithIntegerCheck(new_r11,-1);
8780 if(!x585.valid){
8781 continue;
8782 }
8783 if( IKabs((new_r10*(x584.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst8*(x585.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x584.value)))+IKsqr(((-1.0)*gconst8*(x585.value)))-1) <= IKFAST_SINCOS_THRESH )
8784  continue;
8785 j3array[0]=IKatan2((new_r10*(x584.value)), ((-1.0)*gconst8*(x585.value)));
8786 sj3array[0]=IKsin(j3array[0]);
8787 cj3array[0]=IKcos(j3array[0]);
8788 if( j3array[0] > IKPI )
8789 {
8790  j3array[0]-=IK2PI;
8791 }
8792 else if( j3array[0] < -IKPI )
8793 { j3array[0]+=IK2PI;
8794 }
8795 j3valid[0] = true;
8796 for(int ij3 = 0; ij3 < 1; ++ij3)
8797 {
8798 if( !j3valid[ij3] )
8799 {
8800  continue;
8801 }
8802 _ij3[0] = ij3; _ij3[1] = -1;
8803 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8804 {
8805 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8806 {
8807  j3valid[iij3]=false; _ij3[1] = iij3; break;
8808 }
8809 }
8810 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8811 {
8812 IkReal evalcond[8];
8813 IkReal x586=IKsin(j3);
8814 IkReal x587=IKcos(j3);
8815 IkReal x588=((1.0)*gconst8);
8816 evalcond[0]=(new_r11*x586);
8817 evalcond[1]=((-1.0)*gconst8*x586);
8818 evalcond[2]=(((gconst8*x587))+new_r11);
8819 evalcond[3]=(gconst8+((new_r11*x587)));
8820 evalcond[4]=((((-1.0)*x587*x588))+new_r00);
8821 evalcond[5]=((((-1.0)*x586*x588))+new_r10);
8822 evalcond[6]=((((-1.0)*new_r00*x586))+((new_r10*x587)));
8823 evalcond[7]=(((new_r00*x587))+(((-1.0)*x588))+((new_r10*x586)));
8824 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8825 {
8826 continue;
8827 }
8828 }
8829 
8830 {
8831 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8832 vinfos[0].jointtype = 1;
8833 vinfos[0].foffset = j0;
8834 vinfos[0].indices[0] = _ij0[0];
8835 vinfos[0].indices[1] = _ij0[1];
8836 vinfos[0].maxsolutions = _nj0;
8837 vinfos[1].jointtype = 1;
8838 vinfos[1].foffset = j1;
8839 vinfos[1].indices[0] = _ij1[0];
8840 vinfos[1].indices[1] = _ij1[1];
8841 vinfos[1].maxsolutions = _nj1;
8842 vinfos[2].jointtype = 1;
8843 vinfos[2].foffset = j2;
8844 vinfos[2].indices[0] = _ij2[0];
8845 vinfos[2].indices[1] = _ij2[1];
8846 vinfos[2].maxsolutions = _nj2;
8847 vinfos[3].jointtype = 1;
8848 vinfos[3].foffset = j3;
8849 vinfos[3].indices[0] = _ij3[0];
8850 vinfos[3].indices[1] = _ij3[1];
8851 vinfos[3].maxsolutions = _nj3;
8852 vinfos[4].jointtype = 1;
8853 vinfos[4].foffset = j4;
8854 vinfos[4].indices[0] = _ij4[0];
8855 vinfos[4].indices[1] = _ij4[1];
8856 vinfos[4].maxsolutions = _nj4;
8857 vinfos[5].jointtype = 1;
8858 vinfos[5].foffset = j5;
8859 vinfos[5].indices[0] = _ij5[0];
8860 vinfos[5].indices[1] = _ij5[1];
8861 vinfos[5].maxsolutions = _nj5;
8862 std::vector<int> vfree(0);
8863 solutions.AddSolution(vinfos,vfree);
8864 }
8865 }
8866 }
8867 
8868 }
8869 
8870 }
8871 
8872 } else
8873 {
8874 {
8875 IkReal j3array[1], cj3array[1], sj3array[1];
8876 bool j3valid[1]={false};
8877 _nj3 = 1;
8878 CheckValue<IkReal> x589 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
8879 if(!x589.valid){
8880 continue;
8881 }
8882 CheckValue<IkReal> x590=IKPowWithIntegerCheck(IKsign(gconst8),-1);
8883 if(!x590.valid){
8884 continue;
8885 }
8886 j3array[0]=((-1.5707963267949)+(x589.value)+(((1.5707963267949)*(x590.value))));
8887 sj3array[0]=IKsin(j3array[0]);
8888 cj3array[0]=IKcos(j3array[0]);
8889 if( j3array[0] > IKPI )
8890 {
8891  j3array[0]-=IK2PI;
8892 }
8893 else if( j3array[0] < -IKPI )
8894 { j3array[0]+=IK2PI;
8895 }
8896 j3valid[0] = true;
8897 for(int ij3 = 0; ij3 < 1; ++ij3)
8898 {
8899 if( !j3valid[ij3] )
8900 {
8901  continue;
8902 }
8903 _ij3[0] = ij3; _ij3[1] = -1;
8904 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8905 {
8906 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8907 {
8908  j3valid[iij3]=false; _ij3[1] = iij3; break;
8909 }
8910 }
8911 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8912 {
8913 IkReal evalcond[8];
8914 IkReal x591=IKsin(j3);
8915 IkReal x592=IKcos(j3);
8916 IkReal x593=((1.0)*gconst8);
8917 evalcond[0]=(new_r11*x591);
8918 evalcond[1]=((-1.0)*gconst8*x591);
8919 evalcond[2]=(((gconst8*x592))+new_r11);
8920 evalcond[3]=(((new_r11*x592))+gconst8);
8921 evalcond[4]=((((-1.0)*x592*x593))+new_r00);
8922 evalcond[5]=(new_r10+(((-1.0)*x591*x593)));
8923 evalcond[6]=(((new_r10*x592))+(((-1.0)*new_r00*x591)));
8924 evalcond[7]=(((new_r10*x591))+((new_r00*x592))+(((-1.0)*x593)));
8925 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8926 {
8927 continue;
8928 }
8929 }
8930 
8931 {
8932 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8933 vinfos[0].jointtype = 1;
8934 vinfos[0].foffset = j0;
8935 vinfos[0].indices[0] = _ij0[0];
8936 vinfos[0].indices[1] = _ij0[1];
8937 vinfos[0].maxsolutions = _nj0;
8938 vinfos[1].jointtype = 1;
8939 vinfos[1].foffset = j1;
8940 vinfos[1].indices[0] = _ij1[0];
8941 vinfos[1].indices[1] = _ij1[1];
8942 vinfos[1].maxsolutions = _nj1;
8943 vinfos[2].jointtype = 1;
8944 vinfos[2].foffset = j2;
8945 vinfos[2].indices[0] = _ij2[0];
8946 vinfos[2].indices[1] = _ij2[1];
8947 vinfos[2].maxsolutions = _nj2;
8948 vinfos[3].jointtype = 1;
8949 vinfos[3].foffset = j3;
8950 vinfos[3].indices[0] = _ij3[0];
8951 vinfos[3].indices[1] = _ij3[1];
8952 vinfos[3].maxsolutions = _nj3;
8953 vinfos[4].jointtype = 1;
8954 vinfos[4].foffset = j4;
8955 vinfos[4].indices[0] = _ij4[0];
8956 vinfos[4].indices[1] = _ij4[1];
8957 vinfos[4].maxsolutions = _nj4;
8958 vinfos[5].jointtype = 1;
8959 vinfos[5].foffset = j5;
8960 vinfos[5].indices[0] = _ij5[0];
8961 vinfos[5].indices[1] = _ij5[1];
8962 vinfos[5].maxsolutions = _nj5;
8963 std::vector<int> vfree(0);
8964 solutions.AddSolution(vinfos,vfree);
8965 }
8966 }
8967 }
8968 
8969 }
8970 
8971 }
8972 
8973 } else
8974 {
8975 {
8976 IkReal j3array[1], cj3array[1], sj3array[1];
8977 bool j3valid[1]={false};
8978 _nj3 = 1;
8979 CheckValue<IkReal> x594=IKPowWithIntegerCheck(IKsign(gconst8),-1);
8980 if(!x594.valid){
8981 continue;
8982 }
8983 CheckValue<IkReal> x595 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8984 if(!x595.valid){
8985 continue;
8986 }
8987 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x594.value)))+(x595.value));
8988 sj3array[0]=IKsin(j3array[0]);
8989 cj3array[0]=IKcos(j3array[0]);
8990 if( j3array[0] > IKPI )
8991 {
8992  j3array[0]-=IK2PI;
8993 }
8994 else if( j3array[0] < -IKPI )
8995 { j3array[0]+=IK2PI;
8996 }
8997 j3valid[0] = true;
8998 for(int ij3 = 0; ij3 < 1; ++ij3)
8999 {
9000 if( !j3valid[ij3] )
9001 {
9002  continue;
9003 }
9004 _ij3[0] = ij3; _ij3[1] = -1;
9005 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9006 {
9007 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9008 {
9009  j3valid[iij3]=false; _ij3[1] = iij3; break;
9010 }
9011 }
9012 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9013 {
9014 IkReal evalcond[8];
9015 IkReal x596=IKsin(j3);
9016 IkReal x597=IKcos(j3);
9017 IkReal x598=((1.0)*gconst8);
9018 evalcond[0]=(new_r11*x596);
9019 evalcond[1]=((-1.0)*gconst8*x596);
9020 evalcond[2]=(((gconst8*x597))+new_r11);
9021 evalcond[3]=(((new_r11*x597))+gconst8);
9022 evalcond[4]=((((-1.0)*x597*x598))+new_r00);
9023 evalcond[5]=((((-1.0)*x596*x598))+new_r10);
9024 evalcond[6]=(((new_r10*x597))+(((-1.0)*new_r00*x596)));
9025 evalcond[7]=(((new_r10*x596))+((new_r00*x597))+(((-1.0)*x598)));
9026 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9027 {
9028 continue;
9029 }
9030 }
9031 
9032 {
9033 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9034 vinfos[0].jointtype = 1;
9035 vinfos[0].foffset = j0;
9036 vinfos[0].indices[0] = _ij0[0];
9037 vinfos[0].indices[1] = _ij0[1];
9038 vinfos[0].maxsolutions = _nj0;
9039 vinfos[1].jointtype = 1;
9040 vinfos[1].foffset = j1;
9041 vinfos[1].indices[0] = _ij1[0];
9042 vinfos[1].indices[1] = _ij1[1];
9043 vinfos[1].maxsolutions = _nj1;
9044 vinfos[2].jointtype = 1;
9045 vinfos[2].foffset = j2;
9046 vinfos[2].indices[0] = _ij2[0];
9047 vinfos[2].indices[1] = _ij2[1];
9048 vinfos[2].maxsolutions = _nj2;
9049 vinfos[3].jointtype = 1;
9050 vinfos[3].foffset = j3;
9051 vinfos[3].indices[0] = _ij3[0];
9052 vinfos[3].indices[1] = _ij3[1];
9053 vinfos[3].maxsolutions = _nj3;
9054 vinfos[4].jointtype = 1;
9055 vinfos[4].foffset = j4;
9056 vinfos[4].indices[0] = _ij4[0];
9057 vinfos[4].indices[1] = _ij4[1];
9058 vinfos[4].maxsolutions = _nj4;
9059 vinfos[5].jointtype = 1;
9060 vinfos[5].foffset = j5;
9061 vinfos[5].indices[0] = _ij5[0];
9062 vinfos[5].indices[1] = _ij5[1];
9063 vinfos[5].maxsolutions = _nj5;
9064 std::vector<int> vfree(0);
9065 solutions.AddSolution(vinfos,vfree);
9066 }
9067 }
9068 }
9069 
9070 }
9071 
9072 }
9073 
9074 }
9075 } while(0);
9076 if( bgotonextstatement )
9077 {
9078 bool bgotonextstatement = true;
9079 do
9080 {
9081 if( 1 )
9082 {
9083 bgotonextstatement=false;
9084 continue; // branch miss [j3]
9085 
9086 }
9087 } while(0);
9088 if( bgotonextstatement )
9089 {
9090 }
9091 }
9092 }
9093 }
9094 }
9095 }
9096 }
9097 }
9098 
9099 } else
9100 {
9101 {
9102 IkReal j3array[1], cj3array[1], sj3array[1];
9103 bool j3valid[1]={false};
9104 _nj3 = 1;
9105 IkReal x599=((1.0)*new_r11);
9106 CheckValue<IkReal> x600=IKPowWithIntegerCheck(IKsign(((gconst7*gconst7)+(gconst8*gconst8))),-1);
9107 if(!x600.valid){
9108 continue;
9109 }
9110 CheckValue<IkReal> x601 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x599)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x599)))),IKFAST_ATAN2_MAGTHRESH);
9111 if(!x601.valid){
9112 continue;
9113 }
9114 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x600.value)))+(x601.value));
9115 sj3array[0]=IKsin(j3array[0]);
9116 cj3array[0]=IKcos(j3array[0]);
9117 if( j3array[0] > IKPI )
9118 {
9119  j3array[0]-=IK2PI;
9120 }
9121 else if( j3array[0] < -IKPI )
9122 { j3array[0]+=IK2PI;
9123 }
9124 j3valid[0] = true;
9125 for(int ij3 = 0; ij3 < 1; ++ij3)
9126 {
9127 if( !j3valid[ij3] )
9128 {
9129  continue;
9130 }
9131 _ij3[0] = ij3; _ij3[1] = -1;
9132 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9133 {
9134 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9135 {
9136  j3valid[iij3]=false; _ij3[1] = iij3; break;
9137 }
9138 }
9139 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9140 {
9141 IkReal evalcond[8];
9142 IkReal x602=IKcos(j3);
9143 IkReal x603=IKsin(j3);
9144 IkReal x604=((1.0)*gconst8);
9145 IkReal x605=(gconst7*x602);
9146 IkReal x606=(gconst7*x603);
9147 IkReal x607=((1.0)*x603);
9148 IkReal x608=(x603*x604);
9149 evalcond[0]=(gconst7+((new_r11*x603))+((new_r01*x602)));
9150 evalcond[1]=(((gconst8*x602))+x606+new_r11);
9151 evalcond[2]=(x605+(((-1.0)*x608))+new_r01);
9152 evalcond[3]=(gconst7+((new_r10*x602))+(((-1.0)*new_r00*x607)));
9153 evalcond[4]=(gconst8+((new_r11*x602))+(((-1.0)*new_r01*x607)));
9154 evalcond[5]=(x605+(((-1.0)*x608))+new_r10);
9155 evalcond[6]=(((new_r10*x603))+((new_r00*x602))+(((-1.0)*x604)));
9156 evalcond[7]=((((-1.0)*x602*x604))+(((-1.0)*x606))+new_r00);
9157 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9158 {
9159 continue;
9160 }
9161 }
9162 
9163 {
9164 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9165 vinfos[0].jointtype = 1;
9166 vinfos[0].foffset = j0;
9167 vinfos[0].indices[0] = _ij0[0];
9168 vinfos[0].indices[1] = _ij0[1];
9169 vinfos[0].maxsolutions = _nj0;
9170 vinfos[1].jointtype = 1;
9171 vinfos[1].foffset = j1;
9172 vinfos[1].indices[0] = _ij1[0];
9173 vinfos[1].indices[1] = _ij1[1];
9174 vinfos[1].maxsolutions = _nj1;
9175 vinfos[2].jointtype = 1;
9176 vinfos[2].foffset = j2;
9177 vinfos[2].indices[0] = _ij2[0];
9178 vinfos[2].indices[1] = _ij2[1];
9179 vinfos[2].maxsolutions = _nj2;
9180 vinfos[3].jointtype = 1;
9181 vinfos[3].foffset = j3;
9182 vinfos[3].indices[0] = _ij3[0];
9183 vinfos[3].indices[1] = _ij3[1];
9184 vinfos[3].maxsolutions = _nj3;
9185 vinfos[4].jointtype = 1;
9186 vinfos[4].foffset = j4;
9187 vinfos[4].indices[0] = _ij4[0];
9188 vinfos[4].indices[1] = _ij4[1];
9189 vinfos[4].maxsolutions = _nj4;
9190 vinfos[5].jointtype = 1;
9191 vinfos[5].foffset = j5;
9192 vinfos[5].indices[0] = _ij5[0];
9193 vinfos[5].indices[1] = _ij5[1];
9194 vinfos[5].maxsolutions = _nj5;
9195 std::vector<int> vfree(0);
9196 solutions.AddSolution(vinfos,vfree);
9197 }
9198 }
9199 }
9200 
9201 }
9202 
9203 }
9204 
9205 } else
9206 {
9207 {
9208 IkReal j3array[1], cj3array[1], sj3array[1];
9209 bool j3valid[1]={false};
9210 _nj3 = 1;
9211 IkReal x609=((1.0)*new_r11);
9212 CheckValue<IkReal> x610 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x609)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x609)))),IKFAST_ATAN2_MAGTHRESH);
9213 if(!x610.valid){
9214 continue;
9215 }
9216 CheckValue<IkReal> x611=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
9217 if(!x611.valid){
9218 continue;
9219 }
9220 j3array[0]=((-1.5707963267949)+(x610.value)+(((1.5707963267949)*(x611.value))));
9221 sj3array[0]=IKsin(j3array[0]);
9222 cj3array[0]=IKcos(j3array[0]);
9223 if( j3array[0] > IKPI )
9224 {
9225  j3array[0]-=IK2PI;
9226 }
9227 else if( j3array[0] < -IKPI )
9228 { j3array[0]+=IK2PI;
9229 }
9230 j3valid[0] = true;
9231 for(int ij3 = 0; ij3 < 1; ++ij3)
9232 {
9233 if( !j3valid[ij3] )
9234 {
9235  continue;
9236 }
9237 _ij3[0] = ij3; _ij3[1] = -1;
9238 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9239 {
9240 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9241 {
9242  j3valid[iij3]=false; _ij3[1] = iij3; break;
9243 }
9244 }
9245 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9246 {
9247 IkReal evalcond[8];
9248 IkReal x612=IKcos(j3);
9249 IkReal x613=IKsin(j3);
9250 IkReal x614=((1.0)*gconst8);
9251 IkReal x615=(gconst7*x612);
9252 IkReal x616=(gconst7*x613);
9253 IkReal x617=((1.0)*x613);
9254 IkReal x618=(x613*x614);
9255 evalcond[0]=(gconst7+((new_r11*x613))+((new_r01*x612)));
9256 evalcond[1]=(((gconst8*x612))+x616+new_r11);
9257 evalcond[2]=((((-1.0)*x618))+x615+new_r01);
9258 evalcond[3]=(gconst7+((new_r10*x612))+(((-1.0)*new_r00*x617)));
9259 evalcond[4]=(gconst8+((new_r11*x612))+(((-1.0)*new_r01*x617)));
9260 evalcond[5]=((((-1.0)*x618))+x615+new_r10);
9261 evalcond[6]=(((new_r10*x613))+(((-1.0)*x614))+((new_r00*x612)));
9262 evalcond[7]=((((-1.0)*x612*x614))+(((-1.0)*x616))+new_r00);
9263 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9264 {
9265 continue;
9266 }
9267 }
9268 
9269 {
9270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9271 vinfos[0].jointtype = 1;
9272 vinfos[0].foffset = j0;
9273 vinfos[0].indices[0] = _ij0[0];
9274 vinfos[0].indices[1] = _ij0[1];
9275 vinfos[0].maxsolutions = _nj0;
9276 vinfos[1].jointtype = 1;
9277 vinfos[1].foffset = j1;
9278 vinfos[1].indices[0] = _ij1[0];
9279 vinfos[1].indices[1] = _ij1[1];
9280 vinfos[1].maxsolutions = _nj1;
9281 vinfos[2].jointtype = 1;
9282 vinfos[2].foffset = j2;
9283 vinfos[2].indices[0] = _ij2[0];
9284 vinfos[2].indices[1] = _ij2[1];
9285 vinfos[2].maxsolutions = _nj2;
9286 vinfos[3].jointtype = 1;
9287 vinfos[3].foffset = j3;
9288 vinfos[3].indices[0] = _ij3[0];
9289 vinfos[3].indices[1] = _ij3[1];
9290 vinfos[3].maxsolutions = _nj3;
9291 vinfos[4].jointtype = 1;
9292 vinfos[4].foffset = j4;
9293 vinfos[4].indices[0] = _ij4[0];
9294 vinfos[4].indices[1] = _ij4[1];
9295 vinfos[4].maxsolutions = _nj4;
9296 vinfos[5].jointtype = 1;
9297 vinfos[5].foffset = j5;
9298 vinfos[5].indices[0] = _ij5[0];
9299 vinfos[5].indices[1] = _ij5[1];
9300 vinfos[5].maxsolutions = _nj5;
9301 std::vector<int> vfree(0);
9302 solutions.AddSolution(vinfos,vfree);
9303 }
9304 }
9305 }
9306 
9307 }
9308 
9309 }
9310 
9311 } else
9312 {
9313 {
9314 IkReal j3array[1], cj3array[1], sj3array[1];
9315 bool j3valid[1]={false};
9316 _nj3 = 1;
9317 IkReal x619=((1.0)*gconst7);
9318 CheckValue<IkReal> x620 = IKatan2WithCheck(IkReal(((((-1.0)*new_r10*x619))+((gconst7*new_r01)))),IkReal(((((-1.0)*new_r11*x619))+(((-1.0)*new_r00*x619)))),IKFAST_ATAN2_MAGTHRESH);
9319 if(!x620.valid){
9320 continue;
9321 }
9322 CheckValue<IkReal> x621=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
9323 if(!x621.valid){
9324 continue;
9325 }
9326 j3array[0]=((-1.5707963267949)+(x620.value)+(((1.5707963267949)*(x621.value))));
9327 sj3array[0]=IKsin(j3array[0]);
9328 cj3array[0]=IKcos(j3array[0]);
9329 if( j3array[0] > IKPI )
9330 {
9331  j3array[0]-=IK2PI;
9332 }
9333 else if( j3array[0] < -IKPI )
9334 { j3array[0]+=IK2PI;
9335 }
9336 j3valid[0] = true;
9337 for(int ij3 = 0; ij3 < 1; ++ij3)
9338 {
9339 if( !j3valid[ij3] )
9340 {
9341  continue;
9342 }
9343 _ij3[0] = ij3; _ij3[1] = -1;
9344 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9345 {
9346 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9347 {
9348  j3valid[iij3]=false; _ij3[1] = iij3; break;
9349 }
9350 }
9351 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9352 {
9353 IkReal evalcond[8];
9354 IkReal x622=IKcos(j3);
9355 IkReal x623=IKsin(j3);
9356 IkReal x624=((1.0)*gconst8);
9357 IkReal x625=(gconst7*x622);
9358 IkReal x626=(gconst7*x623);
9359 IkReal x627=((1.0)*x623);
9360 IkReal x628=(x623*x624);
9361 evalcond[0]=(gconst7+((new_r01*x622))+((new_r11*x623)));
9362 evalcond[1]=(x626+((gconst8*x622))+new_r11);
9363 evalcond[2]=((((-1.0)*x628))+x625+new_r01);
9364 evalcond[3]=((((-1.0)*new_r00*x627))+gconst7+((new_r10*x622)));
9365 evalcond[4]=((((-1.0)*new_r01*x627))+gconst8+((new_r11*x622)));
9366 evalcond[5]=((((-1.0)*x628))+x625+new_r10);
9367 evalcond[6]=((((-1.0)*x624))+((new_r00*x622))+((new_r10*x623)));
9368 evalcond[7]=((((-1.0)*x622*x624))+(((-1.0)*x626))+new_r00);
9369 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
9370 {
9371 continue;
9372 }
9373 }
9374 
9375 {
9376 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9377 vinfos[0].jointtype = 1;
9378 vinfos[0].foffset = j0;
9379 vinfos[0].indices[0] = _ij0[0];
9380 vinfos[0].indices[1] = _ij0[1];
9381 vinfos[0].maxsolutions = _nj0;
9382 vinfos[1].jointtype = 1;
9383 vinfos[1].foffset = j1;
9384 vinfos[1].indices[0] = _ij1[0];
9385 vinfos[1].indices[1] = _ij1[1];
9386 vinfos[1].maxsolutions = _nj1;
9387 vinfos[2].jointtype = 1;
9388 vinfos[2].foffset = j2;
9389 vinfos[2].indices[0] = _ij2[0];
9390 vinfos[2].indices[1] = _ij2[1];
9391 vinfos[2].maxsolutions = _nj2;
9392 vinfos[3].jointtype = 1;
9393 vinfos[3].foffset = j3;
9394 vinfos[3].indices[0] = _ij3[0];
9395 vinfos[3].indices[1] = _ij3[1];
9396 vinfos[3].maxsolutions = _nj3;
9397 vinfos[4].jointtype = 1;
9398 vinfos[4].foffset = j4;
9399 vinfos[4].indices[0] = _ij4[0];
9400 vinfos[4].indices[1] = _ij4[1];
9401 vinfos[4].maxsolutions = _nj4;
9402 vinfos[5].jointtype = 1;
9403 vinfos[5].foffset = j5;
9404 vinfos[5].indices[0] = _ij5[0];
9405 vinfos[5].indices[1] = _ij5[1];
9406 vinfos[5].maxsolutions = _nj5;
9407 std::vector<int> vfree(0);
9408 solutions.AddSolution(vinfos,vfree);
9409 }
9410 }
9411 }
9412 
9413 }
9414 
9415 }
9416 
9417 }
9418 } while(0);
9419 if( bgotonextstatement )
9420 {
9421 bool bgotonextstatement = true;
9422 do
9423 {
9424 IkReal x630 = ((new_r01*new_r01)+(new_r11*new_r11));
9425 if(IKabs(x630)==0){
9426 continue;
9427 }
9428 IkReal x629=pow(x630,-0.5);
9429 CheckValue<IkReal> x631 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9430 if(!x631.valid){
9431 continue;
9432 }
9433 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x631.value))));
9434 IkReal gconst10=((1.0)*new_r01*x629);
9435 IkReal gconst11=((-1.0)*new_r11*x629);
9436 CheckValue<IkReal> x632 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9437 if(!x632.valid){
9438 continue;
9439 }
9440 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x632.value)+j5)))), 6.28318530717959)));
9441 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9442 {
9443 bgotonextstatement=false;
9444 {
9445 IkReal j3eval[3];
9446 CheckValue<IkReal> x635 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9447 if(!x635.valid){
9448 continue;
9449 }
9450 IkReal x633=((1.0)*(x635.value));
9451 IkReal x634=x629;
9452 sj4=0;
9453 cj4=-1.0;
9454 j4=3.14159265358979;
9455 sj5=gconst10;
9456 cj5=gconst11;
9457 j5=((3.14159265)+(((-1.0)*x633)));
9458 IkReal gconst9=((3.14159265358979)+(((-1.0)*x633)));
9459 IkReal gconst10=((1.0)*new_r01*x634);
9460 IkReal gconst11=((-1.0)*new_r11*x634);
9461 IkReal x636=new_r01*new_r01;
9462 IkReal x637=(((new_r10*new_r11))+((new_r00*new_r01)));
9463 IkReal x638=x629;
9464 IkReal x639=((1.0)*new_r01*x638);
9465 j3eval[0]=x637;
9466 j3eval[1]=((IKabs(((((-1.0)*new_r00*x639))+(((-1.0)*new_r11*x639)))))+(IKabs((((x636*x638))+(((-1.0)*new_r10*x639))))));
9467 j3eval[2]=IKsign(x637);
9468 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9469 {
9470 {
9471 IkReal j3eval[2];
9472 CheckValue<IkReal> x642 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9473 if(!x642.valid){
9474 continue;
9475 }
9476 IkReal x640=((1.0)*(x642.value));
9477 IkReal x641=x629;
9478 sj4=0;
9479 cj4=-1.0;
9480 j4=3.14159265358979;
9481 sj5=gconst10;
9482 cj5=gconst11;
9483 j5=((3.14159265)+(((-1.0)*x640)));
9484 IkReal gconst9=((3.14159265358979)+(((-1.0)*x640)));
9485 IkReal gconst10=((1.0)*new_r01*x641);
9486 IkReal gconst11=((-1.0)*new_r11*x641);
9487 IkReal x643=((new_r01*new_r01)+(new_r11*new_r11));
9488 j3eval[0]=x643;
9489 j3eval[1]=IKsign(x643);
9490 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9491 {
9492 {
9493 IkReal j3eval[1];
9494 CheckValue<IkReal> x646 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9495 if(!x646.valid){
9496 continue;
9497 }
9498 IkReal x644=((1.0)*(x646.value));
9499 IkReal x645=x629;
9500 sj4=0;
9501 cj4=-1.0;
9502 j4=3.14159265358979;
9503 sj5=gconst10;
9504 cj5=gconst11;
9505 j5=((3.14159265)+(((-1.0)*x644)));
9506 IkReal gconst9=((3.14159265358979)+(((-1.0)*x644)));
9507 IkReal gconst10=((1.0)*new_r01*x645);
9508 IkReal gconst11=((-1.0)*new_r11*x645);
9509 j3eval[0]=((new_r01*new_r01)+(new_r11*new_r11));
9510 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9511 {
9512 {
9513 IkReal evalcond[5];
9514 bool bgotonextstatement = true;
9515 do
9516 {
9517 evalcond[0]=((gconst10*gconst10)+(gconst11*gconst11));
9518 evalcond[1]=new_r01;
9519 evalcond[2]=new_r00;
9520 evalcond[3]=new_r11;
9521 evalcond[4]=new_r10;
9522 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 )
9523 {
9524 bgotonextstatement=false;
9525 {
9526 IkReal j3array[4], cj3array[4], sj3array[4];
9527 bool j3valid[4]={false};
9528 _nj3 = 4;
9529 j3array[0]=0;
9530 sj3array[0]=IKsin(j3array[0]);
9531 cj3array[0]=IKcos(j3array[0]);
9532 j3array[1]=1.5707963267949;
9533 sj3array[1]=IKsin(j3array[1]);
9534 cj3array[1]=IKcos(j3array[1]);
9535 j3array[2]=3.14159265358979;
9536 sj3array[2]=IKsin(j3array[2]);
9537 cj3array[2]=IKcos(j3array[2]);
9538 j3array[3]=-1.5707963267949;
9539 sj3array[3]=IKsin(j3array[3]);
9540 cj3array[3]=IKcos(j3array[3]);
9541 if( j3array[0] > IKPI )
9542 {
9543  j3array[0]-=IK2PI;
9544 }
9545 else if( j3array[0] < -IKPI )
9546 { j3array[0]+=IK2PI;
9547 }
9548 j3valid[0] = true;
9549 if( j3array[1] > IKPI )
9550 {
9551  j3array[1]-=IK2PI;
9552 }
9553 else if( j3array[1] < -IKPI )
9554 { j3array[1]+=IK2PI;
9555 }
9556 j3valid[1] = true;
9557 if( j3array[2] > IKPI )
9558 {
9559  j3array[2]-=IK2PI;
9560 }
9561 else if( j3array[2] < -IKPI )
9562 { j3array[2]+=IK2PI;
9563 }
9564 j3valid[2] = true;
9565 if( j3array[3] > IKPI )
9566 {
9567  j3array[3]-=IK2PI;
9568 }
9569 else if( j3array[3] < -IKPI )
9570 { j3array[3]+=IK2PI;
9571 }
9572 j3valid[3] = true;
9573 for(int ij3 = 0; ij3 < 4; ++ij3)
9574 {
9575 if( !j3valid[ij3] )
9576 {
9577  continue;
9578 }
9579 _ij3[0] = ij3; _ij3[1] = -1;
9580 for(int iij3 = ij3+1; iij3 < 4; ++iij3)
9581 {
9582 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9583 {
9584  j3valid[iij3]=false; _ij3[1] = iij3; break;
9585 }
9586 }
9587 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9588 
9589 {
9590 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9591 vinfos[0].jointtype = 1;
9592 vinfos[0].foffset = j0;
9593 vinfos[0].indices[0] = _ij0[0];
9594 vinfos[0].indices[1] = _ij0[1];
9595 vinfos[0].maxsolutions = _nj0;
9596 vinfos[1].jointtype = 1;
9597 vinfos[1].foffset = j1;
9598 vinfos[1].indices[0] = _ij1[0];
9599 vinfos[1].indices[1] = _ij1[1];
9600 vinfos[1].maxsolutions = _nj1;
9601 vinfos[2].jointtype = 1;
9602 vinfos[2].foffset = j2;
9603 vinfos[2].indices[0] = _ij2[0];
9604 vinfos[2].indices[1] = _ij2[1];
9605 vinfos[2].maxsolutions = _nj2;
9606 vinfos[3].jointtype = 1;
9607 vinfos[3].foffset = j3;
9608 vinfos[3].indices[0] = _ij3[0];
9609 vinfos[3].indices[1] = _ij3[1];
9610 vinfos[3].maxsolutions = _nj3;
9611 vinfos[4].jointtype = 1;
9612 vinfos[4].foffset = j4;
9613 vinfos[4].indices[0] = _ij4[0];
9614 vinfos[4].indices[1] = _ij4[1];
9615 vinfos[4].maxsolutions = _nj4;
9616 vinfos[5].jointtype = 1;
9617 vinfos[5].foffset = j5;
9618 vinfos[5].indices[0] = _ij5[0];
9619 vinfos[5].indices[1] = _ij5[1];
9620 vinfos[5].maxsolutions = _nj5;
9621 std::vector<int> vfree(0);
9622 solutions.AddSolution(vinfos,vfree);
9623 }
9624 }
9625 }
9626 
9627 }
9628 } while(0);
9629 if( bgotonextstatement )
9630 {
9631 bool bgotonextstatement = true;
9632 do
9633 {
9634 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
9635 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9636 {
9637 bgotonextstatement=false;
9638 {
9639 IkReal j3eval[1];
9640 CheckValue<IkReal> x648 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
9641 if(!x648.valid){
9642 continue;
9643 }
9644 IkReal x647=((1.0)*(x648.value));
9645 sj4=0;
9646 cj4=-1.0;
9647 j4=3.14159265358979;
9648 sj5=gconst10;
9649 cj5=gconst11;
9650 j5=((3.14159265)+(((-1.0)*x647)));
9651 new_r11=0;
9652 new_r00=0;
9653 IkReal gconst9=((3.14159265358979)+(((-1.0)*x647)));
9654 IkReal x649 = new_r01*new_r01;
9655 if(IKabs(x649)==0){
9656 continue;
9657 }
9658 IkReal gconst10=((1.0)*new_r01*(pow(x649,-0.5)));
9659 IkReal gconst11=0;
9660 j3eval[0]=new_r01;
9661 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9662 {
9663 {
9664 IkReal j3array[2], cj3array[2], sj3array[2];
9665 bool j3valid[2]={false};
9666 _nj3 = 2;
9667 CheckValue<IkReal> x650=IKPowWithIntegerCheck(gconst10,-1);
9668 if(!x650.valid){
9669 continue;
9670 }
9671 cj3array[0]=((-1.0)*new_r01*(x650.value));
9672 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
9673 {
9674  j3valid[0] = j3valid[1] = true;
9675  j3array[0] = IKacos(cj3array[0]);
9676  sj3array[0] = IKsin(j3array[0]);
9677  cj3array[1] = cj3array[0];
9678  j3array[1] = -j3array[0];
9679  sj3array[1] = -sj3array[0];
9680 }
9681 else if( isnan(cj3array[0]) )
9682 {
9683  // probably any value will work
9684  j3valid[0] = true;
9685  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
9686 }
9687 for(int ij3 = 0; ij3 < 2; ++ij3)
9688 {
9689 if( !j3valid[ij3] )
9690 {
9691  continue;
9692 }
9693 _ij3[0] = ij3; _ij3[1] = -1;
9694 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9695 {
9696 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9697 {
9698  j3valid[iij3]=false; _ij3[1] = iij3; break;
9699 }
9700 }
9701 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9702 {
9703 IkReal evalcond[6];
9704 IkReal x651=IKsin(j3);
9705 IkReal x652=IKcos(j3);
9706 IkReal x653=((-1.0)*x651);
9707 evalcond[0]=(new_r10*x651);
9708 evalcond[1]=(gconst10*x653);
9709 evalcond[2]=(new_r01*x653);
9710 evalcond[3]=(gconst10+((new_r01*x652)));
9711 evalcond[4]=(gconst10+((new_r10*x652)));
9712 evalcond[5]=(((gconst10*x652))+new_r10);
9713 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
9714 {
9715 continue;
9716 }
9717 }
9718 
9719 {
9720 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9721 vinfos[0].jointtype = 1;
9722 vinfos[0].foffset = j0;
9723 vinfos[0].indices[0] = _ij0[0];
9724 vinfos[0].indices[1] = _ij0[1];
9725 vinfos[0].maxsolutions = _nj0;
9726 vinfos[1].jointtype = 1;
9727 vinfos[1].foffset = j1;
9728 vinfos[1].indices[0] = _ij1[0];
9729 vinfos[1].indices[1] = _ij1[1];
9730 vinfos[1].maxsolutions = _nj1;
9731 vinfos[2].jointtype = 1;
9732 vinfos[2].foffset = j2;
9733 vinfos[2].indices[0] = _ij2[0];
9734 vinfos[2].indices[1] = _ij2[1];
9735 vinfos[2].maxsolutions = _nj2;
9736 vinfos[3].jointtype = 1;
9737 vinfos[3].foffset = j3;
9738 vinfos[3].indices[0] = _ij3[0];
9739 vinfos[3].indices[1] = _ij3[1];
9740 vinfos[3].maxsolutions = _nj3;
9741 vinfos[4].jointtype = 1;
9742 vinfos[4].foffset = j4;
9743 vinfos[4].indices[0] = _ij4[0];
9744 vinfos[4].indices[1] = _ij4[1];
9745 vinfos[4].maxsolutions = _nj4;
9746 vinfos[5].jointtype = 1;
9747 vinfos[5].foffset = j5;
9748 vinfos[5].indices[0] = _ij5[0];
9749 vinfos[5].indices[1] = _ij5[1];
9750 vinfos[5].maxsolutions = _nj5;
9751 std::vector<int> vfree(0);
9752 solutions.AddSolution(vinfos,vfree);
9753 }
9754 }
9755 }
9756 
9757 } else
9758 {
9759 {
9760 IkReal j3array[2], cj3array[2], sj3array[2];
9761 bool j3valid[2]={false};
9762 _nj3 = 2;
9763 CheckValue<IkReal> x654=IKPowWithIntegerCheck(new_r01,-1);
9764 if(!x654.valid){
9765 continue;
9766 }
9767 cj3array[0]=((-1.0)*gconst10*(x654.value));
9768 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
9769 {
9770  j3valid[0] = j3valid[1] = true;
9771  j3array[0] = IKacos(cj3array[0]);
9772  sj3array[0] = IKsin(j3array[0]);
9773  cj3array[1] = cj3array[0];
9774  j3array[1] = -j3array[0];
9775  sj3array[1] = -sj3array[0];
9776 }
9777 else if( isnan(cj3array[0]) )
9778 {
9779  // probably any value will work
9780  j3valid[0] = true;
9781  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
9782 }
9783 for(int ij3 = 0; ij3 < 2; ++ij3)
9784 {
9785 if( !j3valid[ij3] )
9786 {
9787  continue;
9788 }
9789 _ij3[0] = ij3; _ij3[1] = -1;
9790 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9791 {
9792 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9793 {
9794  j3valid[iij3]=false; _ij3[1] = iij3; break;
9795 }
9796 }
9797 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9798 {
9799 IkReal evalcond[6];
9800 IkReal x655=IKsin(j3);
9801 IkReal x656=IKcos(j3);
9802 IkReal x657=(gconst10*x656);
9803 IkReal x658=((-1.0)*x655);
9804 evalcond[0]=(new_r10*x655);
9805 evalcond[1]=(gconst10*x658);
9806 evalcond[2]=(new_r01*x658);
9807 evalcond[3]=(x657+new_r01);
9808 evalcond[4]=(gconst10+((new_r10*x656)));
9809 evalcond[5]=(x657+new_r10);
9810 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
9811 {
9812 continue;
9813 }
9814 }
9815 
9816 {
9817 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9818 vinfos[0].jointtype = 1;
9819 vinfos[0].foffset = j0;
9820 vinfos[0].indices[0] = _ij0[0];
9821 vinfos[0].indices[1] = _ij0[1];
9822 vinfos[0].maxsolutions = _nj0;
9823 vinfos[1].jointtype = 1;
9824 vinfos[1].foffset = j1;
9825 vinfos[1].indices[0] = _ij1[0];
9826 vinfos[1].indices[1] = _ij1[1];
9827 vinfos[1].maxsolutions = _nj1;
9828 vinfos[2].jointtype = 1;
9829 vinfos[2].foffset = j2;
9830 vinfos[2].indices[0] = _ij2[0];
9831 vinfos[2].indices[1] = _ij2[1];
9832 vinfos[2].maxsolutions = _nj2;
9833 vinfos[3].jointtype = 1;
9834 vinfos[3].foffset = j3;
9835 vinfos[3].indices[0] = _ij3[0];
9836 vinfos[3].indices[1] = _ij3[1];
9837 vinfos[3].maxsolutions = _nj3;
9838 vinfos[4].jointtype = 1;
9839 vinfos[4].foffset = j4;
9840 vinfos[4].indices[0] = _ij4[0];
9841 vinfos[4].indices[1] = _ij4[1];
9842 vinfos[4].maxsolutions = _nj4;
9843 vinfos[5].jointtype = 1;
9844 vinfos[5].foffset = j5;
9845 vinfos[5].indices[0] = _ij5[0];
9846 vinfos[5].indices[1] = _ij5[1];
9847 vinfos[5].maxsolutions = _nj5;
9848 std::vector<int> vfree(0);
9849 solutions.AddSolution(vinfos,vfree);
9850 }
9851 }
9852 }
9853 
9854 }
9855 
9856 }
9857 
9858 }
9859 } while(0);
9860 if( bgotonextstatement )
9861 {
9862 bool bgotonextstatement = true;
9863 do
9864 {
9865 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9866 evalcond[1]=gconst10;
9867 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
9868 {
9869 bgotonextstatement=false;
9870 {
9871 IkReal j3eval[4];
9872 CheckValue<IkReal> x660 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9873 if(!x660.valid){
9874 continue;
9875 }
9876 IkReal x659=((1.0)*(x660.value));
9877 sj4=0;
9878 cj4=-1.0;
9879 j4=3.14159265358979;
9880 sj5=gconst10;
9881 cj5=gconst11;
9882 j5=((3.14159265)+(((-1.0)*x659)));
9883 new_r00=0;
9884 new_r10=0;
9885 new_r21=0;
9886 new_r22=0;
9887 IkReal gconst9=((3.14159265358979)+(((-1.0)*x659)));
9888 IkReal gconst10=((1.0)*new_r01);
9889 IkReal gconst11=((-1.0)*new_r11);
9890 j3eval[0]=1.0;
9891 j3eval[1]=1.0;
9892 j3eval[2]=new_r01;
9893 j3eval[3]=1.0;
9894 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
9895 {
9896 {
9897 IkReal j3eval[3];
9898 CheckValue<IkReal> x662 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9899 if(!x662.valid){
9900 continue;
9901 }
9902 IkReal x661=((1.0)*(x662.value));
9903 sj4=0;
9904 cj4=-1.0;
9905 j4=3.14159265358979;
9906 sj5=gconst10;
9907 cj5=gconst11;
9908 j5=((3.14159265)+(((-1.0)*x661)));
9909 new_r00=0;
9910 new_r10=0;
9911 new_r21=0;
9912 new_r22=0;
9913 IkReal gconst9=((3.14159265358979)+(((-1.0)*x661)));
9914 IkReal gconst10=((1.0)*new_r01);
9915 IkReal gconst11=((-1.0)*new_r11);
9916 j3eval[0]=-1.0;
9917 j3eval[1]=-1.0;
9918 j3eval[2]=((IKabs(((-1.0)+(new_r01*new_r01))))+(IKabs(((1.0)*new_r01*new_r11))));
9919 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9920 {
9921 {
9922 IkReal j3eval[3];
9923 CheckValue<IkReal> x664 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9924 if(!x664.valid){
9925 continue;
9926 }
9927 IkReal x663=((1.0)*(x664.value));
9928 sj4=0;
9929 cj4=-1.0;
9930 j4=3.14159265358979;
9931 sj5=gconst10;
9932 cj5=gconst11;
9933 j5=((3.14159265)+(((-1.0)*x663)));
9934 new_r00=0;
9935 new_r10=0;
9936 new_r21=0;
9937 new_r22=0;
9938 IkReal gconst9=((3.14159265358979)+(((-1.0)*x663)));
9939 IkReal gconst10=((1.0)*new_r01);
9940 IkReal gconst11=((-1.0)*new_r11);
9941 j3eval[0]=1.0;
9942 j3eval[1]=1.0;
9943 j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((1.0)+(((-2.0)*(new_r01*new_r01)))))));
9944 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9945 {
9946 continue; // 3 cases reached
9947 
9948 } else
9949 {
9950 {
9951 IkReal j3array[1], cj3array[1], sj3array[1];
9952 bool j3valid[1]={false};
9953 _nj3 = 1;
9954 IkReal x665=((1.0)*new_r11);
9955 CheckValue<IkReal> x666 = IKatan2WithCheck(IkReal(((((-1.0)*gconst10*x665))+((gconst11*new_r01)))),IkReal(((((-1.0)*gconst11*x665))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
9956 if(!x666.valid){
9957 continue;
9958 }
9959 CheckValue<IkReal> x667=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
9960 if(!x667.valid){
9961 continue;
9962 }
9963 j3array[0]=((-1.5707963267949)+(x666.value)+(((1.5707963267949)*(x667.value))));
9964 sj3array[0]=IKsin(j3array[0]);
9965 cj3array[0]=IKcos(j3array[0]);
9966 if( j3array[0] > IKPI )
9967 {
9968  j3array[0]-=IK2PI;
9969 }
9970 else if( j3array[0] < -IKPI )
9971 { j3array[0]+=IK2PI;
9972 }
9973 j3valid[0] = true;
9974 for(int ij3 = 0; ij3 < 1; ++ij3)
9975 {
9976 if( !j3valid[ij3] )
9977 {
9978  continue;
9979 }
9980 _ij3[0] = ij3; _ij3[1] = -1;
9981 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9982 {
9983 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9984 {
9985  j3valid[iij3]=false; _ij3[1] = iij3; break;
9986 }
9987 }
9988 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9989 {
9990 IkReal evalcond[6];
9991 IkReal x668=IKcos(j3);
9992 IkReal x669=IKsin(j3);
9993 IkReal x670=(gconst10*x668);
9994 IkReal x671=(gconst10*x669);
9995 IkReal x672=(gconst11*x668);
9996 IkReal x673=((1.0)*x669);
9997 IkReal x674=(gconst11*x673);
9998 evalcond[0]=(gconst10+((new_r11*x669))+((new_r01*x668)));
9999 evalcond[1]=(x672+x671+new_r11);
10000 evalcond[2]=(x670+(((-1.0)*x674)));
10001 evalcond[3]=(x670+new_r01+(((-1.0)*x674)));
10002 evalcond[4]=((((-1.0)*x671))+(((-1.0)*x672)));
10003 evalcond[5]=((((-1.0)*new_r01*x673))+gconst11+((new_r11*x668)));
10004 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10005 {
10006 continue;
10007 }
10008 }
10009 
10010 {
10011 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10012 vinfos[0].jointtype = 1;
10013 vinfos[0].foffset = j0;
10014 vinfos[0].indices[0] = _ij0[0];
10015 vinfos[0].indices[1] = _ij0[1];
10016 vinfos[0].maxsolutions = _nj0;
10017 vinfos[1].jointtype = 1;
10018 vinfos[1].foffset = j1;
10019 vinfos[1].indices[0] = _ij1[0];
10020 vinfos[1].indices[1] = _ij1[1];
10021 vinfos[1].maxsolutions = _nj1;
10022 vinfos[2].jointtype = 1;
10023 vinfos[2].foffset = j2;
10024 vinfos[2].indices[0] = _ij2[0];
10025 vinfos[2].indices[1] = _ij2[1];
10026 vinfos[2].maxsolutions = _nj2;
10027 vinfos[3].jointtype = 1;
10028 vinfos[3].foffset = j3;
10029 vinfos[3].indices[0] = _ij3[0];
10030 vinfos[3].indices[1] = _ij3[1];
10031 vinfos[3].maxsolutions = _nj3;
10032 vinfos[4].jointtype = 1;
10033 vinfos[4].foffset = j4;
10034 vinfos[4].indices[0] = _ij4[0];
10035 vinfos[4].indices[1] = _ij4[1];
10036 vinfos[4].maxsolutions = _nj4;
10037 vinfos[5].jointtype = 1;
10038 vinfos[5].foffset = j5;
10039 vinfos[5].indices[0] = _ij5[0];
10040 vinfos[5].indices[1] = _ij5[1];
10041 vinfos[5].maxsolutions = _nj5;
10042 std::vector<int> vfree(0);
10043 solutions.AddSolution(vinfos,vfree);
10044 }
10045 }
10046 }
10047 
10048 }
10049 
10050 }
10051 
10052 } else
10053 {
10054 {
10055 IkReal j3array[1], cj3array[1], sj3array[1];
10056 bool j3valid[1]={false};
10057 _nj3 = 1;
10058 CheckValue<IkReal> x675=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
10059 if(!x675.valid){
10060 continue;
10061 }
10062 CheckValue<IkReal> x676 = IKatan2WithCheck(IkReal((gconst10*new_r11)),IkReal((gconst11*new_r11)),IKFAST_ATAN2_MAGTHRESH);
10063 if(!x676.valid){
10064 continue;
10065 }
10066 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x675.value)))+(x676.value));
10067 sj3array[0]=IKsin(j3array[0]);
10068 cj3array[0]=IKcos(j3array[0]);
10069 if( j3array[0] > IKPI )
10070 {
10071  j3array[0]-=IK2PI;
10072 }
10073 else if( j3array[0] < -IKPI )
10074 { j3array[0]+=IK2PI;
10075 }
10076 j3valid[0] = true;
10077 for(int ij3 = 0; ij3 < 1; ++ij3)
10078 {
10079 if( !j3valid[ij3] )
10080 {
10081  continue;
10082 }
10083 _ij3[0] = ij3; _ij3[1] = -1;
10084 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10085 {
10086 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10087 {
10088  j3valid[iij3]=false; _ij3[1] = iij3; break;
10089 }
10090 }
10091 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10092 {
10093 IkReal evalcond[6];
10094 IkReal x677=IKcos(j3);
10095 IkReal x678=IKsin(j3);
10096 IkReal x679=(gconst10*x677);
10097 IkReal x680=(gconst10*x678);
10098 IkReal x681=(gconst11*x677);
10099 IkReal x682=((1.0)*x678);
10100 IkReal x683=(gconst11*x682);
10101 evalcond[0]=(((new_r11*x678))+((new_r01*x677))+gconst10);
10102 evalcond[1]=(x681+x680+new_r11);
10103 evalcond[2]=((((-1.0)*x683))+x679);
10104 evalcond[3]=((((-1.0)*x683))+x679+new_r01);
10105 evalcond[4]=((((-1.0)*x681))+(((-1.0)*x680)));
10106 evalcond[5]=(((new_r11*x677))+(((-1.0)*new_r01*x682))+gconst11);
10107 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10108 {
10109 continue;
10110 }
10111 }
10112 
10113 {
10114 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10115 vinfos[0].jointtype = 1;
10116 vinfos[0].foffset = j0;
10117 vinfos[0].indices[0] = _ij0[0];
10118 vinfos[0].indices[1] = _ij0[1];
10119 vinfos[0].maxsolutions = _nj0;
10120 vinfos[1].jointtype = 1;
10121 vinfos[1].foffset = j1;
10122 vinfos[1].indices[0] = _ij1[0];
10123 vinfos[1].indices[1] = _ij1[1];
10124 vinfos[1].maxsolutions = _nj1;
10125 vinfos[2].jointtype = 1;
10126 vinfos[2].foffset = j2;
10127 vinfos[2].indices[0] = _ij2[0];
10128 vinfos[2].indices[1] = _ij2[1];
10129 vinfos[2].maxsolutions = _nj2;
10130 vinfos[3].jointtype = 1;
10131 vinfos[3].foffset = j3;
10132 vinfos[3].indices[0] = _ij3[0];
10133 vinfos[3].indices[1] = _ij3[1];
10134 vinfos[3].maxsolutions = _nj3;
10135 vinfos[4].jointtype = 1;
10136 vinfos[4].foffset = j4;
10137 vinfos[4].indices[0] = _ij4[0];
10138 vinfos[4].indices[1] = _ij4[1];
10139 vinfos[4].maxsolutions = _nj4;
10140 vinfos[5].jointtype = 1;
10141 vinfos[5].foffset = j5;
10142 vinfos[5].indices[0] = _ij5[0];
10143 vinfos[5].indices[1] = _ij5[1];
10144 vinfos[5].maxsolutions = _nj5;
10145 std::vector<int> vfree(0);
10146 solutions.AddSolution(vinfos,vfree);
10147 }
10148 }
10149 }
10150 
10151 }
10152 
10153 }
10154 
10155 } else
10156 {
10157 {
10158 IkReal j3array[1], cj3array[1], sj3array[1];
10159 bool j3valid[1]={false};
10160 _nj3 = 1;
10161 CheckValue<IkReal> x684=IKPowWithIntegerCheck(IKsign((((gconst10*new_r01))+(((-1.0)*gconst11*new_r11)))),-1);
10162 if(!x684.valid){
10163 continue;
10164 }
10165 CheckValue<IkReal> x685 = IKatan2WithCheck(IkReal((gconst10*gconst11)),IkReal(((-1.0)*(gconst10*gconst10))),IKFAST_ATAN2_MAGTHRESH);
10166 if(!x685.valid){
10167 continue;
10168 }
10169 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x684.value)))+(x685.value));
10170 sj3array[0]=IKsin(j3array[0]);
10171 cj3array[0]=IKcos(j3array[0]);
10172 if( j3array[0] > IKPI )
10173 {
10174  j3array[0]-=IK2PI;
10175 }
10176 else if( j3array[0] < -IKPI )
10177 { j3array[0]+=IK2PI;
10178 }
10179 j3valid[0] = true;
10180 for(int ij3 = 0; ij3 < 1; ++ij3)
10181 {
10182 if( !j3valid[ij3] )
10183 {
10184  continue;
10185 }
10186 _ij3[0] = ij3; _ij3[1] = -1;
10187 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10188 {
10189 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10190 {
10191  j3valid[iij3]=false; _ij3[1] = iij3; break;
10192 }
10193 }
10194 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10195 {
10196 IkReal evalcond[6];
10197 IkReal x686=IKcos(j3);
10198 IkReal x687=IKsin(j3);
10199 IkReal x688=(gconst10*x686);
10200 IkReal x689=(gconst10*x687);
10201 IkReal x690=(gconst11*x686);
10202 IkReal x691=((1.0)*x687);
10203 IkReal x692=(gconst11*x691);
10204 evalcond[0]=(gconst10+((new_r01*x686))+((new_r11*x687)));
10205 evalcond[1]=(x689+x690+new_r11);
10206 evalcond[2]=(x688+(((-1.0)*x692)));
10207 evalcond[3]=(x688+(((-1.0)*x692))+new_r01);
10208 evalcond[4]=((((-1.0)*x689))+(((-1.0)*x690)));
10209 evalcond[5]=((((-1.0)*new_r01*x691))+gconst11+((new_r11*x686)));
10210 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10211 {
10212 continue;
10213 }
10214 }
10215 
10216 {
10217 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10218 vinfos[0].jointtype = 1;
10219 vinfos[0].foffset = j0;
10220 vinfos[0].indices[0] = _ij0[0];
10221 vinfos[0].indices[1] = _ij0[1];
10222 vinfos[0].maxsolutions = _nj0;
10223 vinfos[1].jointtype = 1;
10224 vinfos[1].foffset = j1;
10225 vinfos[1].indices[0] = _ij1[0];
10226 vinfos[1].indices[1] = _ij1[1];
10227 vinfos[1].maxsolutions = _nj1;
10228 vinfos[2].jointtype = 1;
10229 vinfos[2].foffset = j2;
10230 vinfos[2].indices[0] = _ij2[0];
10231 vinfos[2].indices[1] = _ij2[1];
10232 vinfos[2].maxsolutions = _nj2;
10233 vinfos[3].jointtype = 1;
10234 vinfos[3].foffset = j3;
10235 vinfos[3].indices[0] = _ij3[0];
10236 vinfos[3].indices[1] = _ij3[1];
10237 vinfos[3].maxsolutions = _nj3;
10238 vinfos[4].jointtype = 1;
10239 vinfos[4].foffset = j4;
10240 vinfos[4].indices[0] = _ij4[0];
10241 vinfos[4].indices[1] = _ij4[1];
10242 vinfos[4].maxsolutions = _nj4;
10243 vinfos[5].jointtype = 1;
10244 vinfos[5].foffset = j5;
10245 vinfos[5].indices[0] = _ij5[0];
10246 vinfos[5].indices[1] = _ij5[1];
10247 vinfos[5].maxsolutions = _nj5;
10248 std::vector<int> vfree(0);
10249 solutions.AddSolution(vinfos,vfree);
10250 }
10251 }
10252 }
10253 
10254 }
10255 
10256 }
10257 
10258 }
10259 } while(0);
10260 if( bgotonextstatement )
10261 {
10262 bool bgotonextstatement = true;
10263 do
10264 {
10265 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
10266 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10267 {
10268 bgotonextstatement=false;
10269 {
10270 IkReal j3array[2], cj3array[2], sj3array[2];
10271 bool j3valid[2]={false};
10272 _nj3 = 2;
10273 CheckValue<IkReal> x693=IKPowWithIntegerCheck(gconst11,-1);
10274 if(!x693.valid){
10275 continue;
10276 }
10277 cj3array[0]=(new_r00*(x693.value));
10278 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10279 {
10280  j3valid[0] = j3valid[1] = true;
10281  j3array[0] = IKacos(cj3array[0]);
10282  sj3array[0] = IKsin(j3array[0]);
10283  cj3array[1] = cj3array[0];
10284  j3array[1] = -j3array[0];
10285  sj3array[1] = -sj3array[0];
10286 }
10287 else if( isnan(cj3array[0]) )
10288 {
10289  // probably any value will work
10290  j3valid[0] = true;
10291  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10292 }
10293 for(int ij3 = 0; ij3 < 2; ++ij3)
10294 {
10295 if( !j3valid[ij3] )
10296 {
10297  continue;
10298 }
10299 _ij3[0] = ij3; _ij3[1] = -1;
10300 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10301 {
10302 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10303 {
10304  j3valid[iij3]=false; _ij3[1] = iij3; break;
10305 }
10306 }
10307 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10308 {
10309 IkReal evalcond[6];
10310 IkReal x694=IKsin(j3);
10311 IkReal x695=IKcos(j3);
10312 IkReal x696=((-1.0)*x694);
10313 evalcond[0]=(new_r11*x694);
10314 evalcond[1]=(gconst11*x696);
10315 evalcond[2]=(new_r00*x696);
10316 evalcond[3]=(((gconst11*x695))+new_r11);
10317 evalcond[4]=(gconst11+((new_r11*x695)));
10318 evalcond[5]=(((new_r00*x695))+(((-1.0)*gconst11)));
10319 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10320 {
10321 continue;
10322 }
10323 }
10324 
10325 {
10326 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10327 vinfos[0].jointtype = 1;
10328 vinfos[0].foffset = j0;
10329 vinfos[0].indices[0] = _ij0[0];
10330 vinfos[0].indices[1] = _ij0[1];
10331 vinfos[0].maxsolutions = _nj0;
10332 vinfos[1].jointtype = 1;
10333 vinfos[1].foffset = j1;
10334 vinfos[1].indices[0] = _ij1[0];
10335 vinfos[1].indices[1] = _ij1[1];
10336 vinfos[1].maxsolutions = _nj1;
10337 vinfos[2].jointtype = 1;
10338 vinfos[2].foffset = j2;
10339 vinfos[2].indices[0] = _ij2[0];
10340 vinfos[2].indices[1] = _ij2[1];
10341 vinfos[2].maxsolutions = _nj2;
10342 vinfos[3].jointtype = 1;
10343 vinfos[3].foffset = j3;
10344 vinfos[3].indices[0] = _ij3[0];
10345 vinfos[3].indices[1] = _ij3[1];
10346 vinfos[3].maxsolutions = _nj3;
10347 vinfos[4].jointtype = 1;
10348 vinfos[4].foffset = j4;
10349 vinfos[4].indices[0] = _ij4[0];
10350 vinfos[4].indices[1] = _ij4[1];
10351 vinfos[4].maxsolutions = _nj4;
10352 vinfos[5].jointtype = 1;
10353 vinfos[5].foffset = j5;
10354 vinfos[5].indices[0] = _ij5[0];
10355 vinfos[5].indices[1] = _ij5[1];
10356 vinfos[5].maxsolutions = _nj5;
10357 std::vector<int> vfree(0);
10358 solutions.AddSolution(vinfos,vfree);
10359 }
10360 }
10361 }
10362 
10363 }
10364 } while(0);
10365 if( bgotonextstatement )
10366 {
10367 bool bgotonextstatement = true;
10368 do
10369 {
10370 evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01)));
10371 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10372 {
10373 bgotonextstatement=false;
10374 {
10375 IkReal j3eval[1];
10376 CheckValue<IkReal> x698 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10377 if(!x698.valid){
10378 continue;
10379 }
10380 IkReal x697=((1.0)*(x698.value));
10381 sj4=0;
10382 cj4=-1.0;
10383 j4=3.14159265358979;
10384 sj5=gconst10;
10385 cj5=gconst11;
10386 j5=((3.14159265)+(((-1.0)*x697)));
10387 new_r00=0;
10388 new_r01=0;
10389 new_r12=0;
10390 new_r22=0;
10391 IkReal gconst9=((3.14159265358979)+(((-1.0)*x697)));
10392 IkReal gconst10=0;
10393 IkReal x699 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
10394 if(IKabs(x699)==0){
10395 continue;
10396 }
10397 IkReal gconst11=((-1.0)*new_r11*(pow(x699,-0.5)));
10398 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
10399 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10400 {
10401 {
10402 IkReal j3eval[1];
10403 CheckValue<IkReal> x701 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10404 if(!x701.valid){
10405 continue;
10406 }
10407 IkReal x700=((1.0)*(x701.value));
10408 sj4=0;
10409 cj4=-1.0;
10410 j4=3.14159265358979;
10411 sj5=gconst10;
10412 cj5=gconst11;
10413 j5=((3.14159265)+(((-1.0)*x700)));
10414 new_r00=0;
10415 new_r01=0;
10416 new_r12=0;
10417 new_r22=0;
10418 IkReal gconst9=((3.14159265358979)+(((-1.0)*x700)));
10419 IkReal gconst10=0;
10420 IkReal x702 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
10421 if(IKabs(x702)==0){
10422 continue;
10423 }
10424 IkReal gconst11=((-1.0)*new_r11*(pow(x702,-0.5)));
10425 j3eval[0]=new_r11;
10426 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10427 {
10428 {
10429 IkReal j3eval[1];
10430 CheckValue<IkReal> x704 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10431 if(!x704.valid){
10432 continue;
10433 }
10434 IkReal x703=((1.0)*(x704.value));
10435 sj4=0;
10436 cj4=-1.0;
10437 j4=3.14159265358979;
10438 sj5=gconst10;
10439 cj5=gconst11;
10440 j5=((3.14159265)+(((-1.0)*x703)));
10441 new_r00=0;
10442 new_r01=0;
10443 new_r12=0;
10444 new_r22=0;
10445 IkReal gconst9=((3.14159265358979)+(((-1.0)*x703)));
10446 IkReal gconst10=0;
10447 IkReal x705 = ((1.0)+(((-1.0)*(new_r10*new_r10))));
10448 if(IKabs(x705)==0){
10449 continue;
10450 }
10451 IkReal gconst11=((-1.0)*new_r11*(pow(x705,-0.5)));
10452 j3eval[0]=new_r10;
10453 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10454 {
10455 continue; // 3 cases reached
10456 
10457 } else
10458 {
10459 {
10460 IkReal j3array[1], cj3array[1], sj3array[1];
10461 bool j3valid[1]={false};
10462 _nj3 = 1;
10463 CheckValue<IkReal> x706=IKPowWithIntegerCheck(new_r10,-1);
10464 if(!x706.valid){
10465 continue;
10466 }
10467 CheckValue<IkReal> x707=IKPowWithIntegerCheck(gconst11,-1);
10468 if(!x707.valid){
10469 continue;
10470 }
10471 if( IKabs((gconst11*(x706.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x707.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst11*(x706.value)))+IKsqr(((-1.0)*new_r11*(x707.value)))-1) <= IKFAST_SINCOS_THRESH )
10472  continue;
10473 j3array[0]=IKatan2((gconst11*(x706.value)), ((-1.0)*new_r11*(x707.value)));
10474 sj3array[0]=IKsin(j3array[0]);
10475 cj3array[0]=IKcos(j3array[0]);
10476 if( j3array[0] > IKPI )
10477 {
10478  j3array[0]-=IK2PI;
10479 }
10480 else if( j3array[0] < -IKPI )
10481 { j3array[0]+=IK2PI;
10482 }
10483 j3valid[0] = true;
10484 for(int ij3 = 0; ij3 < 1; ++ij3)
10485 {
10486 if( !j3valid[ij3] )
10487 {
10488  continue;
10489 }
10490 _ij3[0] = ij3; _ij3[1] = -1;
10491 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10492 {
10493 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10494 {
10495  j3valid[iij3]=false; _ij3[1] = iij3; break;
10496 }
10497 }
10498 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10499 {
10500 IkReal evalcond[8];
10501 IkReal x708=IKsin(j3);
10502 IkReal x709=IKcos(j3);
10503 IkReal x710=(gconst11*x709);
10504 IkReal x711=(gconst11*x708);
10505 evalcond[0]=(new_r11*x708);
10506 evalcond[1]=(new_r10*x709);
10507 evalcond[2]=((-1.0)*x711);
10508 evalcond[3]=((-1.0)*x710);
10509 evalcond[4]=(x710+new_r11);
10510 evalcond[5]=(gconst11+((new_r11*x709)));
10511 evalcond[6]=((((-1.0)*x711))+new_r10);
10512 evalcond[7]=((((-1.0)*gconst11))+((new_r10*x708)));
10513 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10514 {
10515 continue;
10516 }
10517 }
10518 
10519 {
10520 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10521 vinfos[0].jointtype = 1;
10522 vinfos[0].foffset = j0;
10523 vinfos[0].indices[0] = _ij0[0];
10524 vinfos[0].indices[1] = _ij0[1];
10525 vinfos[0].maxsolutions = _nj0;
10526 vinfos[1].jointtype = 1;
10527 vinfos[1].foffset = j1;
10528 vinfos[1].indices[0] = _ij1[0];
10529 vinfos[1].indices[1] = _ij1[1];
10530 vinfos[1].maxsolutions = _nj1;
10531 vinfos[2].jointtype = 1;
10532 vinfos[2].foffset = j2;
10533 vinfos[2].indices[0] = _ij2[0];
10534 vinfos[2].indices[1] = _ij2[1];
10535 vinfos[2].maxsolutions = _nj2;
10536 vinfos[3].jointtype = 1;
10537 vinfos[3].foffset = j3;
10538 vinfos[3].indices[0] = _ij3[0];
10539 vinfos[3].indices[1] = _ij3[1];
10540 vinfos[3].maxsolutions = _nj3;
10541 vinfos[4].jointtype = 1;
10542 vinfos[4].foffset = j4;
10543 vinfos[4].indices[0] = _ij4[0];
10544 vinfos[4].indices[1] = _ij4[1];
10545 vinfos[4].maxsolutions = _nj4;
10546 vinfos[5].jointtype = 1;
10547 vinfos[5].foffset = j5;
10548 vinfos[5].indices[0] = _ij5[0];
10549 vinfos[5].indices[1] = _ij5[1];
10550 vinfos[5].maxsolutions = _nj5;
10551 std::vector<int> vfree(0);
10552 solutions.AddSolution(vinfos,vfree);
10553 }
10554 }
10555 }
10556 
10557 }
10558 
10559 }
10560 
10561 } else
10562 {
10563 {
10564 IkReal j3array[1], cj3array[1], sj3array[1];
10565 bool j3valid[1]={false};
10566 _nj3 = 1;
10567 CheckValue<IkReal> x712=IKPowWithIntegerCheck(gconst11,-1);
10568 if(!x712.valid){
10569 continue;
10570 }
10571 CheckValue<IkReal> x713=IKPowWithIntegerCheck(new_r11,-1);
10572 if(!x713.valid){
10573 continue;
10574 }
10575 if( IKabs((new_r10*(x712.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst11*(x713.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x712.value)))+IKsqr(((-1.0)*gconst11*(x713.value)))-1) <= IKFAST_SINCOS_THRESH )
10576  continue;
10577 j3array[0]=IKatan2((new_r10*(x712.value)), ((-1.0)*gconst11*(x713.value)));
10578 sj3array[0]=IKsin(j3array[0]);
10579 cj3array[0]=IKcos(j3array[0]);
10580 if( j3array[0] > IKPI )
10581 {
10582  j3array[0]-=IK2PI;
10583 }
10584 else if( j3array[0] < -IKPI )
10585 { j3array[0]+=IK2PI;
10586 }
10587 j3valid[0] = true;
10588 for(int ij3 = 0; ij3 < 1; ++ij3)
10589 {
10590 if( !j3valid[ij3] )
10591 {
10592  continue;
10593 }
10594 _ij3[0] = ij3; _ij3[1] = -1;
10595 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10596 {
10597 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10598 {
10599  j3valid[iij3]=false; _ij3[1] = iij3; break;
10600 }
10601 }
10602 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10603 {
10604 IkReal evalcond[8];
10605 IkReal x714=IKsin(j3);
10606 IkReal x715=IKcos(j3);
10607 IkReal x716=(gconst11*x715);
10608 IkReal x717=(gconst11*x714);
10609 evalcond[0]=(new_r11*x714);
10610 evalcond[1]=(new_r10*x715);
10611 evalcond[2]=((-1.0)*x717);
10612 evalcond[3]=((-1.0)*x716);
10613 evalcond[4]=(x716+new_r11);
10614 evalcond[5]=(gconst11+((new_r11*x715)));
10615 evalcond[6]=((((-1.0)*x717))+new_r10);
10616 evalcond[7]=((((-1.0)*gconst11))+((new_r10*x714)));
10617 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10618 {
10619 continue;
10620 }
10621 }
10622 
10623 {
10624 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10625 vinfos[0].jointtype = 1;
10626 vinfos[0].foffset = j0;
10627 vinfos[0].indices[0] = _ij0[0];
10628 vinfos[0].indices[1] = _ij0[1];
10629 vinfos[0].maxsolutions = _nj0;
10630 vinfos[1].jointtype = 1;
10631 vinfos[1].foffset = j1;
10632 vinfos[1].indices[0] = _ij1[0];
10633 vinfos[1].indices[1] = _ij1[1];
10634 vinfos[1].maxsolutions = _nj1;
10635 vinfos[2].jointtype = 1;
10636 vinfos[2].foffset = j2;
10637 vinfos[2].indices[0] = _ij2[0];
10638 vinfos[2].indices[1] = _ij2[1];
10639 vinfos[2].maxsolutions = _nj2;
10640 vinfos[3].jointtype = 1;
10641 vinfos[3].foffset = j3;
10642 vinfos[3].indices[0] = _ij3[0];
10643 vinfos[3].indices[1] = _ij3[1];
10644 vinfos[3].maxsolutions = _nj3;
10645 vinfos[4].jointtype = 1;
10646 vinfos[4].foffset = j4;
10647 vinfos[4].indices[0] = _ij4[0];
10648 vinfos[4].indices[1] = _ij4[1];
10649 vinfos[4].maxsolutions = _nj4;
10650 vinfos[5].jointtype = 1;
10651 vinfos[5].foffset = j5;
10652 vinfos[5].indices[0] = _ij5[0];
10653 vinfos[5].indices[1] = _ij5[1];
10654 vinfos[5].maxsolutions = _nj5;
10655 std::vector<int> vfree(0);
10656 solutions.AddSolution(vinfos,vfree);
10657 }
10658 }
10659 }
10660 
10661 }
10662 
10663 }
10664 
10665 } else
10666 {
10667 {
10668 IkReal j3array[1], cj3array[1], sj3array[1];
10669 bool j3valid[1]={false};
10670 _nj3 = 1;
10671 CheckValue<IkReal> x718=IKPowWithIntegerCheck(IKsign(gconst11),-1);
10672 if(!x718.valid){
10673 continue;
10674 }
10675 CheckValue<IkReal> x719 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
10676 if(!x719.valid){
10677 continue;
10678 }
10679 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x718.value)))+(x719.value));
10680 sj3array[0]=IKsin(j3array[0]);
10681 cj3array[0]=IKcos(j3array[0]);
10682 if( j3array[0] > IKPI )
10683 {
10684  j3array[0]-=IK2PI;
10685 }
10686 else if( j3array[0] < -IKPI )
10687 { j3array[0]+=IK2PI;
10688 }
10689 j3valid[0] = true;
10690 for(int ij3 = 0; ij3 < 1; ++ij3)
10691 {
10692 if( !j3valid[ij3] )
10693 {
10694  continue;
10695 }
10696 _ij3[0] = ij3; _ij3[1] = -1;
10697 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10698 {
10699 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10700 {
10701  j3valid[iij3]=false; _ij3[1] = iij3; break;
10702 }
10703 }
10704 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10705 {
10706 IkReal evalcond[8];
10707 IkReal x720=IKsin(j3);
10708 IkReal x721=IKcos(j3);
10709 IkReal x722=(gconst11*x721);
10710 IkReal x723=(gconst11*x720);
10711 evalcond[0]=(new_r11*x720);
10712 evalcond[1]=(new_r10*x721);
10713 evalcond[2]=((-1.0)*x723);
10714 evalcond[3]=((-1.0)*x722);
10715 evalcond[4]=(x722+new_r11);
10716 evalcond[5]=(gconst11+((new_r11*x721)));
10717 evalcond[6]=((((-1.0)*x723))+new_r10);
10718 evalcond[7]=((((-1.0)*gconst11))+((new_r10*x720)));
10719 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10720 {
10721 continue;
10722 }
10723 }
10724 
10725 {
10726 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10727 vinfos[0].jointtype = 1;
10728 vinfos[0].foffset = j0;
10729 vinfos[0].indices[0] = _ij0[0];
10730 vinfos[0].indices[1] = _ij0[1];
10731 vinfos[0].maxsolutions = _nj0;
10732 vinfos[1].jointtype = 1;
10733 vinfos[1].foffset = j1;
10734 vinfos[1].indices[0] = _ij1[0];
10735 vinfos[1].indices[1] = _ij1[1];
10736 vinfos[1].maxsolutions = _nj1;
10737 vinfos[2].jointtype = 1;
10738 vinfos[2].foffset = j2;
10739 vinfos[2].indices[0] = _ij2[0];
10740 vinfos[2].indices[1] = _ij2[1];
10741 vinfos[2].maxsolutions = _nj2;
10742 vinfos[3].jointtype = 1;
10743 vinfos[3].foffset = j3;
10744 vinfos[3].indices[0] = _ij3[0];
10745 vinfos[3].indices[1] = _ij3[1];
10746 vinfos[3].maxsolutions = _nj3;
10747 vinfos[4].jointtype = 1;
10748 vinfos[4].foffset = j4;
10749 vinfos[4].indices[0] = _ij4[0];
10750 vinfos[4].indices[1] = _ij4[1];
10751 vinfos[4].maxsolutions = _nj4;
10752 vinfos[5].jointtype = 1;
10753 vinfos[5].foffset = j5;
10754 vinfos[5].indices[0] = _ij5[0];
10755 vinfos[5].indices[1] = _ij5[1];
10756 vinfos[5].maxsolutions = _nj5;
10757 std::vector<int> vfree(0);
10758 solutions.AddSolution(vinfos,vfree);
10759 }
10760 }
10761 }
10762 
10763 }
10764 
10765 }
10766 
10767 }
10768 } while(0);
10769 if( bgotonextstatement )
10770 {
10771 bool bgotonextstatement = true;
10772 do
10773 {
10774 evalcond[0]=IKabs(new_r01);
10775 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10776 {
10777 bgotonextstatement=false;
10778 {
10779 IkReal j3eval[1];
10780 CheckValue<IkReal> x725 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10781 if(!x725.valid){
10782 continue;
10783 }
10784 IkReal x724=((1.0)*(x725.value));
10785 sj4=0;
10786 cj4=-1.0;
10787 j4=3.14159265358979;
10788 sj5=gconst10;
10789 cj5=gconst11;
10790 j5=((3.14159265)+(((-1.0)*x724)));
10791 new_r01=0;
10792 IkReal gconst9=((3.14159265358979)+(((-1.0)*x724)));
10793 IkReal gconst10=0;
10794 IkReal x726 = new_r11*new_r11;
10795 if(IKabs(x726)==0){
10796 continue;
10797 }
10798 IkReal gconst11=((-1.0)*new_r11*(pow(x726,-0.5)));
10799 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
10800 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10801 {
10802 {
10803 IkReal j3eval[1];
10804 CheckValue<IkReal> x728 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10805 if(!x728.valid){
10806 continue;
10807 }
10808 IkReal x727=((1.0)*(x728.value));
10809 sj4=0;
10810 cj4=-1.0;
10811 j4=3.14159265358979;
10812 sj5=gconst10;
10813 cj5=gconst11;
10814 j5=((3.14159265)+(((-1.0)*x727)));
10815 new_r01=0;
10816 IkReal gconst9=((3.14159265358979)+(((-1.0)*x727)));
10817 IkReal gconst10=0;
10818 IkReal x729 = new_r11*new_r11;
10819 if(IKabs(x729)==0){
10820 continue;
10821 }
10822 IkReal gconst11=((-1.0)*new_r11*(pow(x729,-0.5)));
10823 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
10824 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10825 {
10826 {
10827 IkReal j3eval[1];
10828 CheckValue<IkReal> x731 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
10829 if(!x731.valid){
10830 continue;
10831 }
10832 IkReal x730=((1.0)*(x731.value));
10833 sj4=0;
10834 cj4=-1.0;
10835 j4=3.14159265358979;
10836 sj5=gconst10;
10837 cj5=gconst11;
10838 j5=((3.14159265)+(((-1.0)*x730)));
10839 new_r01=0;
10840 IkReal gconst9=((3.14159265358979)+(((-1.0)*x730)));
10841 IkReal gconst10=0;
10842 IkReal x732 = new_r11*new_r11;
10843 if(IKabs(x732)==0){
10844 continue;
10845 }
10846 IkReal gconst11=((-1.0)*new_r11*(pow(x732,-0.5)));
10847 j3eval[0]=new_r11;
10848 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10849 {
10850 continue; // 3 cases reached
10851 
10852 } else
10853 {
10854 {
10855 IkReal j3array[1], cj3array[1], sj3array[1];
10856 bool j3valid[1]={false};
10857 _nj3 = 1;
10858 CheckValue<IkReal> x733=IKPowWithIntegerCheck(gconst11,-1);
10859 if(!x733.valid){
10860 continue;
10861 }
10862 CheckValue<IkReal> x734=IKPowWithIntegerCheck(new_r11,-1);
10863 if(!x734.valid){
10864 continue;
10865 }
10866 if( IKabs((new_r10*(x733.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst11*(x734.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x733.value)))+IKsqr(((-1.0)*gconst11*(x734.value)))-1) <= IKFAST_SINCOS_THRESH )
10867  continue;
10868 j3array[0]=IKatan2((new_r10*(x733.value)), ((-1.0)*gconst11*(x734.value)));
10869 sj3array[0]=IKsin(j3array[0]);
10870 cj3array[0]=IKcos(j3array[0]);
10871 if( j3array[0] > IKPI )
10872 {
10873  j3array[0]-=IK2PI;
10874 }
10875 else if( j3array[0] < -IKPI )
10876 { j3array[0]+=IK2PI;
10877 }
10878 j3valid[0] = true;
10879 for(int ij3 = 0; ij3 < 1; ++ij3)
10880 {
10881 if( !j3valid[ij3] )
10882 {
10883  continue;
10884 }
10885 _ij3[0] = ij3; _ij3[1] = -1;
10886 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10887 {
10888 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10889 {
10890  j3valid[iij3]=false; _ij3[1] = iij3; break;
10891 }
10892 }
10893 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10894 {
10895 IkReal evalcond[8];
10896 IkReal x735=IKsin(j3);
10897 IkReal x736=IKcos(j3);
10898 IkReal x737=(gconst11*x736);
10899 IkReal x738=((1.0)*x735);
10900 evalcond[0]=(new_r11*x735);
10901 evalcond[1]=((-1.0)*gconst11*x735);
10902 evalcond[2]=(x737+new_r11);
10903 evalcond[3]=(gconst11+((new_r11*x736)));
10904 evalcond[4]=((((-1.0)*x737))+new_r00);
10905 evalcond[5]=((((-1.0)*gconst11*x738))+new_r10);
10906 evalcond[6]=((((-1.0)*new_r00*x738))+((new_r10*x736)));
10907 evalcond[7]=(((new_r00*x736))+(((-1.0)*gconst11))+((new_r10*x735)));
10908 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10909 {
10910 continue;
10911 }
10912 }
10913 
10914 {
10915 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10916 vinfos[0].jointtype = 1;
10917 vinfos[0].foffset = j0;
10918 vinfos[0].indices[0] = _ij0[0];
10919 vinfos[0].indices[1] = _ij0[1];
10920 vinfos[0].maxsolutions = _nj0;
10921 vinfos[1].jointtype = 1;
10922 vinfos[1].foffset = j1;
10923 vinfos[1].indices[0] = _ij1[0];
10924 vinfos[1].indices[1] = _ij1[1];
10925 vinfos[1].maxsolutions = _nj1;
10926 vinfos[2].jointtype = 1;
10927 vinfos[2].foffset = j2;
10928 vinfos[2].indices[0] = _ij2[0];
10929 vinfos[2].indices[1] = _ij2[1];
10930 vinfos[2].maxsolutions = _nj2;
10931 vinfos[3].jointtype = 1;
10932 vinfos[3].foffset = j3;
10933 vinfos[3].indices[0] = _ij3[0];
10934 vinfos[3].indices[1] = _ij3[1];
10935 vinfos[3].maxsolutions = _nj3;
10936 vinfos[4].jointtype = 1;
10937 vinfos[4].foffset = j4;
10938 vinfos[4].indices[0] = _ij4[0];
10939 vinfos[4].indices[1] = _ij4[1];
10940 vinfos[4].maxsolutions = _nj4;
10941 vinfos[5].jointtype = 1;
10942 vinfos[5].foffset = j5;
10943 vinfos[5].indices[0] = _ij5[0];
10944 vinfos[5].indices[1] = _ij5[1];
10945 vinfos[5].maxsolutions = _nj5;
10946 std::vector<int> vfree(0);
10947 solutions.AddSolution(vinfos,vfree);
10948 }
10949 }
10950 }
10951 
10952 }
10953 
10954 }
10955 
10956 } else
10957 {
10958 {
10959 IkReal j3array[1], cj3array[1], sj3array[1];
10960 bool j3valid[1]={false};
10961 _nj3 = 1;
10962 CheckValue<IkReal> x739=IKPowWithIntegerCheck(IKsign(gconst11),-1);
10963 if(!x739.valid){
10964 continue;
10965 }
10966 CheckValue<IkReal> x740 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH);
10967 if(!x740.valid){
10968 continue;
10969 }
10970 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x739.value)))+(x740.value));
10971 sj3array[0]=IKsin(j3array[0]);
10972 cj3array[0]=IKcos(j3array[0]);
10973 if( j3array[0] > IKPI )
10974 {
10975  j3array[0]-=IK2PI;
10976 }
10977 else if( j3array[0] < -IKPI )
10978 { j3array[0]+=IK2PI;
10979 }
10980 j3valid[0] = true;
10981 for(int ij3 = 0; ij3 < 1; ++ij3)
10982 {
10983 if( !j3valid[ij3] )
10984 {
10985  continue;
10986 }
10987 _ij3[0] = ij3; _ij3[1] = -1;
10988 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10989 {
10990 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10991 {
10992  j3valid[iij3]=false; _ij3[1] = iij3; break;
10993 }
10994 }
10995 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10996 {
10997 IkReal evalcond[8];
10998 IkReal x741=IKsin(j3);
10999 IkReal x742=IKcos(j3);
11000 IkReal x743=(gconst11*x742);
11001 IkReal x744=((1.0)*x741);
11002 evalcond[0]=(new_r11*x741);
11003 evalcond[1]=((-1.0)*gconst11*x741);
11004 evalcond[2]=(x743+new_r11);
11005 evalcond[3]=(gconst11+((new_r11*x742)));
11006 evalcond[4]=((((-1.0)*x743))+new_r00);
11007 evalcond[5]=((((-1.0)*gconst11*x744))+new_r10);
11008 evalcond[6]=((((-1.0)*new_r00*x744))+((new_r10*x742)));
11009 evalcond[7]=(((new_r10*x741))+((new_r00*x742))+(((-1.0)*gconst11)));
11010 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11011 {
11012 continue;
11013 }
11014 }
11015 
11016 {
11017 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11018 vinfos[0].jointtype = 1;
11019 vinfos[0].foffset = j0;
11020 vinfos[0].indices[0] = _ij0[0];
11021 vinfos[0].indices[1] = _ij0[1];
11022 vinfos[0].maxsolutions = _nj0;
11023 vinfos[1].jointtype = 1;
11024 vinfos[1].foffset = j1;
11025 vinfos[1].indices[0] = _ij1[0];
11026 vinfos[1].indices[1] = _ij1[1];
11027 vinfos[1].maxsolutions = _nj1;
11028 vinfos[2].jointtype = 1;
11029 vinfos[2].foffset = j2;
11030 vinfos[2].indices[0] = _ij2[0];
11031 vinfos[2].indices[1] = _ij2[1];
11032 vinfos[2].maxsolutions = _nj2;
11033 vinfos[3].jointtype = 1;
11034 vinfos[3].foffset = j3;
11035 vinfos[3].indices[0] = _ij3[0];
11036 vinfos[3].indices[1] = _ij3[1];
11037 vinfos[3].maxsolutions = _nj3;
11038 vinfos[4].jointtype = 1;
11039 vinfos[4].foffset = j4;
11040 vinfos[4].indices[0] = _ij4[0];
11041 vinfos[4].indices[1] = _ij4[1];
11042 vinfos[4].maxsolutions = _nj4;
11043 vinfos[5].jointtype = 1;
11044 vinfos[5].foffset = j5;
11045 vinfos[5].indices[0] = _ij5[0];
11046 vinfos[5].indices[1] = _ij5[1];
11047 vinfos[5].maxsolutions = _nj5;
11048 std::vector<int> vfree(0);
11049 solutions.AddSolution(vinfos,vfree);
11050 }
11051 }
11052 }
11053 
11054 }
11055 
11056 }
11057 
11058 } else
11059 {
11060 {
11061 IkReal j3array[1], cj3array[1], sj3array[1];
11062 bool j3valid[1]={false};
11063 _nj3 = 1;
11064 CheckValue<IkReal> x745=IKPowWithIntegerCheck(IKsign(gconst11),-1);
11065 if(!x745.valid){
11066 continue;
11067 }
11068 CheckValue<IkReal> x746 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
11069 if(!x746.valid){
11070 continue;
11071 }
11072 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x745.value)))+(x746.value));
11073 sj3array[0]=IKsin(j3array[0]);
11074 cj3array[0]=IKcos(j3array[0]);
11075 if( j3array[0] > IKPI )
11076 {
11077  j3array[0]-=IK2PI;
11078 }
11079 else if( j3array[0] < -IKPI )
11080 { j3array[0]+=IK2PI;
11081 }
11082 j3valid[0] = true;
11083 for(int ij3 = 0; ij3 < 1; ++ij3)
11084 {
11085 if( !j3valid[ij3] )
11086 {
11087  continue;
11088 }
11089 _ij3[0] = ij3; _ij3[1] = -1;
11090 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11091 {
11092 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11093 {
11094  j3valid[iij3]=false; _ij3[1] = iij3; break;
11095 }
11096 }
11097 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11098 {
11099 IkReal evalcond[8];
11100 IkReal x747=IKsin(j3);
11101 IkReal x748=IKcos(j3);
11102 IkReal x749=(gconst11*x748);
11103 IkReal x750=((1.0)*x747);
11104 evalcond[0]=(new_r11*x747);
11105 evalcond[1]=((-1.0)*gconst11*x747);
11106 evalcond[2]=(x749+new_r11);
11107 evalcond[3]=(gconst11+((new_r11*x748)));
11108 evalcond[4]=((((-1.0)*x749))+new_r00);
11109 evalcond[5]=((((-1.0)*gconst11*x750))+new_r10);
11110 evalcond[6]=((((-1.0)*new_r00*x750))+((new_r10*x748)));
11111 evalcond[7]=(((new_r10*x747))+((new_r00*x748))+(((-1.0)*gconst11)));
11112 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11113 {
11114 continue;
11115 }
11116 }
11117 
11118 {
11119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11120 vinfos[0].jointtype = 1;
11121 vinfos[0].foffset = j0;
11122 vinfos[0].indices[0] = _ij0[0];
11123 vinfos[0].indices[1] = _ij0[1];
11124 vinfos[0].maxsolutions = _nj0;
11125 vinfos[1].jointtype = 1;
11126 vinfos[1].foffset = j1;
11127 vinfos[1].indices[0] = _ij1[0];
11128 vinfos[1].indices[1] = _ij1[1];
11129 vinfos[1].maxsolutions = _nj1;
11130 vinfos[2].jointtype = 1;
11131 vinfos[2].foffset = j2;
11132 vinfos[2].indices[0] = _ij2[0];
11133 vinfos[2].indices[1] = _ij2[1];
11134 vinfos[2].maxsolutions = _nj2;
11135 vinfos[3].jointtype = 1;
11136 vinfos[3].foffset = j3;
11137 vinfos[3].indices[0] = _ij3[0];
11138 vinfos[3].indices[1] = _ij3[1];
11139 vinfos[3].maxsolutions = _nj3;
11140 vinfos[4].jointtype = 1;
11141 vinfos[4].foffset = j4;
11142 vinfos[4].indices[0] = _ij4[0];
11143 vinfos[4].indices[1] = _ij4[1];
11144 vinfos[4].maxsolutions = _nj4;
11145 vinfos[5].jointtype = 1;
11146 vinfos[5].foffset = j5;
11147 vinfos[5].indices[0] = _ij5[0];
11148 vinfos[5].indices[1] = _ij5[1];
11149 vinfos[5].maxsolutions = _nj5;
11150 std::vector<int> vfree(0);
11151 solutions.AddSolution(vinfos,vfree);
11152 }
11153 }
11154 }
11155 
11156 }
11157 
11158 }
11159 
11160 }
11161 } while(0);
11162 if( bgotonextstatement )
11163 {
11164 bool bgotonextstatement = true;
11165 do
11166 {
11167 if( 1 )
11168 {
11169 bgotonextstatement=false;
11170 continue; // branch miss [j3]
11171 
11172 }
11173 } while(0);
11174 if( bgotonextstatement )
11175 {
11176 }
11177 }
11178 }
11179 }
11180 }
11181 }
11182 }
11183 }
11184 
11185 } else
11186 {
11187 {
11188 IkReal j3array[1], cj3array[1], sj3array[1];
11189 bool j3valid[1]={false};
11190 _nj3 = 1;
11191 IkReal x751=((1.0)*new_r11);
11192 CheckValue<IkReal> x752 = IKatan2WithCheck(IkReal((((gconst11*new_r01))+(((-1.0)*gconst10*x751)))),IkReal(((((-1.0)*gconst11*x751))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
11193 if(!x752.valid){
11194 continue;
11195 }
11196 CheckValue<IkReal> x753=IKPowWithIntegerCheck(IKsign(((gconst10*gconst10)+(gconst11*gconst11))),-1);
11197 if(!x753.valid){
11198 continue;
11199 }
11200 j3array[0]=((-1.5707963267949)+(x752.value)+(((1.5707963267949)*(x753.value))));
11201 sj3array[0]=IKsin(j3array[0]);
11202 cj3array[0]=IKcos(j3array[0]);
11203 if( j3array[0] > IKPI )
11204 {
11205  j3array[0]-=IK2PI;
11206 }
11207 else if( j3array[0] < -IKPI )
11208 { j3array[0]+=IK2PI;
11209 }
11210 j3valid[0] = true;
11211 for(int ij3 = 0; ij3 < 1; ++ij3)
11212 {
11213 if( !j3valid[ij3] )
11214 {
11215  continue;
11216 }
11217 _ij3[0] = ij3; _ij3[1] = -1;
11218 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11219 {
11220 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11221 {
11222  j3valid[iij3]=false; _ij3[1] = iij3; break;
11223 }
11224 }
11225 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11226 {
11227 IkReal evalcond[8];
11228 IkReal x754=IKcos(j3);
11229 IkReal x755=IKsin(j3);
11230 IkReal x756=(gconst10*x754);
11231 IkReal x757=(gconst11*x754);
11232 IkReal x758=(gconst10*x755);
11233 IkReal x759=((1.0)*x755);
11234 IkReal x760=(gconst11*x759);
11235 evalcond[0]=(gconst10+((new_r11*x755))+((new_r01*x754)));
11236 evalcond[1]=(x757+x758+new_r11);
11237 evalcond[2]=((((-1.0)*x760))+x756+new_r01);
11238 evalcond[3]=((((-1.0)*new_r00*x759))+gconst10+((new_r10*x754)));
11239 evalcond[4]=((((-1.0)*new_r01*x759))+gconst11+((new_r11*x754)));
11240 evalcond[5]=((((-1.0)*x760))+x756+new_r10);
11241 evalcond[6]=(((new_r00*x754))+((new_r10*x755))+(((-1.0)*gconst11)));
11242 evalcond[7]=((((-1.0)*x758))+(((-1.0)*x757))+new_r00);
11243 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11244 {
11245 continue;
11246 }
11247 }
11248 
11249 {
11250 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11251 vinfos[0].jointtype = 1;
11252 vinfos[0].foffset = j0;
11253 vinfos[0].indices[0] = _ij0[0];
11254 vinfos[0].indices[1] = _ij0[1];
11255 vinfos[0].maxsolutions = _nj0;
11256 vinfos[1].jointtype = 1;
11257 vinfos[1].foffset = j1;
11258 vinfos[1].indices[0] = _ij1[0];
11259 vinfos[1].indices[1] = _ij1[1];
11260 vinfos[1].maxsolutions = _nj1;
11261 vinfos[2].jointtype = 1;
11262 vinfos[2].foffset = j2;
11263 vinfos[2].indices[0] = _ij2[0];
11264 vinfos[2].indices[1] = _ij2[1];
11265 vinfos[2].maxsolutions = _nj2;
11266 vinfos[3].jointtype = 1;
11267 vinfos[3].foffset = j3;
11268 vinfos[3].indices[0] = _ij3[0];
11269 vinfos[3].indices[1] = _ij3[1];
11270 vinfos[3].maxsolutions = _nj3;
11271 vinfos[4].jointtype = 1;
11272 vinfos[4].foffset = j4;
11273 vinfos[4].indices[0] = _ij4[0];
11274 vinfos[4].indices[1] = _ij4[1];
11275 vinfos[4].maxsolutions = _nj4;
11276 vinfos[5].jointtype = 1;
11277 vinfos[5].foffset = j5;
11278 vinfos[5].indices[0] = _ij5[0];
11279 vinfos[5].indices[1] = _ij5[1];
11280 vinfos[5].maxsolutions = _nj5;
11281 std::vector<int> vfree(0);
11282 solutions.AddSolution(vinfos,vfree);
11283 }
11284 }
11285 }
11286 
11287 }
11288 
11289 }
11290 
11291 } else
11292 {
11293 {
11294 IkReal j3array[1], cj3array[1], sj3array[1];
11295 bool j3valid[1]={false};
11296 _nj3 = 1;
11297 IkReal x761=((1.0)*new_r11);
11298 CheckValue<IkReal> x762=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
11299 if(!x762.valid){
11300 continue;
11301 }
11302 CheckValue<IkReal> x763 = IKatan2WithCheck(IkReal((((gconst11*new_r01))+(((-1.0)*gconst10*x761)))),IkReal(((((-1.0)*gconst11*x761))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
11303 if(!x763.valid){
11304 continue;
11305 }
11306 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x762.value)))+(x763.value));
11307 sj3array[0]=IKsin(j3array[0]);
11308 cj3array[0]=IKcos(j3array[0]);
11309 if( j3array[0] > IKPI )
11310 {
11311  j3array[0]-=IK2PI;
11312 }
11313 else if( j3array[0] < -IKPI )
11314 { j3array[0]+=IK2PI;
11315 }
11316 j3valid[0] = true;
11317 for(int ij3 = 0; ij3 < 1; ++ij3)
11318 {
11319 if( !j3valid[ij3] )
11320 {
11321  continue;
11322 }
11323 _ij3[0] = ij3; _ij3[1] = -1;
11324 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11325 {
11326 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11327 {
11328  j3valid[iij3]=false; _ij3[1] = iij3; break;
11329 }
11330 }
11331 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11332 {
11333 IkReal evalcond[8];
11334 IkReal x764=IKcos(j3);
11335 IkReal x765=IKsin(j3);
11336 IkReal x766=(gconst10*x764);
11337 IkReal x767=(gconst11*x764);
11338 IkReal x768=(gconst10*x765);
11339 IkReal x769=((1.0)*x765);
11340 IkReal x770=(gconst11*x769);
11341 evalcond[0]=(((new_r11*x765))+gconst10+((new_r01*x764)));
11342 evalcond[1]=(x768+x767+new_r11);
11343 evalcond[2]=(x766+(((-1.0)*x770))+new_r01);
11344 evalcond[3]=(((new_r10*x764))+(((-1.0)*new_r00*x769))+gconst10);
11345 evalcond[4]=(((new_r11*x764))+gconst11+(((-1.0)*new_r01*x769)));
11346 evalcond[5]=(x766+(((-1.0)*x770))+new_r10);
11347 evalcond[6]=(((new_r10*x765))+((new_r00*x764))+(((-1.0)*gconst11)));
11348 evalcond[7]=((((-1.0)*x767))+(((-1.0)*x768))+new_r00);
11349 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11350 {
11351 continue;
11352 }
11353 }
11354 
11355 {
11356 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11357 vinfos[0].jointtype = 1;
11358 vinfos[0].foffset = j0;
11359 vinfos[0].indices[0] = _ij0[0];
11360 vinfos[0].indices[1] = _ij0[1];
11361 vinfos[0].maxsolutions = _nj0;
11362 vinfos[1].jointtype = 1;
11363 vinfos[1].foffset = j1;
11364 vinfos[1].indices[0] = _ij1[0];
11365 vinfos[1].indices[1] = _ij1[1];
11366 vinfos[1].maxsolutions = _nj1;
11367 vinfos[2].jointtype = 1;
11368 vinfos[2].foffset = j2;
11369 vinfos[2].indices[0] = _ij2[0];
11370 vinfos[2].indices[1] = _ij2[1];
11371 vinfos[2].maxsolutions = _nj2;
11372 vinfos[3].jointtype = 1;
11373 vinfos[3].foffset = j3;
11374 vinfos[3].indices[0] = _ij3[0];
11375 vinfos[3].indices[1] = _ij3[1];
11376 vinfos[3].maxsolutions = _nj3;
11377 vinfos[4].jointtype = 1;
11378 vinfos[4].foffset = j4;
11379 vinfos[4].indices[0] = _ij4[0];
11380 vinfos[4].indices[1] = _ij4[1];
11381 vinfos[4].maxsolutions = _nj4;
11382 vinfos[5].jointtype = 1;
11383 vinfos[5].foffset = j5;
11384 vinfos[5].indices[0] = _ij5[0];
11385 vinfos[5].indices[1] = _ij5[1];
11386 vinfos[5].maxsolutions = _nj5;
11387 std::vector<int> vfree(0);
11388 solutions.AddSolution(vinfos,vfree);
11389 }
11390 }
11391 }
11392 
11393 }
11394 
11395 }
11396 
11397 } else
11398 {
11399 {
11400 IkReal j3array[1], cj3array[1], sj3array[1];
11401 bool j3valid[1]={false};
11402 _nj3 = 1;
11403 IkReal x771=((1.0)*gconst10);
11404 CheckValue<IkReal> x772 = IKatan2WithCheck(IkReal(((((-1.0)*new_r10*x771))+((gconst10*new_r01)))),IkReal(((((-1.0)*new_r00*x771))+(((-1.0)*new_r11*x771)))),IKFAST_ATAN2_MAGTHRESH);
11405 if(!x772.valid){
11406 continue;
11407 }
11408 CheckValue<IkReal> x773=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
11409 if(!x773.valid){
11410 continue;
11411 }
11412 j3array[0]=((-1.5707963267949)+(x772.value)+(((1.5707963267949)*(x773.value))));
11413 sj3array[0]=IKsin(j3array[0]);
11414 cj3array[0]=IKcos(j3array[0]);
11415 if( j3array[0] > IKPI )
11416 {
11417  j3array[0]-=IK2PI;
11418 }
11419 else if( j3array[0] < -IKPI )
11420 { j3array[0]+=IK2PI;
11421 }
11422 j3valid[0] = true;
11423 for(int ij3 = 0; ij3 < 1; ++ij3)
11424 {
11425 if( !j3valid[ij3] )
11426 {
11427  continue;
11428 }
11429 _ij3[0] = ij3; _ij3[1] = -1;
11430 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11431 {
11432 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11433 {
11434  j3valid[iij3]=false; _ij3[1] = iij3; break;
11435 }
11436 }
11437 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11438 {
11439 IkReal evalcond[8];
11440 IkReal x774=IKcos(j3);
11441 IkReal x775=IKsin(j3);
11442 IkReal x776=(gconst10*x774);
11443 IkReal x777=(gconst11*x774);
11444 IkReal x778=(gconst10*x775);
11445 IkReal x779=((1.0)*x775);
11446 IkReal x780=(gconst11*x779);
11447 evalcond[0]=(gconst10+((new_r11*x775))+((new_r01*x774)));
11448 evalcond[1]=(x777+x778+new_r11);
11449 evalcond[2]=((((-1.0)*x780))+x776+new_r01);
11450 evalcond[3]=((((-1.0)*new_r00*x779))+gconst10+((new_r10*x774)));
11451 evalcond[4]=((((-1.0)*new_r01*x779))+gconst11+((new_r11*x774)));
11452 evalcond[5]=((((-1.0)*x780))+x776+new_r10);
11453 evalcond[6]=(((new_r00*x774))+((new_r10*x775))+(((-1.0)*gconst11)));
11454 evalcond[7]=(new_r00+(((-1.0)*x778))+(((-1.0)*x777)));
11455 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11456 {
11457 continue;
11458 }
11459 }
11460 
11461 {
11462 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11463 vinfos[0].jointtype = 1;
11464 vinfos[0].foffset = j0;
11465 vinfos[0].indices[0] = _ij0[0];
11466 vinfos[0].indices[1] = _ij0[1];
11467 vinfos[0].maxsolutions = _nj0;
11468 vinfos[1].jointtype = 1;
11469 vinfos[1].foffset = j1;
11470 vinfos[1].indices[0] = _ij1[0];
11471 vinfos[1].indices[1] = _ij1[1];
11472 vinfos[1].maxsolutions = _nj1;
11473 vinfos[2].jointtype = 1;
11474 vinfos[2].foffset = j2;
11475 vinfos[2].indices[0] = _ij2[0];
11476 vinfos[2].indices[1] = _ij2[1];
11477 vinfos[2].maxsolutions = _nj2;
11478 vinfos[3].jointtype = 1;
11479 vinfos[3].foffset = j3;
11480 vinfos[3].indices[0] = _ij3[0];
11481 vinfos[3].indices[1] = _ij3[1];
11482 vinfos[3].maxsolutions = _nj3;
11483 vinfos[4].jointtype = 1;
11484 vinfos[4].foffset = j4;
11485 vinfos[4].indices[0] = _ij4[0];
11486 vinfos[4].indices[1] = _ij4[1];
11487 vinfos[4].maxsolutions = _nj4;
11488 vinfos[5].jointtype = 1;
11489 vinfos[5].foffset = j5;
11490 vinfos[5].indices[0] = _ij5[0];
11491 vinfos[5].indices[1] = _ij5[1];
11492 vinfos[5].maxsolutions = _nj5;
11493 std::vector<int> vfree(0);
11494 solutions.AddSolution(vinfos,vfree);
11495 }
11496 }
11497 }
11498 
11499 }
11500 
11501 }
11502 
11503 }
11504 } while(0);
11505 if( bgotonextstatement )
11506 {
11507 bool bgotonextstatement = true;
11508 do
11509 {
11510 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11511 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11512 {
11513 bgotonextstatement=false;
11514 {
11515 IkReal j3eval[1];
11516 sj4=0;
11517 cj4=-1.0;
11518 j4=3.14159265358979;
11519 new_r11=0;
11520 new_r01=0;
11521 new_r22=0;
11522 new_r20=0;
11523 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
11524 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11525 {
11526 continue; // no branches [j3]
11527 
11528 } else
11529 {
11530 {
11531 IkReal j3array[2], cj3array[2], sj3array[2];
11532 bool j3valid[2]={false};
11533 _nj3 = 2;
11534 CheckValue<IkReal> x782 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11535 if(!x782.valid){
11536 continue;
11537 }
11538 IkReal x781=x782.value;
11539 j3array[0]=((-1.0)*x781);
11540 sj3array[0]=IKsin(j3array[0]);
11541 cj3array[0]=IKcos(j3array[0]);
11542 j3array[1]=((3.14159265358979)+(((-1.0)*x781)));
11543 sj3array[1]=IKsin(j3array[1]);
11544 cj3array[1]=IKcos(j3array[1]);
11545 if( j3array[0] > IKPI )
11546 {
11547  j3array[0]-=IK2PI;
11548 }
11549 else if( j3array[0] < -IKPI )
11550 { j3array[0]+=IK2PI;
11551 }
11552 j3valid[0] = true;
11553 if( j3array[1] > IKPI )
11554 {
11555  j3array[1]-=IK2PI;
11556 }
11557 else if( j3array[1] < -IKPI )
11558 { j3array[1]+=IK2PI;
11559 }
11560 j3valid[1] = true;
11561 for(int ij3 = 0; ij3 < 2; ++ij3)
11562 {
11563 if( !j3valid[ij3] )
11564 {
11565  continue;
11566 }
11567 _ij3[0] = ij3; _ij3[1] = -1;
11568 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11569 {
11570 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11571 {
11572  j3valid[iij3]=false; _ij3[1] = iij3; break;
11573 }
11574 }
11575 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11576 {
11577 IkReal evalcond[1];
11578 evalcond[0]=((((-1.0)*new_r00*(IKsin(j3))))+((new_r10*(IKcos(j3)))));
11579 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
11580 {
11581 continue;
11582 }
11583 }
11584 
11585 {
11586 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11587 vinfos[0].jointtype = 1;
11588 vinfos[0].foffset = j0;
11589 vinfos[0].indices[0] = _ij0[0];
11590 vinfos[0].indices[1] = _ij0[1];
11591 vinfos[0].maxsolutions = _nj0;
11592 vinfos[1].jointtype = 1;
11593 vinfos[1].foffset = j1;
11594 vinfos[1].indices[0] = _ij1[0];
11595 vinfos[1].indices[1] = _ij1[1];
11596 vinfos[1].maxsolutions = _nj1;
11597 vinfos[2].jointtype = 1;
11598 vinfos[2].foffset = j2;
11599 vinfos[2].indices[0] = _ij2[0];
11600 vinfos[2].indices[1] = _ij2[1];
11601 vinfos[2].maxsolutions = _nj2;
11602 vinfos[3].jointtype = 1;
11603 vinfos[3].foffset = j3;
11604 vinfos[3].indices[0] = _ij3[0];
11605 vinfos[3].indices[1] = _ij3[1];
11606 vinfos[3].maxsolutions = _nj3;
11607 vinfos[4].jointtype = 1;
11608 vinfos[4].foffset = j4;
11609 vinfos[4].indices[0] = _ij4[0];
11610 vinfos[4].indices[1] = _ij4[1];
11611 vinfos[4].maxsolutions = _nj4;
11612 vinfos[5].jointtype = 1;
11613 vinfos[5].foffset = j5;
11614 vinfos[5].indices[0] = _ij5[0];
11615 vinfos[5].indices[1] = _ij5[1];
11616 vinfos[5].maxsolutions = _nj5;
11617 std::vector<int> vfree(0);
11618 solutions.AddSolution(vinfos,vfree);
11619 }
11620 }
11621 }
11622 
11623 }
11624 
11625 }
11626 
11627 }
11628 } while(0);
11629 if( bgotonextstatement )
11630 {
11631 bool bgotonextstatement = true;
11632 do
11633 {
11634 if( 1 )
11635 {
11636 bgotonextstatement=false;
11637 continue; // branch miss [j3]
11638 
11639 }
11640 } while(0);
11641 if( bgotonextstatement )
11642 {
11643 }
11644 }
11645 }
11646 }
11647 }
11648 }
11649 }
11650 }
11651 
11652 } else
11653 {
11654 {
11655 IkReal j3array[1], cj3array[1], sj3array[1];
11656 bool j3valid[1]={false};
11657 _nj3 = 1;
11658 IkReal x783=((1.0)*sj5);
11659 CheckValue<IkReal> x784 = IKatan2WithCheck(IkReal((((cj5*new_r01))+(((-1.0)*new_r11*x783)))),IkReal(((((-1.0)*new_r01*x783))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
11660 if(!x784.valid){
11661 continue;
11662 }
11663 CheckValue<IkReal> x785=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1);
11664 if(!x785.valid){
11665 continue;
11666 }
11667 j3array[0]=((-1.5707963267949)+(x784.value)+(((1.5707963267949)*(x785.value))));
11668 sj3array[0]=IKsin(j3array[0]);
11669 cj3array[0]=IKcos(j3array[0]);
11670 if( j3array[0] > IKPI )
11671 {
11672  j3array[0]-=IK2PI;
11673 }
11674 else if( j3array[0] < -IKPI )
11675 { j3array[0]+=IK2PI;
11676 }
11677 j3valid[0] = true;
11678 for(int ij3 = 0; ij3 < 1; ++ij3)
11679 {
11680 if( !j3valid[ij3] )
11681 {
11682  continue;
11683 }
11684 _ij3[0] = ij3; _ij3[1] = -1;
11685 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11686 {
11687 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11688 {
11689  j3valid[iij3]=false; _ij3[1] = iij3; break;
11690 }
11691 }
11692 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11693 {
11694 IkReal evalcond[8];
11695 IkReal x786=IKsin(j3);
11696 IkReal x787=IKcos(j3);
11697 IkReal x788=((1.0)*cj5);
11698 IkReal x789=(sj5*x787);
11699 IkReal x790=(sj5*x786);
11700 IkReal x791=((1.0)*x786);
11701 IkReal x792=(x786*x788);
11702 evalcond[0]=(sj5+((new_r11*x786))+((new_r01*x787)));
11703 evalcond[1]=(((cj5*x787))+x790+new_r11);
11704 evalcond[2]=((((-1.0)*x792))+x789+new_r01);
11705 evalcond[3]=(sj5+((new_r10*x787))+(((-1.0)*new_r00*x791)));
11706 evalcond[4]=((((-1.0)*new_r01*x791))+cj5+((new_r11*x787)));
11707 evalcond[5]=((((-1.0)*x792))+x789+new_r10);
11708 evalcond[6]=(((new_r10*x786))+(((-1.0)*x788))+((new_r00*x787)));
11709 evalcond[7]=((((-1.0)*x787*x788))+(((-1.0)*x790))+new_r00);
11710 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11711 {
11712 continue;
11713 }
11714 }
11715 
11716 {
11717 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11718 vinfos[0].jointtype = 1;
11719 vinfos[0].foffset = j0;
11720 vinfos[0].indices[0] = _ij0[0];
11721 vinfos[0].indices[1] = _ij0[1];
11722 vinfos[0].maxsolutions = _nj0;
11723 vinfos[1].jointtype = 1;
11724 vinfos[1].foffset = j1;
11725 vinfos[1].indices[0] = _ij1[0];
11726 vinfos[1].indices[1] = _ij1[1];
11727 vinfos[1].maxsolutions = _nj1;
11728 vinfos[2].jointtype = 1;
11729 vinfos[2].foffset = j2;
11730 vinfos[2].indices[0] = _ij2[0];
11731 vinfos[2].indices[1] = _ij2[1];
11732 vinfos[2].maxsolutions = _nj2;
11733 vinfos[3].jointtype = 1;
11734 vinfos[3].foffset = j3;
11735 vinfos[3].indices[0] = _ij3[0];
11736 vinfos[3].indices[1] = _ij3[1];
11737 vinfos[3].maxsolutions = _nj3;
11738 vinfos[4].jointtype = 1;
11739 vinfos[4].foffset = j4;
11740 vinfos[4].indices[0] = _ij4[0];
11741 vinfos[4].indices[1] = _ij4[1];
11742 vinfos[4].maxsolutions = _nj4;
11743 vinfos[5].jointtype = 1;
11744 vinfos[5].foffset = j5;
11745 vinfos[5].indices[0] = _ij5[0];
11746 vinfos[5].indices[1] = _ij5[1];
11747 vinfos[5].maxsolutions = _nj5;
11748 std::vector<int> vfree(0);
11749 solutions.AddSolution(vinfos,vfree);
11750 }
11751 }
11752 }
11753 
11754 }
11755 
11756 }
11757 
11758 } else
11759 {
11760 {
11761 IkReal j3array[1], cj3array[1], sj3array[1];
11762 bool j3valid[1]={false};
11763 _nj3 = 1;
11764 IkReal x793=((1.0)*sj5);
11765 CheckValue<IkReal> x794 = IKatan2WithCheck(IkReal((((new_r01*sj5))+(((-1.0)*new_r10*x793)))),IkReal(((((-1.0)*new_r11*x793))+(((-1.0)*new_r00*x793)))),IKFAST_ATAN2_MAGTHRESH);
11766 if(!x794.valid){
11767 continue;
11768 }
11769 CheckValue<IkReal> x795=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1);
11770 if(!x795.valid){
11771 continue;
11772 }
11773 j3array[0]=((-1.5707963267949)+(x794.value)+(((1.5707963267949)*(x795.value))));
11774 sj3array[0]=IKsin(j3array[0]);
11775 cj3array[0]=IKcos(j3array[0]);
11776 if( j3array[0] > IKPI )
11777 {
11778  j3array[0]-=IK2PI;
11779 }
11780 else if( j3array[0] < -IKPI )
11781 { j3array[0]+=IK2PI;
11782 }
11783 j3valid[0] = true;
11784 for(int ij3 = 0; ij3 < 1; ++ij3)
11785 {
11786 if( !j3valid[ij3] )
11787 {
11788  continue;
11789 }
11790 _ij3[0] = ij3; _ij3[1] = -1;
11791 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11792 {
11793 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11794 {
11795  j3valid[iij3]=false; _ij3[1] = iij3; break;
11796 }
11797 }
11798 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11799 {
11800 IkReal evalcond[8];
11801 IkReal x796=IKsin(j3);
11802 IkReal x797=IKcos(j3);
11803 IkReal x798=((1.0)*cj5);
11804 IkReal x799=(sj5*x797);
11805 IkReal x800=(sj5*x796);
11806 IkReal x801=((1.0)*x796);
11807 IkReal x802=(x796*x798);
11808 evalcond[0]=(sj5+((new_r11*x796))+((new_r01*x797)));
11809 evalcond[1]=(((cj5*x797))+new_r11+x800);
11810 evalcond[2]=(x799+new_r01+(((-1.0)*x802)));
11811 evalcond[3]=(sj5+((new_r10*x797))+(((-1.0)*new_r00*x801)));
11812 evalcond[4]=((((-1.0)*new_r01*x801))+cj5+((new_r11*x797)));
11813 evalcond[5]=(x799+new_r10+(((-1.0)*x802)));
11814 evalcond[6]=((((-1.0)*x798))+((new_r10*x796))+((new_r00*x797)));
11815 evalcond[7]=((((-1.0)*x800))+(((-1.0)*x797*x798))+new_r00);
11816 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11817 {
11818 continue;
11819 }
11820 }
11821 
11822 {
11823 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11824 vinfos[0].jointtype = 1;
11825 vinfos[0].foffset = j0;
11826 vinfos[0].indices[0] = _ij0[0];
11827 vinfos[0].indices[1] = _ij0[1];
11828 vinfos[0].maxsolutions = _nj0;
11829 vinfos[1].jointtype = 1;
11830 vinfos[1].foffset = j1;
11831 vinfos[1].indices[0] = _ij1[0];
11832 vinfos[1].indices[1] = _ij1[1];
11833 vinfos[1].maxsolutions = _nj1;
11834 vinfos[2].jointtype = 1;
11835 vinfos[2].foffset = j2;
11836 vinfos[2].indices[0] = _ij2[0];
11837 vinfos[2].indices[1] = _ij2[1];
11838 vinfos[2].maxsolutions = _nj2;
11839 vinfos[3].jointtype = 1;
11840 vinfos[3].foffset = j3;
11841 vinfos[3].indices[0] = _ij3[0];
11842 vinfos[3].indices[1] = _ij3[1];
11843 vinfos[3].maxsolutions = _nj3;
11844 vinfos[4].jointtype = 1;
11845 vinfos[4].foffset = j4;
11846 vinfos[4].indices[0] = _ij4[0];
11847 vinfos[4].indices[1] = _ij4[1];
11848 vinfos[4].maxsolutions = _nj4;
11849 vinfos[5].jointtype = 1;
11850 vinfos[5].foffset = j5;
11851 vinfos[5].indices[0] = _ij5[0];
11852 vinfos[5].indices[1] = _ij5[1];
11853 vinfos[5].maxsolutions = _nj5;
11854 std::vector<int> vfree(0);
11855 solutions.AddSolution(vinfos,vfree);
11856 }
11857 }
11858 }
11859 
11860 }
11861 
11862 }
11863 
11864 } else
11865 {
11866 {
11867 IkReal j3array[1], cj3array[1], sj3array[1];
11868 bool j3valid[1]={false};
11869 _nj3 = 1;
11870 CheckValue<IkReal> x803 = IKatan2WithCheck(IkReal(((-1.0)+(new_r01*new_r01)+(cj5*cj5))),IkReal(((((-1.0)*cj5*sj5))+(((-1.0)*new_r01*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
11871 if(!x803.valid){
11872 continue;
11873 }
11874 CheckValue<IkReal> x804=IKPowWithIntegerCheck(IKsign((((new_r11*sj5))+((cj5*new_r01)))),-1);
11875 if(!x804.valid){
11876 continue;
11877 }
11878 j3array[0]=((-1.5707963267949)+(x803.value)+(((1.5707963267949)*(x804.value))));
11879 sj3array[0]=IKsin(j3array[0]);
11880 cj3array[0]=IKcos(j3array[0]);
11881 if( j3array[0] > IKPI )
11882 {
11883  j3array[0]-=IK2PI;
11884 }
11885 else if( j3array[0] < -IKPI )
11886 { j3array[0]+=IK2PI;
11887 }
11888 j3valid[0] = true;
11889 for(int ij3 = 0; ij3 < 1; ++ij3)
11890 {
11891 if( !j3valid[ij3] )
11892 {
11893  continue;
11894 }
11895 _ij3[0] = ij3; _ij3[1] = -1;
11896 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11897 {
11898 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11899 {
11900  j3valid[iij3]=false; _ij3[1] = iij3; break;
11901 }
11902 }
11903 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11904 {
11905 IkReal evalcond[8];
11906 IkReal x805=IKsin(j3);
11907 IkReal x806=IKcos(j3);
11908 IkReal x807=((1.0)*cj5);
11909 IkReal x808=(sj5*x806);
11910 IkReal x809=(sj5*x805);
11911 IkReal x810=((1.0)*x805);
11912 IkReal x811=(x805*x807);
11913 evalcond[0]=(sj5+((new_r11*x805))+((new_r01*x806)));
11914 evalcond[1]=(((cj5*x806))+new_r11+x809);
11915 evalcond[2]=((((-1.0)*x811))+new_r01+x808);
11916 evalcond[3]=(sj5+(((-1.0)*new_r00*x810))+((new_r10*x806)));
11917 evalcond[4]=(cj5+(((-1.0)*new_r01*x810))+((new_r11*x806)));
11918 evalcond[5]=((((-1.0)*x811))+new_r10+x808);
11919 evalcond[6]=(((new_r00*x806))+((new_r10*x805))+(((-1.0)*x807)));
11920 evalcond[7]=((((-1.0)*x809))+(((-1.0)*x806*x807))+new_r00);
11921 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11922 {
11923 continue;
11924 }
11925 }
11926 
11927 {
11928 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11929 vinfos[0].jointtype = 1;
11930 vinfos[0].foffset = j0;
11931 vinfos[0].indices[0] = _ij0[0];
11932 vinfos[0].indices[1] = _ij0[1];
11933 vinfos[0].maxsolutions = _nj0;
11934 vinfos[1].jointtype = 1;
11935 vinfos[1].foffset = j1;
11936 vinfos[1].indices[0] = _ij1[0];
11937 vinfos[1].indices[1] = _ij1[1];
11938 vinfos[1].maxsolutions = _nj1;
11939 vinfos[2].jointtype = 1;
11940 vinfos[2].foffset = j2;
11941 vinfos[2].indices[0] = _ij2[0];
11942 vinfos[2].indices[1] = _ij2[1];
11943 vinfos[2].maxsolutions = _nj2;
11944 vinfos[3].jointtype = 1;
11945 vinfos[3].foffset = j3;
11946 vinfos[3].indices[0] = _ij3[0];
11947 vinfos[3].indices[1] = _ij3[1];
11948 vinfos[3].maxsolutions = _nj3;
11949 vinfos[4].jointtype = 1;
11950 vinfos[4].foffset = j4;
11951 vinfos[4].indices[0] = _ij4[0];
11952 vinfos[4].indices[1] = _ij4[1];
11953 vinfos[4].maxsolutions = _nj4;
11954 vinfos[5].jointtype = 1;
11955 vinfos[5].foffset = j5;
11956 vinfos[5].indices[0] = _ij5[0];
11957 vinfos[5].indices[1] = _ij5[1];
11958 vinfos[5].maxsolutions = _nj5;
11959 std::vector<int> vfree(0);
11960 solutions.AddSolution(vinfos,vfree);
11961 }
11962 }
11963 }
11964 
11965 }
11966 
11967 }
11968 
11969 }
11970 } while(0);
11971 if( bgotonextstatement )
11972 {
11973 bool bgotonextstatement = true;
11974 do
11975 {
11976 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
11977 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11978 {
11979 bgotonextstatement=false;
11980 {
11981 IkReal j3eval[1];
11982 new_r02=0;
11983 new_r12=0;
11984 new_r20=0;
11985 new_r21=0;
11986 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11987 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11988 {
11989 {
11990 IkReal j3eval[1];
11991 new_r02=0;
11992 new_r12=0;
11993 new_r20=0;
11994 new_r21=0;
11995 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
11996 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11997 {
11998 {
11999 IkReal j3eval[1];
12000 new_r02=0;
12001 new_r12=0;
12002 new_r20=0;
12003 new_r21=0;
12004 j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
12005 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12006 {
12007 continue; // no branches [j3]
12008 
12009 } else
12010 {
12011 {
12012 IkReal j3array[2], cj3array[2], sj3array[2];
12013 bool j3valid[2]={false};
12014 _nj3 = 2;
12015 CheckValue<IkReal> x813 = IKatan2WithCheck(IkReal((new_r10*new_r22)),IkReal(((-1.0)*new_r00*new_r22)),IKFAST_ATAN2_MAGTHRESH);
12016 if(!x813.valid){
12017 continue;
12018 }
12019 IkReal x812=x813.value;
12020 j3array[0]=((-1.0)*x812);
12021 sj3array[0]=IKsin(j3array[0]);
12022 cj3array[0]=IKcos(j3array[0]);
12023 j3array[1]=((3.14159265358979)+(((-1.0)*x812)));
12024 sj3array[1]=IKsin(j3array[1]);
12025 cj3array[1]=IKcos(j3array[1]);
12026 if( j3array[0] > IKPI )
12027 {
12028  j3array[0]-=IK2PI;
12029 }
12030 else if( j3array[0] < -IKPI )
12031 { j3array[0]+=IK2PI;
12032 }
12033 j3valid[0] = true;
12034 if( j3array[1] > IKPI )
12035 {
12036  j3array[1]-=IK2PI;
12037 }
12038 else if( j3array[1] < -IKPI )
12039 { j3array[1]+=IK2PI;
12040 }
12041 j3valid[1] = true;
12042 for(int ij3 = 0; ij3 < 2; ++ij3)
12043 {
12044 if( !j3valid[ij3] )
12045 {
12046  continue;
12047 }
12048 _ij3[0] = ij3; _ij3[1] = -1;
12049 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12050 {
12051 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12052 {
12053  j3valid[iij3]=false; _ij3[1] = iij3; break;
12054 }
12055 }
12056 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12057 {
12058 IkReal evalcond[5];
12059 IkReal x814=IKsin(j3);
12060 IkReal x815=IKcos(j3);
12061 IkReal x816=(new_r11*x815);
12062 IkReal x817=((1.0)*x814);
12063 evalcond[0]=(((new_r11*x814))+((new_r01*x815)));
12064 evalcond[1]=(((new_r00*x815))+((new_r10*x814)));
12065 evalcond[2]=(((new_r10*x815))+(((-1.0)*new_r00*x817)));
12066 evalcond[3]=((((-1.0)*new_r01*x817))+x816);
12067 evalcond[4]=(((new_r22*x816))+(((-1.0)*new_r01*new_r22*x817)));
12068 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
12069 {
12070 continue;
12071 }
12072 }
12073 
12074 {
12075 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12076 vinfos[0].jointtype = 1;
12077 vinfos[0].foffset = j0;
12078 vinfos[0].indices[0] = _ij0[0];
12079 vinfos[0].indices[1] = _ij0[1];
12080 vinfos[0].maxsolutions = _nj0;
12081 vinfos[1].jointtype = 1;
12082 vinfos[1].foffset = j1;
12083 vinfos[1].indices[0] = _ij1[0];
12084 vinfos[1].indices[1] = _ij1[1];
12085 vinfos[1].maxsolutions = _nj1;
12086 vinfos[2].jointtype = 1;
12087 vinfos[2].foffset = j2;
12088 vinfos[2].indices[0] = _ij2[0];
12089 vinfos[2].indices[1] = _ij2[1];
12090 vinfos[2].maxsolutions = _nj2;
12091 vinfos[3].jointtype = 1;
12092 vinfos[3].foffset = j3;
12093 vinfos[3].indices[0] = _ij3[0];
12094 vinfos[3].indices[1] = _ij3[1];
12095 vinfos[3].maxsolutions = _nj3;
12096 vinfos[4].jointtype = 1;
12097 vinfos[4].foffset = j4;
12098 vinfos[4].indices[0] = _ij4[0];
12099 vinfos[4].indices[1] = _ij4[1];
12100 vinfos[4].maxsolutions = _nj4;
12101 vinfos[5].jointtype = 1;
12102 vinfos[5].foffset = j5;
12103 vinfos[5].indices[0] = _ij5[0];
12104 vinfos[5].indices[1] = _ij5[1];
12105 vinfos[5].maxsolutions = _nj5;
12106 std::vector<int> vfree(0);
12107 solutions.AddSolution(vinfos,vfree);
12108 }
12109 }
12110 }
12111 
12112 }
12113 
12114 }
12115 
12116 } else
12117 {
12118 {
12119 IkReal j3array[2], cj3array[2], sj3array[2];
12120 bool j3valid[2]={false};
12121 _nj3 = 2;
12122 CheckValue<IkReal> x819 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
12123 if(!x819.valid){
12124 continue;
12125 }
12126 IkReal x818=x819.value;
12127 j3array[0]=((-1.0)*x818);
12128 sj3array[0]=IKsin(j3array[0]);
12129 cj3array[0]=IKcos(j3array[0]);
12130 j3array[1]=((3.14159265358979)+(((-1.0)*x818)));
12131 sj3array[1]=IKsin(j3array[1]);
12132 cj3array[1]=IKcos(j3array[1]);
12133 if( j3array[0] > IKPI )
12134 {
12135  j3array[0]-=IK2PI;
12136 }
12137 else if( j3array[0] < -IKPI )
12138 { j3array[0]+=IK2PI;
12139 }
12140 j3valid[0] = true;
12141 if( j3array[1] > IKPI )
12142 {
12143  j3array[1]-=IK2PI;
12144 }
12145 else if( j3array[1] < -IKPI )
12146 { j3array[1]+=IK2PI;
12147 }
12148 j3valid[1] = true;
12149 for(int ij3 = 0; ij3 < 2; ++ij3)
12150 {
12151 if( !j3valid[ij3] )
12152 {
12153  continue;
12154 }
12155 _ij3[0] = ij3; _ij3[1] = -1;
12156 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12157 {
12158 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12159 {
12160  j3valid[iij3]=false; _ij3[1] = iij3; break;
12161 }
12162 }
12163 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12164 {
12165 IkReal evalcond[5];
12166 IkReal x820=IKcos(j3);
12167 IkReal x821=IKsin(j3);
12168 IkReal x822=(new_r11*x820);
12169 IkReal x823=(new_r10*x820);
12170 IkReal x824=((1.0)*x821);
12171 evalcond[0]=(((new_r01*x820))+((new_r11*x821)));
12172 evalcond[1]=((((-1.0)*new_r00*x824))+x823);
12173 evalcond[2]=((((-1.0)*new_r01*x824))+x822);
12174 evalcond[3]=((((-1.0)*new_r00*new_r22*x824))+((new_r22*x823)));
12175 evalcond[4]=(((new_r22*x822))+(((-1.0)*new_r01*new_r22*x824)));
12176 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
12177 {
12178 continue;
12179 }
12180 }
12181 
12182 {
12183 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12184 vinfos[0].jointtype = 1;
12185 vinfos[0].foffset = j0;
12186 vinfos[0].indices[0] = _ij0[0];
12187 vinfos[0].indices[1] = _ij0[1];
12188 vinfos[0].maxsolutions = _nj0;
12189 vinfos[1].jointtype = 1;
12190 vinfos[1].foffset = j1;
12191 vinfos[1].indices[0] = _ij1[0];
12192 vinfos[1].indices[1] = _ij1[1];
12193 vinfos[1].maxsolutions = _nj1;
12194 vinfos[2].jointtype = 1;
12195 vinfos[2].foffset = j2;
12196 vinfos[2].indices[0] = _ij2[0];
12197 vinfos[2].indices[1] = _ij2[1];
12198 vinfos[2].maxsolutions = _nj2;
12199 vinfos[3].jointtype = 1;
12200 vinfos[3].foffset = j3;
12201 vinfos[3].indices[0] = _ij3[0];
12202 vinfos[3].indices[1] = _ij3[1];
12203 vinfos[3].maxsolutions = _nj3;
12204 vinfos[4].jointtype = 1;
12205 vinfos[4].foffset = j4;
12206 vinfos[4].indices[0] = _ij4[0];
12207 vinfos[4].indices[1] = _ij4[1];
12208 vinfos[4].maxsolutions = _nj4;
12209 vinfos[5].jointtype = 1;
12210 vinfos[5].foffset = j5;
12211 vinfos[5].indices[0] = _ij5[0];
12212 vinfos[5].indices[1] = _ij5[1];
12213 vinfos[5].maxsolutions = _nj5;
12214 std::vector<int> vfree(0);
12215 solutions.AddSolution(vinfos,vfree);
12216 }
12217 }
12218 }
12219 
12220 }
12221 
12222 }
12223 
12224 } else
12225 {
12226 {
12227 IkReal j3array[2], cj3array[2], sj3array[2];
12228 bool j3valid[2]={false};
12229 _nj3 = 2;
12230 CheckValue<IkReal> x826 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
12231 if(!x826.valid){
12232 continue;
12233 }
12234 IkReal x825=x826.value;
12235 j3array[0]=((-1.0)*x825);
12236 sj3array[0]=IKsin(j3array[0]);
12237 cj3array[0]=IKcos(j3array[0]);
12238 j3array[1]=((3.14159265358979)+(((-1.0)*x825)));
12239 sj3array[1]=IKsin(j3array[1]);
12240 cj3array[1]=IKcos(j3array[1]);
12241 if( j3array[0] > IKPI )
12242 {
12243  j3array[0]-=IK2PI;
12244 }
12245 else if( j3array[0] < -IKPI )
12246 { j3array[0]+=IK2PI;
12247 }
12248 j3valid[0] = true;
12249 if( j3array[1] > IKPI )
12250 {
12251  j3array[1]-=IK2PI;
12252 }
12253 else if( j3array[1] < -IKPI )
12254 { j3array[1]+=IK2PI;
12255 }
12256 j3valid[1] = true;
12257 for(int ij3 = 0; ij3 < 2; ++ij3)
12258 {
12259 if( !j3valid[ij3] )
12260 {
12261  continue;
12262 }
12263 _ij3[0] = ij3; _ij3[1] = -1;
12264 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12265 {
12266 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12267 {
12268  j3valid[iij3]=false; _ij3[1] = iij3; break;
12269 }
12270 }
12271 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12272 {
12273 IkReal evalcond[5];
12274 IkReal x827=IKcos(j3);
12275 IkReal x828=IKsin(j3);
12276 IkReal x829=(new_r11*x827);
12277 IkReal x830=(new_r10*x827);
12278 IkReal x831=((1.0)*x828);
12279 evalcond[0]=(((new_r10*x828))+((new_r00*x827)));
12280 evalcond[1]=(x830+(((-1.0)*new_r00*x831)));
12281 evalcond[2]=((((-1.0)*new_r01*x831))+x829);
12282 evalcond[3]=((((-1.0)*new_r00*new_r22*x831))+((new_r22*x830)));
12283 evalcond[4]=(((new_r22*x829))+(((-1.0)*new_r01*new_r22*x831)));
12284 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
12285 {
12286 continue;
12287 }
12288 }
12289 
12290 {
12291 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12292 vinfos[0].jointtype = 1;
12293 vinfos[0].foffset = j0;
12294 vinfos[0].indices[0] = _ij0[0];
12295 vinfos[0].indices[1] = _ij0[1];
12296 vinfos[0].maxsolutions = _nj0;
12297 vinfos[1].jointtype = 1;
12298 vinfos[1].foffset = j1;
12299 vinfos[1].indices[0] = _ij1[0];
12300 vinfos[1].indices[1] = _ij1[1];
12301 vinfos[1].maxsolutions = _nj1;
12302 vinfos[2].jointtype = 1;
12303 vinfos[2].foffset = j2;
12304 vinfos[2].indices[0] = _ij2[0];
12305 vinfos[2].indices[1] = _ij2[1];
12306 vinfos[2].maxsolutions = _nj2;
12307 vinfos[3].jointtype = 1;
12308 vinfos[3].foffset = j3;
12309 vinfos[3].indices[0] = _ij3[0];
12310 vinfos[3].indices[1] = _ij3[1];
12311 vinfos[3].maxsolutions = _nj3;
12312 vinfos[4].jointtype = 1;
12313 vinfos[4].foffset = j4;
12314 vinfos[4].indices[0] = _ij4[0];
12315 vinfos[4].indices[1] = _ij4[1];
12316 vinfos[4].maxsolutions = _nj4;
12317 vinfos[5].jointtype = 1;
12318 vinfos[5].foffset = j5;
12319 vinfos[5].indices[0] = _ij5[0];
12320 vinfos[5].indices[1] = _ij5[1];
12321 vinfos[5].maxsolutions = _nj5;
12322 std::vector<int> vfree(0);
12323 solutions.AddSolution(vinfos,vfree);
12324 }
12325 }
12326 }
12327 
12328 }
12329 
12330 }
12331 
12332 }
12333 } while(0);
12334 if( bgotonextstatement )
12335 {
12336 bool bgotonextstatement = true;
12337 do
12338 {
12339 if( 1 )
12340 {
12341 bgotonextstatement=false;
12342 continue; // branch miss [j3]
12343 
12344 }
12345 } while(0);
12346 if( bgotonextstatement )
12347 {
12348 }
12349 }
12350 }
12351 }
12352 }
12353 
12354 } else
12355 {
12356 {
12357 IkReal j3array[1], cj3array[1], sj3array[1];
12358 bool j3valid[1]={false};
12359 _nj3 = 1;
12360 CheckValue<IkReal> x833=IKPowWithIntegerCheck(sj4,-1);
12361 if(!x833.valid){
12362 continue;
12363 }
12364 IkReal x832=x833.value;
12365 CheckValue<IkReal> x834=IKPowWithIntegerCheck(new_r11,-1);
12366 if(!x834.valid){
12367 continue;
12368 }
12369 if( IKabs((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x832)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5))))))+IKsqr(((-1.0)*new_r12*x832))-1) <= IKFAST_SINCOS_THRESH )
12370  continue;
12371 j3array[0]=IKatan2((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5))))), ((-1.0)*new_r12*x832));
12372 sj3array[0]=IKsin(j3array[0]);
12373 cj3array[0]=IKcos(j3array[0]);
12374 if( j3array[0] > IKPI )
12375 {
12376  j3array[0]-=IK2PI;
12377 }
12378 else if( j3array[0] < -IKPI )
12379 { j3array[0]+=IK2PI;
12380 }
12381 j3valid[0] = true;
12382 for(int ij3 = 0; ij3 < 1; ++ij3)
12383 {
12384 if( !j3valid[ij3] )
12385 {
12386  continue;
12387 }
12388 _ij3[0] = ij3; _ij3[1] = -1;
12389 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12390 {
12391 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12392 {
12393  j3valid[iij3]=false; _ij3[1] = iij3; break;
12394 }
12395 }
12396 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12397 {
12398 IkReal evalcond[18];
12399 IkReal x835=IKsin(j3);
12400 IkReal x836=IKcos(j3);
12401 IkReal x837=((1.0)*cj5);
12402 IkReal x838=((1.0)*cj4);
12403 IkReal x839=(new_r11*x836);
12404 IkReal x840=(sj5*x836);
12405 IkReal x841=(cj4*x836);
12406 IkReal x842=(sj4*x836);
12407 IkReal x843=(new_r00*x835);
12408 IkReal x844=(cj4*x835);
12409 IkReal x845=(new_r01*x835);
12410 IkReal x846=(new_r02*x835);
12411 IkReal x847=((1.0)*sj4*x835);
12412 evalcond[0]=(new_r12+x842);
12413 evalcond[1]=((((-1.0)*x847))+new_r02);
12414 evalcond[2]=(((new_r12*x835))+((new_r02*x836)));
12415 evalcond[3]=(((new_r11*x835))+sj5+((new_r01*x836)));
12416 evalcond[4]=(sj4+((new_r12*x836))+(((-1.0)*x846)));
12417 evalcond[5]=(((cj5*x844))+new_r01+x840);
12418 evalcond[6]=(((new_r00*x836))+(((-1.0)*x837))+((new_r10*x835)));
12419 evalcond[7]=((((-1.0)*x836*x837))+((sj5*x844))+new_r00);
12420 evalcond[8]=((((-1.0)*x837*x841))+((sj5*x835))+new_r11);
12421 evalcond[9]=((((-1.0)*x835*x837))+(((-1.0)*x838*x840))+new_r10);
12422 evalcond[10]=((((-1.0)*x843))+(((-1.0)*sj5*x838))+((new_r10*x836)));
12423 evalcond[11]=((((-1.0)*cj4*x837))+(((-1.0)*x845))+x839);
12424 evalcond[12]=(((new_r12*x841))+((new_r22*sj4))+(((-1.0)*x838*x846)));
12425 evalcond[13]=(((new_r10*x842))+(((-1.0)*sj4*x843))+(((-1.0)*new_r20*x838)));
12426 evalcond[14]=(((sj4*x839))+(((-1.0)*sj4*x845))+(((-1.0)*new_r21*x838)));
12427 evalcond[15]=((1.0)+(((-1.0)*new_r22*x838))+((new_r12*x842))+(((-1.0)*sj4*x846)));
12428 evalcond[16]=(((new_r10*x841))+(((-1.0)*sj5))+((new_r20*sj4))+(((-1.0)*x838*x843)));
12429 evalcond[17]=(((cj4*x839))+(((-1.0)*x837))+(((-1.0)*x838*x845))+((new_r21*sj4)));
12430 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
12431 {
12432 continue;
12433 }
12434 }
12435 
12436 {
12437 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12438 vinfos[0].jointtype = 1;
12439 vinfos[0].foffset = j0;
12440 vinfos[0].indices[0] = _ij0[0];
12441 vinfos[0].indices[1] = _ij0[1];
12442 vinfos[0].maxsolutions = _nj0;
12443 vinfos[1].jointtype = 1;
12444 vinfos[1].foffset = j1;
12445 vinfos[1].indices[0] = _ij1[0];
12446 vinfos[1].indices[1] = _ij1[1];
12447 vinfos[1].maxsolutions = _nj1;
12448 vinfos[2].jointtype = 1;
12449 vinfos[2].foffset = j2;
12450 vinfos[2].indices[0] = _ij2[0];
12451 vinfos[2].indices[1] = _ij2[1];
12452 vinfos[2].maxsolutions = _nj2;
12453 vinfos[3].jointtype = 1;
12454 vinfos[3].foffset = j3;
12455 vinfos[3].indices[0] = _ij3[0];
12456 vinfos[3].indices[1] = _ij3[1];
12457 vinfos[3].maxsolutions = _nj3;
12458 vinfos[4].jointtype = 1;
12459 vinfos[4].foffset = j4;
12460 vinfos[4].indices[0] = _ij4[0];
12461 vinfos[4].indices[1] = _ij4[1];
12462 vinfos[4].maxsolutions = _nj4;
12463 vinfos[5].jointtype = 1;
12464 vinfos[5].foffset = j5;
12465 vinfos[5].indices[0] = _ij5[0];
12466 vinfos[5].indices[1] = _ij5[1];
12467 vinfos[5].maxsolutions = _nj5;
12468 std::vector<int> vfree(0);
12469 solutions.AddSolution(vinfos,vfree);
12470 }
12471 }
12472 }
12473 
12474 }
12475 
12476 }
12477 
12478 } else
12479 {
12480 {
12481 IkReal j3array[1], cj3array[1], sj3array[1];
12482 bool j3valid[1]={false};
12483 _nj3 = 1;
12484 CheckValue<IkReal> x848=IKPowWithIntegerCheck(IKsign(sj4),-1);
12485 if(!x848.valid){
12486 continue;
12487 }
12488 CheckValue<IkReal> x849 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH);
12489 if(!x849.valid){
12490 continue;
12491 }
12492 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x848.value)))+(x849.value));
12493 sj3array[0]=IKsin(j3array[0]);
12494 cj3array[0]=IKcos(j3array[0]);
12495 if( j3array[0] > IKPI )
12496 {
12497  j3array[0]-=IK2PI;
12498 }
12499 else if( j3array[0] < -IKPI )
12500 { j3array[0]+=IK2PI;
12501 }
12502 j3valid[0] = true;
12503 for(int ij3 = 0; ij3 < 1; ++ij3)
12504 {
12505 if( !j3valid[ij3] )
12506 {
12507  continue;
12508 }
12509 _ij3[0] = ij3; _ij3[1] = -1;
12510 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12511 {
12512 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12513 {
12514  j3valid[iij3]=false; _ij3[1] = iij3; break;
12515 }
12516 }
12517 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12518 {
12519 IkReal evalcond[18];
12520 IkReal x850=IKsin(j3);
12521 IkReal x851=IKcos(j3);
12522 IkReal x852=((1.0)*cj5);
12523 IkReal x853=((1.0)*cj4);
12524 IkReal x854=(new_r11*x851);
12525 IkReal x855=(sj5*x851);
12526 IkReal x856=(cj4*x851);
12527 IkReal x857=(sj4*x851);
12528 IkReal x858=(new_r00*x850);
12529 IkReal x859=(cj4*x850);
12530 IkReal x860=(new_r01*x850);
12531 IkReal x861=(new_r02*x850);
12532 IkReal x862=((1.0)*sj4*x850);
12533 evalcond[0]=(new_r12+x857);
12534 evalcond[1]=((((-1.0)*x862))+new_r02);
12535 evalcond[2]=(((new_r12*x850))+((new_r02*x851)));
12536 evalcond[3]=(sj5+((new_r11*x850))+((new_r01*x851)));
12537 evalcond[4]=(sj4+(((-1.0)*x861))+((new_r12*x851)));
12538 evalcond[5]=(new_r01+x855+((cj5*x859)));
12539 evalcond[6]=((((-1.0)*x852))+((new_r10*x850))+((new_r00*x851)));
12540 evalcond[7]=(((sj5*x859))+new_r00+(((-1.0)*x851*x852)));
12541 evalcond[8]=(((sj5*x850))+(((-1.0)*x852*x856))+new_r11);
12542 evalcond[9]=((((-1.0)*x853*x855))+(((-1.0)*x850*x852))+new_r10);
12543 evalcond[10]=((((-1.0)*x858))+(((-1.0)*sj5*x853))+((new_r10*x851)));
12544 evalcond[11]=((((-1.0)*x860))+x854+(((-1.0)*cj4*x852)));
12545 evalcond[12]=((((-1.0)*x853*x861))+((new_r22*sj4))+((new_r12*x856)));
12546 evalcond[13]=((((-1.0)*sj4*x858))+(((-1.0)*new_r20*x853))+((new_r10*x857)));
12547 evalcond[14]=((((-1.0)*new_r21*x853))+((sj4*x854))+(((-1.0)*sj4*x860)));
12548 evalcond[15]=((1.0)+((new_r12*x857))+(((-1.0)*new_r22*x853))+(((-1.0)*sj4*x861)));
12549 evalcond[16]=((((-1.0)*sj5))+(((-1.0)*x853*x858))+((new_r20*sj4))+((new_r10*x856)));
12550 evalcond[17]=((((-1.0)*x852))+(((-1.0)*x853*x860))+((new_r21*sj4))+((cj4*x854)));
12551 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
12552 {
12553 continue;
12554 }
12555 }
12556 
12557 {
12558 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12559 vinfos[0].jointtype = 1;
12560 vinfos[0].foffset = j0;
12561 vinfos[0].indices[0] = _ij0[0];
12562 vinfos[0].indices[1] = _ij0[1];
12563 vinfos[0].maxsolutions = _nj0;
12564 vinfos[1].jointtype = 1;
12565 vinfos[1].foffset = j1;
12566 vinfos[1].indices[0] = _ij1[0];
12567 vinfos[1].indices[1] = _ij1[1];
12568 vinfos[1].maxsolutions = _nj1;
12569 vinfos[2].jointtype = 1;
12570 vinfos[2].foffset = j2;
12571 vinfos[2].indices[0] = _ij2[0];
12572 vinfos[2].indices[1] = _ij2[1];
12573 vinfos[2].maxsolutions = _nj2;
12574 vinfos[3].jointtype = 1;
12575 vinfos[3].foffset = j3;
12576 vinfos[3].indices[0] = _ij3[0];
12577 vinfos[3].indices[1] = _ij3[1];
12578 vinfos[3].maxsolutions = _nj3;
12579 vinfos[4].jointtype = 1;
12580 vinfos[4].foffset = j4;
12581 vinfos[4].indices[0] = _ij4[0];
12582 vinfos[4].indices[1] = _ij4[1];
12583 vinfos[4].maxsolutions = _nj4;
12584 vinfos[5].jointtype = 1;
12585 vinfos[5].foffset = j5;
12586 vinfos[5].indices[0] = _ij5[0];
12587 vinfos[5].indices[1] = _ij5[1];
12588 vinfos[5].maxsolutions = _nj5;
12589 std::vector<int> vfree(0);
12590 solutions.AddSolution(vinfos,vfree);
12591 }
12592 }
12593 }
12594 
12595 }
12596 
12597 }
12598 }
12599 }
12600 
12601 }
12602 
12603 }
12604 }
12605 }
12606 }
12607 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
12608 {
12609  using std::complex;
12610  if( rawcoeffs[0] == 0 ) {
12611  // solve with one reduced degree
12612  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
12613  return;
12614  }
12615  IKFAST_ASSERT(rawcoeffs[0] != 0);
12616  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
12617  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
12618  complex<IkReal> coeffs[3];
12619  const int maxsteps = 110;
12620  for(int i = 0; i < 3; ++i) {
12621  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
12622  }
12623  complex<IkReal> roots[3];
12624  IkReal err[3];
12625  roots[0] = complex<IkReal>(1,0);
12626  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
12627  err[0] = 1.0;
12628  err[1] = 1.0;
12629  for(int i = 2; i < 3; ++i) {
12630  roots[i] = roots[i-1]*roots[1];
12631  err[i] = 1.0;
12632  }
12633  for(int step = 0; step < maxsteps; ++step) {
12634  bool changed = false;
12635  for(int i = 0; i < 3; ++i) {
12636  if ( err[i] >= tol ) {
12637  changed = true;
12638  // evaluate
12639  complex<IkReal> x = roots[i] + coeffs[0];
12640  for(int j = 1; j < 3; ++j) {
12641  x = roots[i] * x + coeffs[j];
12642  }
12643  for(int j = 0; j < 3; ++j) {
12644  if( i != j ) {
12645  if( roots[i] != roots[j] ) {
12646  x /= (roots[i] - roots[j]);
12647  }
12648  }
12649  }
12650  roots[i] -= x;
12651  err[i] = abs(x);
12652  }
12653  }
12654  if( !changed ) {
12655  break;
12656  }
12657  }
12658 
12659  numroots = 0;
12660  bool visited[3] = {false};
12661  for(int i = 0; i < 3; ++i) {
12662  if( !visited[i] ) {
12663  // might be a multiple root, in which case it will have more error than the other roots
12664  // find any neighboring roots, and take the average
12665  complex<IkReal> newroot=roots[i];
12666  int n = 1;
12667  for(int j = i+1; j < 3; ++j) {
12668  // care about error in real much more than imaginary
12669  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
12670  newroot += roots[j];
12671  n += 1;
12672  visited[j] = true;
12673  }
12674  }
12675  if( n > 1 ) {
12676  newroot /= n;
12677  }
12678  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
12679  if( IKabs(imag(newroot)) < tolsqrt ) {
12680  rawroots[numroots++] = real(newroot);
12681  }
12682  }
12683  }
12684 }
12685 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
12686  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
12687  if( det < 0 ) {
12688  numroots=0;
12689  }
12690  else if( det == 0 ) {
12691  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
12692  numroots = 1;
12693  }
12694  else {
12695  det = IKsqrt(det);
12696  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
12697  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
12698  numroots = 2;
12699  }
12700 }
12701 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
12702 {
12703  using std::complex;
12704  if( rawcoeffs[0] == 0 ) {
12705  // solve with one reduced degree
12706  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
12707  return;
12708  }
12709  IKFAST_ASSERT(rawcoeffs[0] != 0);
12710  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
12711  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
12712  complex<IkReal> coeffs[4];
12713  const int maxsteps = 110;
12714  for(int i = 0; i < 4; ++i) {
12715  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
12716  }
12717  complex<IkReal> roots[4];
12718  IkReal err[4];
12719  roots[0] = complex<IkReal>(1,0);
12720  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
12721  err[0] = 1.0;
12722  err[1] = 1.0;
12723  for(int i = 2; i < 4; ++i) {
12724  roots[i] = roots[i-1]*roots[1];
12725  err[i] = 1.0;
12726  }
12727  for(int step = 0; step < maxsteps; ++step) {
12728  bool changed = false;
12729  for(int i = 0; i < 4; ++i) {
12730  if ( err[i] >= tol ) {
12731  changed = true;
12732  // evaluate
12733  complex<IkReal> x = roots[i] + coeffs[0];
12734  for(int j = 1; j < 4; ++j) {
12735  x = roots[i] * x + coeffs[j];
12736  }
12737  for(int j = 0; j < 4; ++j) {
12738  if( i != j ) {
12739  if( roots[i] != roots[j] ) {
12740  x /= (roots[i] - roots[j]);
12741  }
12742  }
12743  }
12744  roots[i] -= x;
12745  err[i] = abs(x);
12746  }
12747  }
12748  if( !changed ) {
12749  break;
12750  }
12751  }
12752 
12753  numroots = 0;
12754  bool visited[4] = {false};
12755  for(int i = 0; i < 4; ++i) {
12756  if( !visited[i] ) {
12757  // might be a multiple root, in which case it will have more error than the other roots
12758  // find any neighboring roots, and take the average
12759  complex<IkReal> newroot=roots[i];
12760  int n = 1;
12761  for(int j = i+1; j < 4; ++j) {
12762  // care about error in real much more than imaginary
12763  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
12764  newroot += roots[j];
12765  n += 1;
12766  visited[j] = true;
12767  }
12768  }
12769  if( n > 1 ) {
12770  newroot /= n;
12771  }
12772  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
12773  if( IKabs(imag(newroot)) < tolsqrt ) {
12774  rawroots[numroots++] = real(newroot);
12775  }
12776  }
12777  }
12778 }
12779 };
12780 
12781 
12784 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
12785 IKSolver solver;
12786 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
12787 }
12788 
12789 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
12790 IKSolver solver;
12791 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
12792 }
12793 
12794 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - rs080n (ca1c438b655ffad7bc13162d3719a526)>"; }
12795 
12796 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
12797 
12798 #ifdef IKFAST_NAMESPACE
12799 } // end namespace
12800 #endif
12801 
12802 #ifndef IKFAST_NO_MAIN
12803 #include <stdio.h>
12804 #include <stdlib.h>
12805 #ifdef IKFAST_NAMESPACE
12806 using namespace IKFAST_NAMESPACE;
12807 #endif
12808 int main(int argc, char** argv)
12809 {
12810  if( argc != 12+GetNumFreeParameters()+1 ) {
12811  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
12812  "Returns the ik solutions given the transformation of the end effector specified by\n"
12813  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
12814  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
12815  return 1;
12816  }
12817 
12818  IkSolutionList<IkReal> solutions;
12819  std::vector<IkReal> vfree(GetNumFreeParameters());
12820  IkReal eerot[9],eetrans[3];
12821  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
12822  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
12823  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
12824  for(std::size_t i = 0; i < vfree.size(); ++i)
12825  vfree[i] = atof(argv[13+i]);
12826  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
12827 
12828  if( !bSuccess ) {
12829  fprintf(stderr,"Failed to get ik solution\n");
12830  return -1;
12831  }
12832 
12833  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
12834  std::vector<IkReal> solvalues(GetNumJoints());
12835  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
12836  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
12837  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
12838  std::vector<IkReal> vsolfree(sol.GetFree().size());
12839  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
12840  for( std::size_t j = 0; j < solvalues.size(); ++j)
12841  printf("%.15f, ", solvalues[j]);
12842  printf("\n");
12843  }
12844  return 0;
12845 }
12846 
12847 #endif
ikfast
Definition: ikfast.h:45
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: rs080n_manipulator_ikfast_solver.cpp:246
IKsin
float IKsin(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:168
IKcos
float IKcos(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:170
IKPI
#define IKPI
Definition: rs080n_manipulator_ikfast_solver.cpp:60
IKsqr
float IKsqr(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:94
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: rs080n_manipulator_ikfast_solver.cpp:102
IKsqrt
float IKsqrt(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:174
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: rs080n_manipulator_ikfast_solver.cpp:299
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IKfmod
float IKfmod(float x, float y)
Definition: rs080n_manipulator_ikfast_solver.cpp:137
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
IKlog
float IKlog(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:97
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: rs080n_manipulator_ikfast_solver.cpp:12794
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: rs080n_manipulator_ikfast_solver.cpp:117
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IK2PI
#define IK2PI
Definition: rs080n_manipulator_ikfast_solver.cpp:59
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: rs080n_manipulator_ikfast_solver.cpp:107
IKabs
float IKabs(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:91
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: rs080n_manipulator_ikfast_solver.cpp:361
IKPI_2
#define IKPI_2
Definition: rs080n_manipulator_ikfast_solver.cpp:61
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: rs080n_manipulator_ikfast_solver.cpp:787
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: rs080n_manipulator_ikfast_solver.cpp:211
IKatan2
float IKatan2(float fy, float fx)
Definition: rs080n_manipulator_ikfast_solver.cpp:179
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: rs080n_manipulator_ikfast_solver.cpp:362
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
IKasin
float IKasin(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:121
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: rs080n_manipulator_ikfast_solver.cpp:12607
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: rs080n_manipulator_ikfast_solver.cpp:112
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: rs080n_manipulator_ikfast_solver.cpp:360
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: rs080n_manipulator_ikfast_solver.cpp:25
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: rs080n_manipulator_ikfast_solver.cpp:375
pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: rs080n_manipulator_ikfast_solver.cpp:12701
step
unsigned int step
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
dgetrs_
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: rs080n_manipulator_ikfast_solver.cpp:364
IKsign
float IKsign(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:225
main
int main(int argc, char **argv)
Definition: rs080n_manipulator_ikfast_solver.cpp:12808
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
IKacos
float IKacos(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:154
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: rs080n_manipulator_ikfast_solver.cpp:12796
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: rs080n_manipulator_ikfast_solver.cpp:49
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: rs080n_manipulator_ikfast_solver.cpp:12685
IKtan
float IKtan(float f)
Definition: rs080n_manipulator_ikfast_solver.cpp:172
x
double x
GetIkType
IKFAST_API int GetIkType()
Definition: rs080n_manipulator_ikfast_solver.cpp:366
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: rs080n_manipulator_ikfast_solver.cpp:176
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: rs080n_manipulator_ikfast_solver.cpp:12789
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: rs080n_manipulator_ikfast_solver.cpp:12784

khi_rs_ikfast_plugin
Author(s): matsui_hiro
autogenerated on Fri Mar 26 2021 02:34:44