fanuc_m20ib25_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
23 
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
27 
28 #include <cmath>
29 #include <vector>
30 #include <limits>
31 #include <algorithm>
32 #include <complex>
33 
34 #ifndef IKFAST_ASSERT
35 #include <stdexcept>
36 #include <sstream>
37 #include <iostream>
38 
39 #ifdef _MSC_VER
40 #ifndef __PRETTY_FUNCTION__
41 #define __PRETTY_FUNCTION__ __FUNCDNAME__
42 #endif
43 #endif
44 
45 #ifndef __PRETTY_FUNCTION__
46 #define __PRETTY_FUNCTION__ __func__
47 #endif
48 
49 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
50 
51 #endif
52 
53 #if defined(_MSC_VER)
54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
55 #else
56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
57 #endif
58 
59 #define IK2PI ((IkReal)6.28318530717959)
60 #define IKPI ((IkReal)3.14159265358979)
61 #define IKPI_2 ((IkReal)1.57079632679490)
62 
63 #ifdef _MSC_VER
64 #ifndef isnan
65 #define isnan _isnan
66 #endif
67 #ifndef isinf
68 #define isinf _isinf
69 #endif
70 //#ifndef isfinite
71 //#define isfinite _isfinite
72 //#endif
73 #endif // _MSC_VER
74 
75 // lapack routines
76 extern "C" {
77  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
78  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
79  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
80  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
81  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
82  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
83 }
84 
85 using namespace std; // necessary to get std math routines
86 
87 #ifdef IKFAST_NAMESPACE
88 namespace IKFAST_NAMESPACE {
89 #endif
90 
91 inline float IKabs(float f) { return fabsf(f); }
92 inline double IKabs(double f) { return fabs(f); }
93 
94 inline float IKsqr(float f) { return f*f; }
95 inline double IKsqr(double f) { return f*f; }
96 
97 inline float IKlog(float f) { return logf(f); }
98 inline double IKlog(double f) { return log(f); }
99 
100 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
101 #ifndef IKFAST_SINCOS_THRESH
102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
103 #endif
104 
105 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
106 #ifndef IKFAST_ATAN2_MAGTHRESH
107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
108 #endif
109 
110 // minimum distance of separate solutions
111 #ifndef IKFAST_SOLUTION_THRESH
112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
113 #endif
114 
115 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
116 #ifndef IKFAST_EVALCOND_THRESH
117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
118 #endif
119 
120 
121 inline float IKasin(float f)
122 {
123 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
124 if( f <= -1 ) return float(-IKPI_2);
125 else if( f >= 1 ) return float(IKPI_2);
126 return asinf(f);
127 }
128 inline double IKasin(double f)
129 {
130 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
131 if( f <= -1 ) return -IKPI_2;
132 else if( f >= 1 ) return IKPI_2;
133 return asin(f);
134 }
135 
136 // return positive value in [0,y)
137 inline float IKfmod(float x, float y)
138 {
139  while(x < 0) {
140  x += y;
141  }
142  return fmodf(x,y);
143 }
144 
145 // return positive value in [0,y)
146 inline double IKfmod(double x, double y)
147 {
148  while(x < 0) {
149  x += y;
150  }
151  return fmod(x,y);
152 }
153 
154 inline float IKacos(float f)
155 {
156 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
157 if( f <= -1 ) return float(IKPI);
158 else if( f >= 1 ) return float(0);
159 return acosf(f);
160 }
161 inline double IKacos(double f)
162 {
163 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
164 if( f <= -1 ) return IKPI;
165 else if( f >= 1 ) return 0;
166 return acos(f);
167 }
168 inline float IKsin(float f) { return sinf(f); }
169 inline double IKsin(double f) { return sin(f); }
170 inline float IKcos(float f) { return cosf(f); }
171 inline double IKcos(double f) { return cos(f); }
172 inline float IKtan(float f) { return tanf(f); }
173 inline double IKtan(double f) { return tan(f); }
174 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
175 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
176 inline float IKatan2Simple(float fy, float fx) {
177  return atan2f(fy,fx);
178 }
179 inline float IKatan2(float fy, float fx) {
180  if( isnan(fy) ) {
181  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
182  return float(IKPI_2);
183  }
184  else if( isnan(fx) ) {
185  return 0;
186  }
187  return atan2f(fy,fx);
188 }
189 inline double IKatan2Simple(double fy, double fx) {
190  return atan2(fy,fx);
191 }
192 inline double IKatan2(double fy, double fx) {
193  if( isnan(fy) ) {
194  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
195  return IKPI_2;
196  }
197  else if( isnan(fx) ) {
198  return 0;
199  }
200  return atan2(fy,fx);
201 }
202 
203 template <typename T>
204 struct CheckValue
205 {
206  T value;
207  bool valid;
208 };
209 
210 template <typename T>
211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
212 {
213  CheckValue<T> ret;
214  ret.valid = false;
215  ret.value = 0;
216  if( !isnan(fy) && !isnan(fx) ) {
217  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
218  ret.value = IKatan2Simple(fy,fx);
219  ret.valid = true;
220  }
221  }
222  return ret;
223 }
224 
225 inline float IKsign(float f) {
226  if( f > 0 ) {
227  return float(1);
228  }
229  else if( f < 0 ) {
230  return float(-1);
231  }
232  return 0;
233 }
234 
235 inline double IKsign(double f) {
236  if( f > 0 ) {
237  return 1.0;
238  }
239  else if( f < 0 ) {
240  return -1.0;
241  }
242  return 0;
243 }
244 
245 template <typename T>
246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
247 {
248  CheckValue<T> ret;
249  ret.valid = true;
250  if( n == 0 ) {
251  ret.value = 1.0;
252  return ret;
253  }
254  else if( n == 1 )
255  {
256  ret.value = f;
257  return ret;
258  }
259  else if( n < 0 )
260  {
261  if( f == 0 )
262  {
263  ret.valid = false;
264  ret.value = (T)1.0e30;
265  return ret;
266  }
267  if( n == -1 ) {
268  ret.value = T(1.0)/f;
269  return ret;
270  }
271  }
272 
273  int num = n > 0 ? n : -n;
274  if( num == 2 ) {
275  ret.value = f*f;
276  }
277  else if( num == 3 ) {
278  ret.value = f*f*f;
279  }
280  else {
281  ret.value = 1.0;
282  while(num>0) {
283  if( num & 1 ) {
284  ret.value *= f;
285  }
286  num >>= 1;
287  f *= f;
288  }
289  }
290 
291  if( n < 0 ) {
292  ret.value = T(1.0)/ret.value;
293  }
294  return ret;
295 }
296 
299 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
300 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44;
301 x0=IKcos(j[0]);
302 x1=IKcos(j[3]);
303 x2=IKcos(j[2]);
304 x3=IKsin(j[1]);
305 x4=IKsin(j[0]);
306 x5=IKsin(j[3]);
307 x6=IKcos(j[1]);
308 x7=IKsin(j[2]);
309 x8=IKsin(j[5]);
310 x9=IKcos(j[5]);
311 x10=IKcos(j[4]);
312 x11=IKsin(j[4]);
313 x12=((0.865)*x4);
314 x13=((0.12)*x2);
315 x14=((0.1)*x1);
316 x15=((0.1)*x4);
317 x16=((0.1)*x7);
318 x17=((1.0)*x5);
319 x18=((1.0)*x4);
320 x19=((1.0)*x2);
321 x20=((0.1)*x0);
322 x21=((1.0)*x0);
323 x22=(x2*x6);
324 x23=(x0*x3);
325 x24=(x3*x7);
326 x25=(x3*x4);
327 x26=(x6*x7);
328 x27=(x2*x3);
329 x28=(x1*x18);
330 x29=(x19*x3);
331 x30=(x21*x26);
332 x31=(x18*x26);
333 x32=(x24+x22);
334 x33=(x1*x32);
335 x34=((((-1.0)*x30))+((x2*x23)));
336 x35=((((-1.0)*x31))+((x2*x25)));
337 x36=((((-1.0)*x19*x23))+x30);
338 x37=((((-1.0)*x18*x27))+x31);
339 x38=(x1*x34);
340 x39=(x37*x5);
341 x40=((((-1.0)*x17*x4))+x38);
342 x41=(((x0*x5))+((x1*x35)));
343 x42=(((x10*x33))+((x11*(((((-1.0)*x26))+x29)))));
344 x43=(((x10*x40))+((x11*(((((-1.0)*x21*x24))+(((-1.0)*x0*x19*x6)))))));
345 IkReal x45=((1.0)*x18);
346 x44=(((x10*x41))+((x11*(((((-1.0)*x22*x45))+(((-1.0)*x24*x45)))))));
347 eerot[0]=(((x8*(((((-1.0)*x28))+((x36*x5))))))+((x43*x9)));
348 eerot[1]=(((x43*x8))+((x9*(((((-1.0)*x17*x36))+x28)))));
349 eerot[2]=(((x11*x40))+((x10*((((x0*x22))+((x23*x7)))))));
350 eetrans[0]=((((0.865)*x23*x7))+(((-0.12)*x0*x26))+((x11*((((x14*x34))+(((-1.0)*x15*x5))))))+(((0.905)*x23))+((x10*((((x16*x23))+((x20*x22))))))+(((0.075)*x0))+((x13*x23))+(((0.865)*x0*x22)));
351 eerot[3]=(((x44*x9))+((x8*((((x0*x1))+x39)))));
352 eerot[4]=(((x44*x8))+((x9*(((((-1.0)*x1*x21))+(((-1.0)*x17*x37)))))));
353 eerot[5]=(((x11*x41))+((x10*((((x22*x4))+((x24*x4)))))));
354 eetrans[1]=((((-0.12)*x26*x4))+((x11*((((x14*x35))+((x20*x5))))))+(((0.905)*x25))+((x10*((((x15*x24))+((x15*x22))))))+(((0.075)*x4))+((x12*x24))+((x12*x22))+((x13*x25)));
355 eerot[6]=(((x42*x9))+((x5*x8*(((((-1.0)*x19*x6))+(((-1.0)*x24)))))));
356 eerot[7]=(((x42*x8))+((x32*x5*x9)));
357 eerot[8]=(((x10*(((((-1.0)*x29))+x26))))+((x11*x33)));
358 eetrans[2]=((0.65)+(((0.12)*x24))+(((0.865)*x26))+(((0.905)*x6))+((x10*(((((-0.1)*x27))+((x16*x6))))))+(((-0.865)*x27))+((x13*x6))+((x1*x11*((((x16*x3))+(((0.1)*x22)))))));
359 }
360 
361 IKFAST_API int GetNumFreeParameters() { return 0; }
362 IKFAST_API int* GetFreeParameters() { return NULL; }
363 IKFAST_API int GetNumJoints() { return 6; }
364 
365 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
366 
367 IKFAST_API int GetIkType() { return 0x67000001; }
368 
369 class IKSolver {
370 public:
371 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
372 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
373 
374 IkReal j100, cj100, sj100;
375 unsigned char _ij100[2], _nj100;
376 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
377 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
378 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
379  solutions.Clear();
380 r00 = eerot[0*3+0];
381 r01 = eerot[0*3+1];
382 r02 = eerot[0*3+2];
383 r10 = eerot[1*3+0];
384 r11 = eerot[1*3+1];
385 r12 = eerot[1*3+2];
386 r20 = eerot[2*3+0];
387 r21 = eerot[2*3+1];
388 r22 = eerot[2*3+2];
389 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
390 
391 new_r00=r00;
392 new_r01=((-1.0)*r01);
393 new_r02=((-1.0)*r02);
394 new_px=(px+(((-0.1)*r02)));
395 new_r10=r10;
396 new_r11=((-1.0)*r11);
397 new_r12=((-1.0)*r12);
398 new_py=(py+(((-0.1)*r12)));
399 new_r20=r20;
400 new_r21=((-1.0)*r21);
401 new_r22=((-1.0)*r22);
402 new_pz=((-0.65)+(((-0.1)*r22))+pz);
403 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
404 IkReal x46=((1.0)*px);
405 IkReal x47=((1.0)*pz);
406 IkReal x48=((1.0)*py);
407 pp=((px*px)+(py*py)+(pz*pz));
408 npx=(((px*r00))+((py*r10))+((pz*r20)));
409 npy=(((px*r01))+((py*r11))+((pz*r21)));
410 npz=(((px*r02))+((py*r12))+((pz*r22)));
411 rxp0_0=((((-1.0)*r20*x48))+((pz*r10)));
412 rxp0_1=(((px*r20))+(((-1.0)*r00*x47)));
413 rxp0_2=((((-1.0)*r10*x46))+((py*r00)));
414 rxp1_0=((((-1.0)*r21*x48))+((pz*r11)));
415 rxp1_1=(((px*r21))+(((-1.0)*r01*x47)));
416 rxp1_2=((((-1.0)*r11*x46))+((py*r01)));
417 rxp2_0=((((-1.0)*r22*x48))+((pz*r12)));
418 rxp2_1=((((-1.0)*r02*x47))+((px*r22)));
419 rxp2_2=((((-1.0)*r12*x46))+((py*r02)));
420 {
421 IkReal j0eval[1];
422 j0eval[0]=((IKabs(px))+(IKabs(py)));
423 if( IKabs(j0eval[0]) < 0.0000010000000000 )
424 {
425 continue; // no branches [j0, j1, j2]
426 
427 } else
428 {
429 {
430 IkReal j0array[2], cj0array[2], sj0array[2];
431 bool j0valid[2]={false};
432 _nj0 = 2;
433 CheckValue<IkReal> x50 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
434 if(!x50.valid){
435 continue;
436 }
437 IkReal x49=x50.value;
438 j0array[0]=((-1.0)*x49);
439 sj0array[0]=IKsin(j0array[0]);
440 cj0array[0]=IKcos(j0array[0]);
441 j0array[1]=((3.14159265358979)+(((-1.0)*x49)));
442 sj0array[1]=IKsin(j0array[1]);
443 cj0array[1]=IKcos(j0array[1]);
444 if( j0array[0] > IKPI )
445 {
446  j0array[0]-=IK2PI;
447 }
448 else if( j0array[0] < -IKPI )
449 { j0array[0]+=IK2PI;
450 }
451 j0valid[0] = true;
452 if( j0array[1] > IKPI )
453 {
454  j0array[1]-=IK2PI;
455 }
456 else if( j0array[1] < -IKPI )
457 { j0array[1]+=IK2PI;
458 }
459 j0valid[1] = true;
460 for(int ij0 = 0; ij0 < 2; ++ij0)
461 {
462 if( !j0valid[ij0] )
463 {
464  continue;
465 }
466 _ij0[0] = ij0; _ij0[1] = -1;
467 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
468 {
469 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
470 {
471  j0valid[iij0]=false; _ij0[1] = iij0; break;
472 }
473 }
474 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
475 
476 {
477 IkReal j2array[2], cj2array[2], sj2array[2];
478 bool j2valid[2]={false};
479 _nj2 = 2;
480 if( (((0.997077711951907)+(((-0.632653487065184)*pp))+(((0.0948980230597776)*py*sj0))+(((0.0948980230597776)*cj0*px)))) < -1-IKFAST_SINCOS_THRESH || (((0.997077711951907)+(((-0.632653487065184)*pp))+(((0.0948980230597776)*py*sj0))+(((0.0948980230597776)*cj0*px)))) > 1+IKFAST_SINCOS_THRESH )
481  continue;
482 IkReal x51=IKasin(((0.997077711951907)+(((-0.632653487065184)*pp))+(((0.0948980230597776)*py*sj0))+(((0.0948980230597776)*cj0*px))));
483 j2array[0]=((-0.137848493728136)+(((-1.0)*x51)));
484 sj2array[0]=IKsin(j2array[0]);
485 cj2array[0]=IKcos(j2array[0]);
486 j2array[1]=((3.00374415986166)+x51);
487 sj2array[1]=IKsin(j2array[1]);
488 cj2array[1]=IKcos(j2array[1]);
489 if( j2array[0] > IKPI )
490 {
491  j2array[0]-=IK2PI;
492 }
493 else if( j2array[0] < -IKPI )
494 { j2array[0]+=IK2PI;
495 }
496 j2valid[0] = true;
497 if( j2array[1] > IKPI )
498 {
499  j2array[1]-=IK2PI;
500 }
501 else if( j2array[1] < -IKPI )
502 { j2array[1]+=IK2PI;
503 }
504 j2valid[1] = true;
505 for(int ij2 = 0; ij2 < 2; ++ij2)
506 {
507 if( !j2valid[ij2] )
508 {
509  continue;
510 }
511 _ij2[0] = ij2; _ij2[1] = -1;
512 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
513 {
514 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
515 {
516  j2valid[iij2]=false; _ij2[1] = iij2; break;
517 }
518 }
519 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
520 
521 {
522 IkReal j1eval[3];
523 IkReal x52=cj2*cj2;
524 IkReal x53=(py*sj0);
525 IkReal x54=((0.865)*cj2);
526 IkReal x55=(cj0*px);
527 IkReal x56=(cj2*pz);
528 IkReal x57=(pz*sj2);
529 IkReal x58=((13.3333333333333)*sj2);
530 IkReal x59=((96.1111111111111)*cj2);
531 IkReal x60=((0.12)*sj2);
532 IkReal x61=(cj2*sj2);
533 j1eval[0]=((((-1.0)*x53*x59))+(((96.1111111111111)*x57))+(((-1.0)*sj2))+(((13.3333333333333)*x56))+(((100.555555555556)*pz))+(((-1.0)*x55*x59))+(((7.20833333333333)*cj2))+((x53*x58))+((x55*x58)));
534 j1eval[1]=((IKabs(((0.1038)+(((-0.075)*pz))+(((-0.733825)*x61))+(((-0.2076)*x52))+((pz*x55))+((pz*x53))+(((0.1086)*sj2))+(((-0.782825)*cj2)))))+(IKabs(((-0.0144)+(((0.2076)*x61))+(((-0.733825)*x52))+(pz*pz)))));
535 j1eval[2]=IKsign(((((-1.0)*x53*x54))+(((0.865)*x57))+(((0.064875)*cj2))+(((0.905)*pz))+(((0.12)*x56))+(((-1.0)*x54*x55))+((x53*x60))+(((-0.009)*sj2))+((x55*x60))));
536 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
537 {
538 {
539 IkReal j1eval[2];
540 IkReal x62=(pz*sj2);
541 IkReal x63=(py*sj0);
542 IkReal x64=((96.1111111111111)*sj2);
543 IkReal x65=(cj0*px);
544 IkReal x66=((0.865)*sj2);
545 IkReal x67=(cj2*pz);
546 IkReal x68=((13.3333333333333)*cj2);
547 IkReal x69=((0.12)*cj2);
548 j1eval[0]=((7.54166666666667)+cj2+(((7.20833333333333)*sj2))+(((13.3333333333333)*x62))+(((-96.1111111111111)*x67))+(((-1.0)*x64*x65))+(((-100.555555555556)*x63))+(((-100.555555555556)*x65))+(((-1.0)*x65*x68))+(((-1.0)*x63*x68))+(((-1.0)*x63*x64)));
549 j1eval[1]=IKsign(((0.067875)+(((0.009)*cj2))+(((0.12)*x62))+(((0.064875)*sj2))+(((-0.905)*x65))+(((-0.905)*x63))+(((-1.0)*x65*x66))+(((-1.0)*x65*x69))+(((-1.0)*x63*x69))+(((-1.0)*x63*x66))+(((-0.865)*x67))));
550 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
551 {
552 {
553 IkReal j1eval[2];
554 IkReal x70=cj0*cj0;
555 IkReal x71=py*py;
556 IkReal x72=px*px;
557 IkReal x73=pz*pz;
558 IkReal x74=(py*sj0);
559 IkReal x75=(cj0*px);
560 IkReal x76=(x70*x71);
561 IkReal x77=(x70*x72);
562 IkReal x78=(x74*x75);
563 j1eval[0]=((-1.0)+(((-355.555555555556)*x78))+(((-177.777777777778)*x77))+(((-177.777777777778)*x71))+(((-177.777777777778)*x73))+(((26.6666666666667)*x74))+(((26.6666666666667)*x75))+(((177.777777777778)*x76)));
564 j1eval[1]=IKsign(((-0.005625)+(((0.15)*x75))+(((0.15)*x74))+(((-2.0)*x78))+x76+(((-1.0)*x73))+(((-1.0)*x71))+(((-1.0)*x77))));
565 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
566 {
567 continue; // no branches [j1]
568 
569 } else
570 {
571 {
572 IkReal j1array[1], cj1array[1], sj1array[1];
573 bool j1valid[1]={false};
574 _nj1 = 1;
575 IkReal x79=py*py;
576 IkReal x80=cj0*cj0;
577 IkReal x81=(py*sj0);
578 IkReal x82=((0.865)*cj2);
579 IkReal x83=((0.12)*cj2);
580 IkReal x84=((0.865)*sj2);
581 IkReal x85=(cj0*px);
582 IkReal x86=((0.12)*sj2);
583 CheckValue<IkReal> x87=IKPowWithIntegerCheck(IKsign(((-0.005625)+((x79*x80))+(((-1.0)*x80*(px*px)))+(((0.15)*x85))+(((0.15)*x81))+(((-2.0)*x81*x85))+(((-1.0)*(pz*pz)))+(((-1.0)*x79)))),-1);
584 if(!x87.valid){
585 continue;
586 }
587 CheckValue<IkReal> x88 = IKatan2WithCheck(IkReal(((0.067875)+(((-1.0)*x81*x83))+(((-1.0)*x81*x84))+(((-1.0)*x83*x85))+(((-0.905)*x81))+(((-0.905)*x85))+(((0.009)*cj2))+(((0.064875)*sj2))+((pz*x82))+(((-1.0)*x84*x85))+(((-1.0)*pz*x86)))),IkReal(((((-1.0)*x81*x82))+(((-1.0)*x82*x85))+(((0.064875)*cj2))+((x81*x86))+((x85*x86))+(((-0.905)*pz))+(((-0.009)*sj2))+(((-1.0)*pz*x84))+(((-1.0)*pz*x83)))),IKFAST_ATAN2_MAGTHRESH);
588 if(!x88.valid){
589 continue;
590 }
591 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x87.value)))+(x88.value));
592 sj1array[0]=IKsin(j1array[0]);
593 cj1array[0]=IKcos(j1array[0]);
594 if( j1array[0] > IKPI )
595 {
596  j1array[0]-=IK2PI;
597 }
598 else if( j1array[0] < -IKPI )
599 { j1array[0]+=IK2PI;
600 }
601 j1valid[0] = true;
602 for(int ij1 = 0; ij1 < 1; ++ij1)
603 {
604 if( !j1valid[ij1] )
605 {
606  continue;
607 }
608 _ij1[0] = ij1; _ij1[1] = -1;
609 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
610 {
611 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
612 {
613  j1valid[iij1]=false; _ij1[1] = iij1; break;
614 }
615 }
616 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
617 {
618 IkReal evalcond[5];
619 IkReal x89=IKcos(j1);
620 IkReal x90=IKsin(j1);
621 IkReal x91=((0.865)*cj2);
622 IkReal x92=((0.865)*sj2);
623 IkReal x93=((0.12)*cj2);
624 IkReal x94=(py*sj0);
625 IkReal x95=((0.12)*sj2);
626 IkReal x96=(cj0*px);
627 IkReal x97=((1.0)*x96);
628 IkReal x98=((1.0)*x89);
629 IkReal x99=((1.81)*x90);
630 evalcond[0]=((((0.075)*x89))+(((-1.0)*x94*x98))+((pz*x90))+x91+(((-1.0)*x95))+(((-1.0)*x89*x97)));
631 evalcond[1]=((((-1.0)*x90*x95))+(((-0.905)*x89))+((x90*x91))+pz+(((-1.0)*x89*x92))+(((-1.0)*x89*x93)));
632 evalcond[2]=((0.905)+(((-1.0)*pz*x98))+(((0.075)*x90))+(((-1.0)*x90*x97))+(((-1.0)*x90*x94))+x93+x92);
633 evalcond[3]=((-0.062025)+((x96*x99))+(((1.81)*pz*x89))+(((0.15)*x94))+(((0.15)*x96))+(((-1.0)*pp))+(((-0.13575)*x90))+((x94*x99)));
634 evalcond[4]=((0.075)+((x89*x91))+((x90*x93))+((x90*x92))+(((0.905)*x90))+(((-1.0)*x94))+(((-1.0)*x97))+(((-1.0)*x89*x95)));
635 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
636 {
637 continue;
638 }
639 }
640 
641 rotationfunction0(solutions);
642 }
643 }
644 
645 }
646 
647 }
648 
649 } else
650 {
651 {
652 IkReal j1array[1], cj1array[1], sj1array[1];
653 bool j1valid[1]={false};
654 _nj1 = 1;
655 IkReal x1519=cj2*cj2;
656 IkReal x1520=(py*sj0);
657 IkReal x1521=((0.865)*sj2);
658 IkReal x1522=((0.12)*cj2);
659 IkReal x1523=(cj0*px);
660 IkReal x1524=(cj2*sj2);
661 IkReal x1525=((1.0)*pz);
662 CheckValue<IkReal> x1526 = IKatan2WithCheck(IkReal(((-1.56725)+(((-0.2076)*x1524))+(((0.733825)*x1519))+(((-1.56565)*sj2))+(pz*pz)+(((-0.2172)*cj2)))),IkReal(((0.1038)+(((-0.2076)*x1519))+(((-1.0)*x1523*x1525))+(((0.1086)*sj2))+(((-0.782825)*cj2))+(((-1.0)*x1520*x1525))+(((0.075)*pz))+(((-0.733825)*x1524)))),IKFAST_ATAN2_MAGTHRESH);
663 if(!x1526.valid){
664 continue;
665 }
666 CheckValue<IkReal> x1527=IKPowWithIntegerCheck(IKsign(((0.067875)+(((0.12)*pz*sj2))+(((-0.865)*cj2*pz))+(((0.009)*cj2))+(((0.064875)*sj2))+(((-0.905)*x1523))+(((-0.905)*x1520))+(((-1.0)*x1522*x1523))+(((-1.0)*x1520*x1521))+(((-1.0)*x1520*x1522))+(((-1.0)*x1521*x1523)))),-1);
667 if(!x1527.valid){
668 continue;
669 }
670 j1array[0]=((-1.5707963267949)+(x1526.value)+(((1.5707963267949)*(x1527.value))));
671 sj1array[0]=IKsin(j1array[0]);
672 cj1array[0]=IKcos(j1array[0]);
673 if( j1array[0] > IKPI )
674 {
675  j1array[0]-=IK2PI;
676 }
677 else if( j1array[0] < -IKPI )
678 { j1array[0]+=IK2PI;
679 }
680 j1valid[0] = true;
681 for(int ij1 = 0; ij1 < 1; ++ij1)
682 {
683 if( !j1valid[ij1] )
684 {
685  continue;
686 }
687 _ij1[0] = ij1; _ij1[1] = -1;
688 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
689 {
690 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
691 {
692  j1valid[iij1]=false; _ij1[1] = iij1; break;
693 }
694 }
695 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
696 {
697 IkReal evalcond[5];
698 IkReal x1528=IKcos(j1);
699 IkReal x1529=IKsin(j1);
700 IkReal x1530=((0.865)*cj2);
701 IkReal x1531=((0.865)*sj2);
702 IkReal x1532=((0.12)*cj2);
703 IkReal x1533=(py*sj0);
704 IkReal x1534=((0.12)*sj2);
705 IkReal x1535=(cj0*px);
706 IkReal x1536=((1.0)*x1535);
707 IkReal x1537=((1.0)*x1528);
708 IkReal x1538=((1.81)*x1529);
709 evalcond[0]=((((-1.0)*x1534))+x1530+(((-1.0)*x1533*x1537))+(((-1.0)*x1528*x1536))+((pz*x1529))+(((0.075)*x1528)));
710 evalcond[1]=((((-1.0)*x1529*x1534))+(((-0.905)*x1528))+((x1529*x1530))+pz+(((-1.0)*x1528*x1531))+(((-1.0)*x1528*x1532)));
711 evalcond[2]=((0.905)+(((-1.0)*x1529*x1533))+(((-1.0)*x1529*x1536))+x1532+x1531+(((-1.0)*pz*x1537))+(((0.075)*x1529)));
712 evalcond[3]=((-0.062025)+(((-0.13575)*x1529))+(((0.15)*x1535))+(((0.15)*x1533))+(((-1.0)*pp))+(((1.81)*pz*x1528))+((x1535*x1538))+((x1533*x1538)));
713 evalcond[4]=((0.075)+(((-1.0)*x1533))+(((0.905)*x1529))+((x1528*x1530))+(((-1.0)*x1536))+((x1529*x1531))+((x1529*x1532))+(((-1.0)*x1528*x1534)));
714 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
715 {
716 continue;
717 }
718 }
719 
720 rotationfunction0(solutions);
721 }
722 }
723 
724 }
725 
726 }
727 
728 } else
729 {
730 {
731 IkReal j1array[1], cj1array[1], sj1array[1];
732 bool j1valid[1]={false};
733 _nj1 = 1;
734 IkReal x1539=cj2*cj2;
735 IkReal x1540=(cj2*sj2);
736 IkReal x1541=(py*sj0);
737 IkReal x1542=((0.865)*cj2);
738 IkReal x1543=((0.12)*sj2);
739 IkReal x1544=(cj0*px);
740 CheckValue<IkReal> x1545 = IKatan2WithCheck(IkReal(((0.1038)+(((-0.733825)*x1540))+(((-0.075)*pz))+(((-0.2076)*x1539))+(((0.1086)*sj2))+((pz*x1541))+((pz*x1544))+(((-0.782825)*cj2)))),IkReal(((-0.0144)+(((0.2076)*x1540))+(pz*pz)+(((-0.733825)*x1539)))),IKFAST_ATAN2_MAGTHRESH);
741 if(!x1545.valid){
742 continue;
743 }
744 CheckValue<IkReal> x1546=IKPowWithIntegerCheck(IKsign(((((0.064875)*cj2))+((x1541*x1543))+(((0.905)*pz))+(((0.865)*pz*sj2))+(((0.12)*cj2*pz))+(((-0.009)*sj2))+((x1543*x1544))+(((-1.0)*x1542*x1544))+(((-1.0)*x1541*x1542)))),-1);
745 if(!x1546.valid){
746 continue;
747 }
748 j1array[0]=((-1.5707963267949)+(x1545.value)+(((1.5707963267949)*(x1546.value))));
749 sj1array[0]=IKsin(j1array[0]);
750 cj1array[0]=IKcos(j1array[0]);
751 if( j1array[0] > IKPI )
752 {
753  j1array[0]-=IK2PI;
754 }
755 else if( j1array[0] < -IKPI )
756 { j1array[0]+=IK2PI;
757 }
758 j1valid[0] = true;
759 for(int ij1 = 0; ij1 < 1; ++ij1)
760 {
761 if( !j1valid[ij1] )
762 {
763  continue;
764 }
765 _ij1[0] = ij1; _ij1[1] = -1;
766 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
767 {
768 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
769 {
770  j1valid[iij1]=false; _ij1[1] = iij1; break;
771 }
772 }
773 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
774 {
775 IkReal evalcond[5];
776 IkReal x1547=IKcos(j1);
777 IkReal x1548=IKsin(j1);
778 IkReal x1549=((0.865)*cj2);
779 IkReal x1550=((0.865)*sj2);
780 IkReal x1551=((0.12)*cj2);
781 IkReal x1552=(py*sj0);
782 IkReal x1553=((0.12)*sj2);
783 IkReal x1554=(cj0*px);
784 IkReal x1555=((1.0)*x1554);
785 IkReal x1556=((1.0)*x1547);
786 IkReal x1557=((1.81)*x1548);
787 evalcond[0]=((((0.075)*x1547))+(((-1.0)*x1552*x1556))+x1549+(((-1.0)*x1553))+((pz*x1548))+(((-1.0)*x1547*x1555)));
788 evalcond[1]=((((-0.905)*x1547))+(((-1.0)*x1548*x1553))+pz+((x1548*x1549))+(((-1.0)*x1547*x1550))+(((-1.0)*x1547*x1551)));
789 evalcond[2]=((0.905)+(((-1.0)*pz*x1556))+(((0.075)*x1548))+(((-1.0)*x1548*x1552))+(((-1.0)*x1548*x1555))+x1551+x1550);
790 evalcond[3]=((-0.062025)+((x1552*x1557))+(((1.81)*pz*x1547))+(((-0.13575)*x1548))+(((-1.0)*pp))+(((0.15)*x1552))+(((0.15)*x1554))+((x1554*x1557)));
791 evalcond[4]=((0.075)+(((-1.0)*x1552))+((x1547*x1549))+(((0.905)*x1548))+((x1548*x1551))+((x1548*x1550))+(((-1.0)*x1555))+(((-1.0)*x1547*x1553)));
792 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
793 {
794 continue;
795 }
796 }
797 
798 rotationfunction0(solutions);
799 }
800 }
801 
802 }
803 
804 }
805 }
806 }
807 }
808 }
809 
810 }
811 
812 }
813 }
814 return solutions.GetNumSolutions()>0;
815 }
816 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
817 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
818 IkReal x100=(r11*sj0);
819 IkReal x101=(cj0*r02);
820 IkReal x102=(sj1*sj2);
821 IkReal x103=(r10*sj0);
822 IkReal x104=((1.0)*cj1);
823 IkReal x105=((1.0)*sj0);
824 IkReal x106=(cj0*r00);
825 IkReal x107=(r12*sj0);
826 IkReal x108=(cj0*r01);
827 IkReal x109=((((-1.0)*sj2*x104))+((cj2*sj1)));
828 IkReal x110=(x102+((cj1*cj2)));
829 IkReal x111=(sj0*x109);
830 IkReal x112=(cj0*x109);
831 IkReal x113=((((-1.0)*cj2*x104))+(((-1.0)*x102)));
832 new_r00=(((x106*x109))+((x103*x109))+((r20*x110)));
833 new_r01=(((r21*x110))+((x108*x109))+((x100*x109)));
834 new_r02=(((x107*x109))+((r22*x110))+((x101*x109)));
835 new_r10=((((-1.0)*r00*x105))+((cj0*r10)));
836 new_r11=((((-1.0)*r01*x105))+((cj0*r11)));
837 new_r12=((((-1.0)*r02*x105))+((cj0*r12)));
838 new_r20=(((x106*x113))+((x103*x113))+((r20*x109)));
839 new_r21=(((x100*x113))+((r21*x109))+((x108*x113)));
840 new_r22=(((r22*x109))+((x101*x113))+((x107*x113)));
841 {
842 IkReal j4array[2], cj4array[2], sj4array[2];
843 bool j4valid[2]={false};
844 _nj4 = 2;
845 cj4array[0]=new_r22;
846 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
847 {
848  j4valid[0] = j4valid[1] = true;
849  j4array[0] = IKacos(cj4array[0]);
850  sj4array[0] = IKsin(j4array[0]);
851  cj4array[1] = cj4array[0];
852  j4array[1] = -j4array[0];
853  sj4array[1] = -sj4array[0];
854 }
855 else if( isnan(cj4array[0]) )
856 {
857  // probably any value will work
858  j4valid[0] = true;
859  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
860 }
861 for(int ij4 = 0; ij4 < 2; ++ij4)
862 {
863 if( !j4valid[ij4] )
864 {
865  continue;
866 }
867 _ij4[0] = ij4; _ij4[1] = -1;
868 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
869 {
870 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
871 {
872  j4valid[iij4]=false; _ij4[1] = iij4; break;
873 }
874 }
875 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
876 
877 {
878 IkReal j5eval[3];
879 j5eval[0]=sj4;
880 j5eval[1]=IKsign(sj4);
881 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
882 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
883 {
884 {
885 IkReal j3eval[3];
886 j3eval[0]=sj4;
887 j3eval[1]=IKsign(sj4);
888 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
889 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
890 {
891 {
892 IkReal j3eval[2];
893 j3eval[0]=new_r12;
894 j3eval[1]=sj4;
895 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
896 {
897 {
898 IkReal evalcond[5];
899 bool bgotonextstatement = true;
900 do
901 {
902 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
903 evalcond[1]=new_r02;
904 evalcond[2]=new_r12;
905 evalcond[3]=new_r21;
906 evalcond[4]=new_r20;
907 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 )
908 {
909 bgotonextstatement=false;
910 IkReal j5mul = 1;
911 j5=0;
912 j3mul=-1.0;
913 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 )
914  continue;
915 j3=IKatan2(((-1.0)*new_r01), new_r00);
916 {
917 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
918 vinfos[0].jointtype = 1;
919 vinfos[0].foffset = j0;
920 vinfos[0].indices[0] = _ij0[0];
921 vinfos[0].indices[1] = _ij0[1];
922 vinfos[0].maxsolutions = _nj0;
923 vinfos[1].jointtype = 1;
924 vinfos[1].foffset = j1;
925 vinfos[1].indices[0] = _ij1[0];
926 vinfos[1].indices[1] = _ij1[1];
927 vinfos[1].maxsolutions = _nj1;
928 vinfos[2].jointtype = 1;
929 vinfos[2].foffset = j2;
930 vinfos[2].indices[0] = _ij2[0];
931 vinfos[2].indices[1] = _ij2[1];
932 vinfos[2].maxsolutions = _nj2;
933 vinfos[3].jointtype = 1;
934 vinfos[3].foffset = j3;
935 vinfos[3].fmul = j3mul;
936 vinfos[3].freeind = 0;
937 vinfos[3].maxsolutions = 0;
938 vinfos[4].jointtype = 1;
939 vinfos[4].foffset = j4;
940 vinfos[4].indices[0] = _ij4[0];
941 vinfos[4].indices[1] = _ij4[1];
942 vinfos[4].maxsolutions = _nj4;
943 vinfos[5].jointtype = 1;
944 vinfos[5].foffset = j5;
945 vinfos[5].fmul = j5mul;
946 vinfos[5].freeind = 0;
947 vinfos[5].maxsolutions = 0;
948 std::vector<int> vfree(1);
949 vfree[0] = 5;
950 solutions.AddSolution(vinfos,vfree);
951 }
952 
953 }
954 } while(0);
955 if( bgotonextstatement )
956 {
957 bool bgotonextstatement = true;
958 do
959 {
960 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
961 evalcond[1]=new_r02;
962 evalcond[2]=new_r12;
963 evalcond[3]=new_r21;
964 evalcond[4]=new_r20;
965 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 )
966 {
967 bgotonextstatement=false;
968 IkReal j5mul = 1;
969 j5=0;
970 j3mul=1.0;
971 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 )
972  continue;
973 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
974 {
975 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
976 vinfos[0].jointtype = 1;
977 vinfos[0].foffset = j0;
978 vinfos[0].indices[0] = _ij0[0];
979 vinfos[0].indices[1] = _ij0[1];
980 vinfos[0].maxsolutions = _nj0;
981 vinfos[1].jointtype = 1;
982 vinfos[1].foffset = j1;
983 vinfos[1].indices[0] = _ij1[0];
984 vinfos[1].indices[1] = _ij1[1];
985 vinfos[1].maxsolutions = _nj1;
986 vinfos[2].jointtype = 1;
987 vinfos[2].foffset = j2;
988 vinfos[2].indices[0] = _ij2[0];
989 vinfos[2].indices[1] = _ij2[1];
990 vinfos[2].maxsolutions = _nj2;
991 vinfos[3].jointtype = 1;
992 vinfos[3].foffset = j3;
993 vinfos[3].fmul = j3mul;
994 vinfos[3].freeind = 0;
995 vinfos[3].maxsolutions = 0;
996 vinfos[4].jointtype = 1;
997 vinfos[4].foffset = j4;
998 vinfos[4].indices[0] = _ij4[0];
999 vinfos[4].indices[1] = _ij4[1];
1000 vinfos[4].maxsolutions = _nj4;
1001 vinfos[5].jointtype = 1;
1002 vinfos[5].foffset = j5;
1003 vinfos[5].fmul = j5mul;
1004 vinfos[5].freeind = 0;
1005 vinfos[5].maxsolutions = 0;
1006 std::vector<int> vfree(1);
1007 vfree[0] = 5;
1008 solutions.AddSolution(vinfos,vfree);
1009 }
1010 
1011 }
1012 } while(0);
1013 if( bgotonextstatement )
1014 {
1015 bool bgotonextstatement = true;
1016 do
1017 {
1018 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1019 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1020 {
1021 bgotonextstatement=false;
1022 {
1023 IkReal j3eval[1];
1024 new_r21=0;
1025 new_r20=0;
1026 new_r02=0;
1027 new_r12=0;
1028 IkReal x114=new_r22*new_r22;
1029 IkReal x115=((16.0)*new_r10);
1030 IkReal x116=((16.0)*new_r01);
1031 IkReal x117=((16.0)*new_r22);
1032 IkReal x118=((8.0)*new_r11);
1033 IkReal x119=((8.0)*new_r00);
1034 IkReal x120=(x114*x115);
1035 IkReal x121=(x114*x116);
1036 j3eval[0]=((IKabs((((new_r22*x118))+(((-1.0)*x119)))))+(IKabs(((((-32.0)*new_r11))+((new_r00*x117))+(((16.0)*new_r11*x114)))))+(IKabs(((((-1.0)*x115))+x120)))+(IKabs(((((-1.0)*x120))+x115)))+(IKabs(((((-1.0)*x121))+x116)))+(IKabs((((new_r11*x117))+(((16.0)*new_r00))+(((-32.0)*new_r00*x114)))))+(IKabs(((((-1.0)*x116))+x121)))+(IKabs((((new_r22*x119))+(((-1.0)*x114*x118))))));
1037 if( IKabs(j3eval[0]) < 0.0000000010000000 )
1038 {
1039 continue; // no branches [j3, j5]
1040 
1041 } else
1042 {
1043 IkReal op[4+1], zeror[4];
1044 int numroots;
1045 IkReal j3evalpoly[1];
1046 IkReal x122=new_r22*new_r22;
1047 IkReal x123=((16.0)*new_r10);
1048 IkReal x124=(new_r11*new_r22);
1049 IkReal x125=(x122*x123);
1050 IkReal x126=((((-8.0)*new_r00))+(((8.0)*x124)));
1051 op[0]=x126;
1052 op[1]=((((-1.0)*x125))+x123);
1053 op[2]=((((16.0)*x124))+(((16.0)*new_r00))+(((-32.0)*new_r00*x122)));
1054 op[3]=((((-1.0)*x123))+x125);
1055 op[4]=x126;
1056 polyroots4(op,zeror,numroots);
1057 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1058 int numsolutions = 0;
1059 for(int ij3 = 0; ij3 < numroots; ++ij3)
1060 {
1061 IkReal htj3 = zeror[ij3];
1062 tempj3array[0]=((2.0)*(atan(htj3)));
1063 for(int kj3 = 0; kj3 < 1; ++kj3)
1064 {
1065 j3array[numsolutions] = tempj3array[kj3];
1066 if( j3array[numsolutions] > IKPI )
1067 {
1068  j3array[numsolutions]-=IK2PI;
1069 }
1070 else if( j3array[numsolutions] < -IKPI )
1071 {
1072  j3array[numsolutions]+=IK2PI;
1073 }
1074 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1075 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1076 numsolutions++;
1077 }
1078 }
1079 bool j3valid[4]={true,true,true,true};
1080 _nj3 = 4;
1081 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1082  {
1083 if( !j3valid[ij3] )
1084 {
1085  continue;
1086 }
1087  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1088 htj3 = IKtan(j3/2);
1089 
1090 IkReal x127=((16.0)*new_r01);
1091 IkReal x128=new_r22*new_r22;
1092 IkReal x129=(new_r00*new_r22);
1093 IkReal x130=((8.0)*x129);
1094 IkReal x131=(new_r11*x128);
1095 IkReal x132=(x127*x128);
1096 IkReal x133=((8.0)*x131);
1097 j3evalpoly[0]=((((htj3*htj3)*(((((16.0)*x129))+(((16.0)*x131))+(((-32.0)*new_r11))))))+(((htj3*htj3*htj3)*((x127+(((-1.0)*x132))))))+(((htj3*htj3*htj3*htj3)*((x130+(((-1.0)*x133))))))+x130+(((-1.0)*x133))+((htj3*(((((-1.0)*x127))+x132)))));
1098 if( IKabs(j3evalpoly[0]) > 0.0000000010000000 )
1099 {
1100  continue;
1101 }
1102 _ij3[0] = ij3; _ij3[1] = -1;
1103 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1104 {
1105 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1106 {
1107  j3valid[iij3]=false; _ij3[1] = iij3; break;
1108 }
1109 }
1110 {
1111 IkReal j5eval[3];
1112 new_r21=0;
1113 new_r20=0;
1114 new_r02=0;
1115 new_r12=0;
1116 IkReal x134=cj3*cj3;
1117 IkReal x135=(cj3*new_r22);
1118 IkReal x136=((-1.0)+x134+(((-1.0)*x134*(new_r22*new_r22))));
1119 j5eval[0]=x136;
1120 j5eval[1]=((IKabs((((new_r01*x135))+((new_r00*sj3)))))+(IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x135))))));
1121 j5eval[2]=IKsign(x136);
1122 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1123 {
1124 {
1125 IkReal j5eval[1];
1126 new_r21=0;
1127 new_r20=0;
1128 new_r02=0;
1129 new_r12=0;
1130 j5eval[0]=new_r22;
1131 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1132 {
1133 {
1134 IkReal j5eval[1];
1135 new_r21=0;
1136 new_r20=0;
1137 new_r02=0;
1138 new_r12=0;
1139 j5eval[0]=sj3;
1140 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1141 {
1142 {
1143 IkReal evalcond[1];
1144 bool bgotonextstatement = true;
1145 do
1146 {
1147 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1148 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1149 {
1150 bgotonextstatement=false;
1151 {
1152 IkReal j5array[1], cj5array[1], sj5array[1];
1153 bool j5valid[1]={false};
1154 _nj5 = 1;
1155 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1156  continue;
1157 j5array[0]=IKatan2(new_r10, new_r11);
1158 sj5array[0]=IKsin(j5array[0]);
1159 cj5array[0]=IKcos(j5array[0]);
1160 if( j5array[0] > IKPI )
1161 {
1162  j5array[0]-=IK2PI;
1163 }
1164 else if( j5array[0] < -IKPI )
1165 { j5array[0]+=IK2PI;
1166 }
1167 j5valid[0] = true;
1168 for(int ij5 = 0; ij5 < 1; ++ij5)
1169 {
1170 if( !j5valid[ij5] )
1171 {
1172  continue;
1173 }
1174 _ij5[0] = ij5; _ij5[1] = -1;
1175 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1176 {
1177 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1178 {
1179  j5valid[iij5]=false; _ij5[1] = iij5; break;
1180 }
1181 }
1182 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1183 {
1184 IkReal evalcond[4];
1185 IkReal x137=IKsin(j5);
1186 IkReal x138=IKcos(j5);
1187 evalcond[0]=x137;
1188 evalcond[1]=((-1.0)*x138);
1189 evalcond[2]=(x137+(((-1.0)*new_r10)));
1190 evalcond[3]=(x138+(((-1.0)*new_r11)));
1191 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1192 {
1193 continue;
1194 }
1195 }
1196 
1197 {
1198 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1199 vinfos[0].jointtype = 1;
1200 vinfos[0].foffset = j0;
1201 vinfos[0].indices[0] = _ij0[0];
1202 vinfos[0].indices[1] = _ij0[1];
1203 vinfos[0].maxsolutions = _nj0;
1204 vinfos[1].jointtype = 1;
1205 vinfos[1].foffset = j1;
1206 vinfos[1].indices[0] = _ij1[0];
1207 vinfos[1].indices[1] = _ij1[1];
1208 vinfos[1].maxsolutions = _nj1;
1209 vinfos[2].jointtype = 1;
1210 vinfos[2].foffset = j2;
1211 vinfos[2].indices[0] = _ij2[0];
1212 vinfos[2].indices[1] = _ij2[1];
1213 vinfos[2].maxsolutions = _nj2;
1214 vinfos[3].jointtype = 1;
1215 vinfos[3].foffset = j3;
1216 vinfos[3].indices[0] = _ij3[0];
1217 vinfos[3].indices[1] = _ij3[1];
1218 vinfos[3].maxsolutions = _nj3;
1219 vinfos[4].jointtype = 1;
1220 vinfos[4].foffset = j4;
1221 vinfos[4].indices[0] = _ij4[0];
1222 vinfos[4].indices[1] = _ij4[1];
1223 vinfos[4].maxsolutions = _nj4;
1224 vinfos[5].jointtype = 1;
1225 vinfos[5].foffset = j5;
1226 vinfos[5].indices[0] = _ij5[0];
1227 vinfos[5].indices[1] = _ij5[1];
1228 vinfos[5].maxsolutions = _nj5;
1229 std::vector<int> vfree(0);
1230 solutions.AddSolution(vinfos,vfree);
1231 }
1232 }
1233 }
1234 
1235 }
1236 } while(0);
1237 if( bgotonextstatement )
1238 {
1239 bool bgotonextstatement = true;
1240 do
1241 {
1242 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1243 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1244 {
1245 bgotonextstatement=false;
1246 {
1247 IkReal j5array[1], cj5array[1], sj5array[1];
1248 bool j5valid[1]={false};
1249 _nj5 = 1;
1250 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
1251  continue;
1252 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1253 sj5array[0]=IKsin(j5array[0]);
1254 cj5array[0]=IKcos(j5array[0]);
1255 if( j5array[0] > IKPI )
1256 {
1257  j5array[0]-=IK2PI;
1258 }
1259 else if( j5array[0] < -IKPI )
1260 { j5array[0]+=IK2PI;
1261 }
1262 j5valid[0] = true;
1263 for(int ij5 = 0; ij5 < 1; ++ij5)
1264 {
1265 if( !j5valid[ij5] )
1266 {
1267  continue;
1268 }
1269 _ij5[0] = ij5; _ij5[1] = -1;
1270 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1271 {
1272 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1273 {
1274  j5valid[iij5]=false; _ij5[1] = iij5; break;
1275 }
1276 }
1277 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1278 {
1279 IkReal evalcond[4];
1280 IkReal x139=IKsin(j5);
1281 IkReal x140=IKcos(j5);
1282 evalcond[0]=x139;
1283 evalcond[1]=(x139+new_r10);
1284 evalcond[2]=(x140+new_r11);
1285 evalcond[3]=((-1.0)*x140);
1286 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1287 {
1288 continue;
1289 }
1290 }
1291 
1292 {
1293 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1294 vinfos[0].jointtype = 1;
1295 vinfos[0].foffset = j0;
1296 vinfos[0].indices[0] = _ij0[0];
1297 vinfos[0].indices[1] = _ij0[1];
1298 vinfos[0].maxsolutions = _nj0;
1299 vinfos[1].jointtype = 1;
1300 vinfos[1].foffset = j1;
1301 vinfos[1].indices[0] = _ij1[0];
1302 vinfos[1].indices[1] = _ij1[1];
1303 vinfos[1].maxsolutions = _nj1;
1304 vinfos[2].jointtype = 1;
1305 vinfos[2].foffset = j2;
1306 vinfos[2].indices[0] = _ij2[0];
1307 vinfos[2].indices[1] = _ij2[1];
1308 vinfos[2].maxsolutions = _nj2;
1309 vinfos[3].jointtype = 1;
1310 vinfos[3].foffset = j3;
1311 vinfos[3].indices[0] = _ij3[0];
1312 vinfos[3].indices[1] = _ij3[1];
1313 vinfos[3].maxsolutions = _nj3;
1314 vinfos[4].jointtype = 1;
1315 vinfos[4].foffset = j4;
1316 vinfos[4].indices[0] = _ij4[0];
1317 vinfos[4].indices[1] = _ij4[1];
1318 vinfos[4].maxsolutions = _nj4;
1319 vinfos[5].jointtype = 1;
1320 vinfos[5].foffset = j5;
1321 vinfos[5].indices[0] = _ij5[0];
1322 vinfos[5].indices[1] = _ij5[1];
1323 vinfos[5].maxsolutions = _nj5;
1324 std::vector<int> vfree(0);
1325 solutions.AddSolution(vinfos,vfree);
1326 }
1327 }
1328 }
1329 
1330 }
1331 } while(0);
1332 if( bgotonextstatement )
1333 {
1334 bool bgotonextstatement = true;
1335 do
1336 {
1337 CheckValue<IkReal> x141=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1338 if(!x141.valid){
1339 continue;
1340 }
1341 if((x141.value) < -0.00001)
1342 continue;
1343 IkReal gconst24=((-1.0)*(IKsqrt(x141.value)));
1344 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1345 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1346 {
1347 bgotonextstatement=false;
1348 {
1349 IkReal j5eval[1];
1350 new_r21=0;
1351 new_r20=0;
1352 new_r02=0;
1353 new_r12=0;
1354 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1355 continue;
1356 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
1357 cj3=gconst24;
1358 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1359  continue;
1360 j3=IKacos(gconst24);
1361 CheckValue<IkReal> x142=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1362 if(!x142.valid){
1363 continue;
1364 }
1365 if((x142.value) < -0.00001)
1366 continue;
1367 IkReal gconst24=((-1.0)*(IKsqrt(x142.value)));
1368 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1369 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1370 {
1371 {
1372 IkReal j5array[1], cj5array[1], sj5array[1];
1373 bool j5valid[1]={false};
1374 _nj5 = 1;
1375 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1376 continue;
1377 CheckValue<IkReal> x143=IKPowWithIntegerCheck(gconst24,-1);
1378 if(!x143.valid){
1379 continue;
1380 }
1381 if( IKabs(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x143.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x143.value)))-1) <= IKFAST_SINCOS_THRESH )
1382  continue;
1383 j5array[0]=IKatan2(((((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x143.value)));
1384 sj5array[0]=IKsin(j5array[0]);
1385 cj5array[0]=IKcos(j5array[0]);
1386 if( j5array[0] > IKPI )
1387 {
1388  j5array[0]-=IK2PI;
1389 }
1390 else if( j5array[0] < -IKPI )
1391 { j5array[0]+=IK2PI;
1392 }
1393 j5valid[0] = true;
1394 for(int ij5 = 0; ij5 < 1; ++ij5)
1395 {
1396 if( !j5valid[ij5] )
1397 {
1398  continue;
1399 }
1400 _ij5[0] = ij5; _ij5[1] = -1;
1401 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1402 {
1403 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1404 {
1405  j5valid[iij5]=false; _ij5[1] = iij5; break;
1406 }
1407 }
1408 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1409 {
1410 IkReal evalcond[8];
1411 IkReal x144=IKcos(j5);
1412 IkReal x145=IKsin(j5);
1413 IkReal x146=((1.0)*gconst24);
1414 if((((1.0)+(((-1.0)*gconst24*x146)))) < -0.00001)
1415 continue;
1416 IkReal x147=IKsqrt(((1.0)+(((-1.0)*gconst24*x146))));
1417 evalcond[0]=x145;
1418 evalcond[1]=((-1.0)*x144);
1419 evalcond[2]=((((-1.0)*x144*x146))+new_r11);
1420 evalcond[3]=((((-1.0)*x145*x146))+new_r10);
1421 evalcond[4]=(((x144*x147))+new_r01);
1422 evalcond[5]=(((x145*x147))+new_r00);
1423 evalcond[6]=(((new_r00*x147))+x145+(((-1.0)*new_r10*x146)));
1424 evalcond[7]=(((new_r01*x147))+(((-1.0)*new_r11*x146))+x144);
1425 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1426 {
1427 continue;
1428 }
1429 }
1430 
1431 {
1432 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1433 vinfos[0].jointtype = 1;
1434 vinfos[0].foffset = j0;
1435 vinfos[0].indices[0] = _ij0[0];
1436 vinfos[0].indices[1] = _ij0[1];
1437 vinfos[0].maxsolutions = _nj0;
1438 vinfos[1].jointtype = 1;
1439 vinfos[1].foffset = j1;
1440 vinfos[1].indices[0] = _ij1[0];
1441 vinfos[1].indices[1] = _ij1[1];
1442 vinfos[1].maxsolutions = _nj1;
1443 vinfos[2].jointtype = 1;
1444 vinfos[2].foffset = j2;
1445 vinfos[2].indices[0] = _ij2[0];
1446 vinfos[2].indices[1] = _ij2[1];
1447 vinfos[2].maxsolutions = _nj2;
1448 vinfos[3].jointtype = 1;
1449 vinfos[3].foffset = j3;
1450 vinfos[3].indices[0] = _ij3[0];
1451 vinfos[3].indices[1] = _ij3[1];
1452 vinfos[3].maxsolutions = _nj3;
1453 vinfos[4].jointtype = 1;
1454 vinfos[4].foffset = j4;
1455 vinfos[4].indices[0] = _ij4[0];
1456 vinfos[4].indices[1] = _ij4[1];
1457 vinfos[4].maxsolutions = _nj4;
1458 vinfos[5].jointtype = 1;
1459 vinfos[5].foffset = j5;
1460 vinfos[5].indices[0] = _ij5[0];
1461 vinfos[5].indices[1] = _ij5[1];
1462 vinfos[5].maxsolutions = _nj5;
1463 std::vector<int> vfree(0);
1464 solutions.AddSolution(vinfos,vfree);
1465 }
1466 }
1467 }
1468 
1469 } else
1470 {
1471 {
1472 IkReal j5array[1], cj5array[1], sj5array[1];
1473 bool j5valid[1]={false};
1474 _nj5 = 1;
1475 CheckValue<IkReal> x148=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1476 if(!x148.valid){
1477 continue;
1478 }
1479 CheckValue<IkReal> x149 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1480 if(!x149.valid){
1481 continue;
1482 }
1483 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x148.value)))+(x149.value));
1484 sj5array[0]=IKsin(j5array[0]);
1485 cj5array[0]=IKcos(j5array[0]);
1486 if( j5array[0] > IKPI )
1487 {
1488  j5array[0]-=IK2PI;
1489 }
1490 else if( j5array[0] < -IKPI )
1491 { j5array[0]+=IK2PI;
1492 }
1493 j5valid[0] = true;
1494 for(int ij5 = 0; ij5 < 1; ++ij5)
1495 {
1496 if( !j5valid[ij5] )
1497 {
1498  continue;
1499 }
1500 _ij5[0] = ij5; _ij5[1] = -1;
1501 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1502 {
1503 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1504 {
1505  j5valid[iij5]=false; _ij5[1] = iij5; break;
1506 }
1507 }
1508 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1509 {
1510 IkReal evalcond[8];
1511 IkReal x150=IKcos(j5);
1512 IkReal x151=IKsin(j5);
1513 IkReal x152=((1.0)*gconst24);
1514 if((((1.0)+(((-1.0)*gconst24*x152)))) < -0.00001)
1515 continue;
1516 IkReal x153=IKsqrt(((1.0)+(((-1.0)*gconst24*x152))));
1517 evalcond[0]=x151;
1518 evalcond[1]=((-1.0)*x150);
1519 evalcond[2]=((((-1.0)*x150*x152))+new_r11);
1520 evalcond[3]=((((-1.0)*x151*x152))+new_r10);
1521 evalcond[4]=(((x150*x153))+new_r01);
1522 evalcond[5]=(((x151*x153))+new_r00);
1523 evalcond[6]=(((new_r00*x153))+x151+(((-1.0)*new_r10*x152)));
1524 evalcond[7]=(((new_r01*x153))+x150+(((-1.0)*new_r11*x152)));
1525 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1526 {
1527 continue;
1528 }
1529 }
1530 
1531 {
1532 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1533 vinfos[0].jointtype = 1;
1534 vinfos[0].foffset = j0;
1535 vinfos[0].indices[0] = _ij0[0];
1536 vinfos[0].indices[1] = _ij0[1];
1537 vinfos[0].maxsolutions = _nj0;
1538 vinfos[1].jointtype = 1;
1539 vinfos[1].foffset = j1;
1540 vinfos[1].indices[0] = _ij1[0];
1541 vinfos[1].indices[1] = _ij1[1];
1542 vinfos[1].maxsolutions = _nj1;
1543 vinfos[2].jointtype = 1;
1544 vinfos[2].foffset = j2;
1545 vinfos[2].indices[0] = _ij2[0];
1546 vinfos[2].indices[1] = _ij2[1];
1547 vinfos[2].maxsolutions = _nj2;
1548 vinfos[3].jointtype = 1;
1549 vinfos[3].foffset = j3;
1550 vinfos[3].indices[0] = _ij3[0];
1551 vinfos[3].indices[1] = _ij3[1];
1552 vinfos[3].maxsolutions = _nj3;
1553 vinfos[4].jointtype = 1;
1554 vinfos[4].foffset = j4;
1555 vinfos[4].indices[0] = _ij4[0];
1556 vinfos[4].indices[1] = _ij4[1];
1557 vinfos[4].maxsolutions = _nj4;
1558 vinfos[5].jointtype = 1;
1559 vinfos[5].foffset = j5;
1560 vinfos[5].indices[0] = _ij5[0];
1561 vinfos[5].indices[1] = _ij5[1];
1562 vinfos[5].maxsolutions = _nj5;
1563 std::vector<int> vfree(0);
1564 solutions.AddSolution(vinfos,vfree);
1565 }
1566 }
1567 }
1568 
1569 }
1570 
1571 }
1572 
1573 }
1574 } while(0);
1575 if( bgotonextstatement )
1576 {
1577 bool bgotonextstatement = true;
1578 do
1579 {
1580 CheckValue<IkReal> x154=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1581 if(!x154.valid){
1582 continue;
1583 }
1584 if((x154.value) < -0.00001)
1585 continue;
1586 IkReal gconst24=((-1.0)*(IKsqrt(x154.value)));
1587 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst24)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1588 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1589 {
1590 bgotonextstatement=false;
1591 {
1592 IkReal j5eval[1];
1593 new_r21=0;
1594 new_r20=0;
1595 new_r02=0;
1596 new_r12=0;
1597 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1598 continue;
1599 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
1600 cj3=gconst24;
1601 if( (gconst24) < -1-IKFAST_SINCOS_THRESH || (gconst24) > 1+IKFAST_SINCOS_THRESH )
1602  continue;
1603 j3=((-1.0)*(IKacos(gconst24)));
1604 CheckValue<IkReal> x155=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1605 if(!x155.valid){
1606 continue;
1607 }
1608 if((x155.value) < -0.00001)
1609 continue;
1610 IkReal gconst24=((-1.0)*(IKsqrt(x155.value)));
1611 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1612 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1613 {
1614 {
1615 IkReal j5array[1], cj5array[1], sj5array[1];
1616 bool j5valid[1]={false};
1617 _nj5 = 1;
1618 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
1619 continue;
1620 CheckValue<IkReal> x156=IKPowWithIntegerCheck(gconst24,-1);
1621 if(!x156.valid){
1622 continue;
1623 }
1624 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x156.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))))+IKsqr((new_r11*(x156.value)))-1) <= IKFAST_SINCOS_THRESH )
1625  continue;
1626 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x156.value)));
1627 sj5array[0]=IKsin(j5array[0]);
1628 cj5array[0]=IKcos(j5array[0]);
1629 if( j5array[0] > IKPI )
1630 {
1631  j5array[0]-=IK2PI;
1632 }
1633 else if( j5array[0] < -IKPI )
1634 { j5array[0]+=IK2PI;
1635 }
1636 j5valid[0] = true;
1637 for(int ij5 = 0; ij5 < 1; ++ij5)
1638 {
1639 if( !j5valid[ij5] )
1640 {
1641  continue;
1642 }
1643 _ij5[0] = ij5; _ij5[1] = -1;
1644 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1645 {
1646 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1647 {
1648  j5valid[iij5]=false; _ij5[1] = iij5; break;
1649 }
1650 }
1651 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1652 {
1653 IkReal evalcond[8];
1654 IkReal x157=IKcos(j5);
1655 IkReal x158=IKsin(j5);
1656 IkReal x159=((1.0)*gconst24);
1657 IkReal x160=((1.0)*x158);
1658 if((((1.0)+(((-1.0)*gconst24*x159)))) < -0.00001)
1659 continue;
1660 IkReal x161=IKsqrt(((1.0)+(((-1.0)*gconst24*x159))));
1661 IkReal x162=((1.0)*x161);
1662 evalcond[0]=x158;
1663 evalcond[1]=((-1.0)*x157);
1664 evalcond[2]=((((-1.0)*x157*x159))+new_r11);
1665 evalcond[3]=((((-1.0)*x158*x159))+new_r10);
1666 evalcond[4]=((((-1.0)*x157*x162))+new_r01);
1667 evalcond[5]=((((-1.0)*x160*x161))+new_r00);
1668 evalcond[6]=((((-1.0)*new_r00*x162))+x158+(((-1.0)*new_r10*x159)));
1669 evalcond[7]=(x157+(((-1.0)*new_r11*x159))+(((-1.0)*new_r01*x162)));
1670 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1671 {
1672 continue;
1673 }
1674 }
1675 
1676 {
1677 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1678 vinfos[0].jointtype = 1;
1679 vinfos[0].foffset = j0;
1680 vinfos[0].indices[0] = _ij0[0];
1681 vinfos[0].indices[1] = _ij0[1];
1682 vinfos[0].maxsolutions = _nj0;
1683 vinfos[1].jointtype = 1;
1684 vinfos[1].foffset = j1;
1685 vinfos[1].indices[0] = _ij1[0];
1686 vinfos[1].indices[1] = _ij1[1];
1687 vinfos[1].maxsolutions = _nj1;
1688 vinfos[2].jointtype = 1;
1689 vinfos[2].foffset = j2;
1690 vinfos[2].indices[0] = _ij2[0];
1691 vinfos[2].indices[1] = _ij2[1];
1692 vinfos[2].maxsolutions = _nj2;
1693 vinfos[3].jointtype = 1;
1694 vinfos[3].foffset = j3;
1695 vinfos[3].indices[0] = _ij3[0];
1696 vinfos[3].indices[1] = _ij3[1];
1697 vinfos[3].maxsolutions = _nj3;
1698 vinfos[4].jointtype = 1;
1699 vinfos[4].foffset = j4;
1700 vinfos[4].indices[0] = _ij4[0];
1701 vinfos[4].indices[1] = _ij4[1];
1702 vinfos[4].maxsolutions = _nj4;
1703 vinfos[5].jointtype = 1;
1704 vinfos[5].foffset = j5;
1705 vinfos[5].indices[0] = _ij5[0];
1706 vinfos[5].indices[1] = _ij5[1];
1707 vinfos[5].maxsolutions = _nj5;
1708 std::vector<int> vfree(0);
1709 solutions.AddSolution(vinfos,vfree);
1710 }
1711 }
1712 }
1713 
1714 } else
1715 {
1716 {
1717 IkReal j5array[1], cj5array[1], sj5array[1];
1718 bool j5valid[1]={false};
1719 _nj5 = 1;
1720 CheckValue<IkReal> x163=IKPowWithIntegerCheck(IKsign(gconst24),-1);
1721 if(!x163.valid){
1722 continue;
1723 }
1724 CheckValue<IkReal> x164 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1725 if(!x164.valid){
1726 continue;
1727 }
1728 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x163.value)))+(x164.value));
1729 sj5array[0]=IKsin(j5array[0]);
1730 cj5array[0]=IKcos(j5array[0]);
1731 if( j5array[0] > IKPI )
1732 {
1733  j5array[0]-=IK2PI;
1734 }
1735 else if( j5array[0] < -IKPI )
1736 { j5array[0]+=IK2PI;
1737 }
1738 j5valid[0] = true;
1739 for(int ij5 = 0; ij5 < 1; ++ij5)
1740 {
1741 if( !j5valid[ij5] )
1742 {
1743  continue;
1744 }
1745 _ij5[0] = ij5; _ij5[1] = -1;
1746 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1747 {
1748 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1749 {
1750  j5valid[iij5]=false; _ij5[1] = iij5; break;
1751 }
1752 }
1753 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1754 {
1755 IkReal evalcond[8];
1756 IkReal x165=IKcos(j5);
1757 IkReal x166=IKsin(j5);
1758 IkReal x167=((1.0)*gconst24);
1759 IkReal x168=((1.0)*x166);
1760 if((((1.0)+(((-1.0)*gconst24*x167)))) < -0.00001)
1761 continue;
1762 IkReal x169=IKsqrt(((1.0)+(((-1.0)*gconst24*x167))));
1763 IkReal x170=((1.0)*x169);
1764 evalcond[0]=x166;
1765 evalcond[1]=((-1.0)*x165);
1766 evalcond[2]=(new_r11+(((-1.0)*x165*x167)));
1767 evalcond[3]=((((-1.0)*x166*x167))+new_r10);
1768 evalcond[4]=(new_r01+(((-1.0)*x165*x170)));
1769 evalcond[5]=((((-1.0)*x168*x169))+new_r00);
1770 evalcond[6]=((((-1.0)*new_r10*x167))+x166+(((-1.0)*new_r00*x170)));
1771 evalcond[7]=((((-1.0)*new_r11*x167))+x165+(((-1.0)*new_r01*x170)));
1772 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1773 {
1774 continue;
1775 }
1776 }
1777 
1778 {
1779 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1780 vinfos[0].jointtype = 1;
1781 vinfos[0].foffset = j0;
1782 vinfos[0].indices[0] = _ij0[0];
1783 vinfos[0].indices[1] = _ij0[1];
1784 vinfos[0].maxsolutions = _nj0;
1785 vinfos[1].jointtype = 1;
1786 vinfos[1].foffset = j1;
1787 vinfos[1].indices[0] = _ij1[0];
1788 vinfos[1].indices[1] = _ij1[1];
1789 vinfos[1].maxsolutions = _nj1;
1790 vinfos[2].jointtype = 1;
1791 vinfos[2].foffset = j2;
1792 vinfos[2].indices[0] = _ij2[0];
1793 vinfos[2].indices[1] = _ij2[1];
1794 vinfos[2].maxsolutions = _nj2;
1795 vinfos[3].jointtype = 1;
1796 vinfos[3].foffset = j3;
1797 vinfos[3].indices[0] = _ij3[0];
1798 vinfos[3].indices[1] = _ij3[1];
1799 vinfos[3].maxsolutions = _nj3;
1800 vinfos[4].jointtype = 1;
1801 vinfos[4].foffset = j4;
1802 vinfos[4].indices[0] = _ij4[0];
1803 vinfos[4].indices[1] = _ij4[1];
1804 vinfos[4].maxsolutions = _nj4;
1805 vinfos[5].jointtype = 1;
1806 vinfos[5].foffset = j5;
1807 vinfos[5].indices[0] = _ij5[0];
1808 vinfos[5].indices[1] = _ij5[1];
1809 vinfos[5].maxsolutions = _nj5;
1810 std::vector<int> vfree(0);
1811 solutions.AddSolution(vinfos,vfree);
1812 }
1813 }
1814 }
1815 
1816 }
1817 
1818 }
1819 
1820 }
1821 } while(0);
1822 if( bgotonextstatement )
1823 {
1824 bool bgotonextstatement = true;
1825 do
1826 {
1827 CheckValue<IkReal> x171=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1828 if(!x171.valid){
1829 continue;
1830 }
1831 if((x171.value) < -0.00001)
1832 continue;
1833 IkReal gconst25=IKsqrt(x171.value);
1834 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((-1.0)+(IKsign(sj3)))))), 6.28318530717959)));
1835 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1836 {
1837 bgotonextstatement=false;
1838 {
1839 IkReal j5eval[1];
1840 new_r21=0;
1841 new_r20=0;
1842 new_r02=0;
1843 new_r12=0;
1844 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1845 continue;
1846 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
1847 cj3=gconst25;
1848 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
1849  continue;
1850 j3=IKacos(gconst25);
1851 CheckValue<IkReal> x172=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1852 if(!x172.valid){
1853 continue;
1854 }
1855 if((x172.value) < -0.00001)
1856 continue;
1857 IkReal gconst25=IKsqrt(x172.value);
1858 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1859 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1860 {
1861 {
1862 IkReal j5array[1], cj5array[1], sj5array[1];
1863 bool j5valid[1]={false};
1864 _nj5 = 1;
1865 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
1866 continue;
1867 CheckValue<IkReal> x173=IKPowWithIntegerCheck(gconst25,-1);
1868 if(!x173.valid){
1869 continue;
1870 }
1871 if( IKabs((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x173.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x173.value)))-1) <= IKFAST_SINCOS_THRESH )
1872  continue;
1873 j5array[0]=IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x173.value)));
1874 sj5array[0]=IKsin(j5array[0]);
1875 cj5array[0]=IKcos(j5array[0]);
1876 if( j5array[0] > IKPI )
1877 {
1878  j5array[0]-=IK2PI;
1879 }
1880 else if( j5array[0] < -IKPI )
1881 { j5array[0]+=IK2PI;
1882 }
1883 j5valid[0] = true;
1884 for(int ij5 = 0; ij5 < 1; ++ij5)
1885 {
1886 if( !j5valid[ij5] )
1887 {
1888  continue;
1889 }
1890 _ij5[0] = ij5; _ij5[1] = -1;
1891 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1892 {
1893 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1894 {
1895  j5valid[iij5]=false; _ij5[1] = iij5; break;
1896 }
1897 }
1898 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1899 {
1900 IkReal evalcond[8];
1901 IkReal x174=IKcos(j5);
1902 IkReal x175=IKsin(j5);
1903 IkReal x176=((1.0)*gconst25);
1904 if((((1.0)+(((-1.0)*gconst25*x176)))) < -0.00001)
1905 continue;
1906 IkReal x177=IKsqrt(((1.0)+(((-1.0)*gconst25*x176))));
1907 evalcond[0]=x175;
1908 evalcond[1]=((-1.0)*x174);
1909 evalcond[2]=((((-1.0)*x174*x176))+new_r11);
1910 evalcond[3]=((((-1.0)*x175*x176))+new_r10);
1911 evalcond[4]=(((x174*x177))+new_r01);
1912 evalcond[5]=(((x175*x177))+new_r00);
1913 evalcond[6]=(((new_r00*x177))+(((-1.0)*new_r10*x176))+x175);
1914 evalcond[7]=(((new_r01*x177))+(((-1.0)*new_r11*x176))+x174);
1915 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1916 {
1917 continue;
1918 }
1919 }
1920 
1921 {
1922 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1923 vinfos[0].jointtype = 1;
1924 vinfos[0].foffset = j0;
1925 vinfos[0].indices[0] = _ij0[0];
1926 vinfos[0].indices[1] = _ij0[1];
1927 vinfos[0].maxsolutions = _nj0;
1928 vinfos[1].jointtype = 1;
1929 vinfos[1].foffset = j1;
1930 vinfos[1].indices[0] = _ij1[0];
1931 vinfos[1].indices[1] = _ij1[1];
1932 vinfos[1].maxsolutions = _nj1;
1933 vinfos[2].jointtype = 1;
1934 vinfos[2].foffset = j2;
1935 vinfos[2].indices[0] = _ij2[0];
1936 vinfos[2].indices[1] = _ij2[1];
1937 vinfos[2].maxsolutions = _nj2;
1938 vinfos[3].jointtype = 1;
1939 vinfos[3].foffset = j3;
1940 vinfos[3].indices[0] = _ij3[0];
1941 vinfos[3].indices[1] = _ij3[1];
1942 vinfos[3].maxsolutions = _nj3;
1943 vinfos[4].jointtype = 1;
1944 vinfos[4].foffset = j4;
1945 vinfos[4].indices[0] = _ij4[0];
1946 vinfos[4].indices[1] = _ij4[1];
1947 vinfos[4].maxsolutions = _nj4;
1948 vinfos[5].jointtype = 1;
1949 vinfos[5].foffset = j5;
1950 vinfos[5].indices[0] = _ij5[0];
1951 vinfos[5].indices[1] = _ij5[1];
1952 vinfos[5].maxsolutions = _nj5;
1953 std::vector<int> vfree(0);
1954 solutions.AddSolution(vinfos,vfree);
1955 }
1956 }
1957 }
1958 
1959 } else
1960 {
1961 {
1962 IkReal j5array[1], cj5array[1], sj5array[1];
1963 bool j5valid[1]={false};
1964 _nj5 = 1;
1965 CheckValue<IkReal> x178=IKPowWithIntegerCheck(IKsign(gconst25),-1);
1966 if(!x178.valid){
1967 continue;
1968 }
1969 CheckValue<IkReal> x179 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1970 if(!x179.valid){
1971 continue;
1972 }
1973 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x178.value)))+(x179.value));
1974 sj5array[0]=IKsin(j5array[0]);
1975 cj5array[0]=IKcos(j5array[0]);
1976 if( j5array[0] > IKPI )
1977 {
1978  j5array[0]-=IK2PI;
1979 }
1980 else if( j5array[0] < -IKPI )
1981 { j5array[0]+=IK2PI;
1982 }
1983 j5valid[0] = true;
1984 for(int ij5 = 0; ij5 < 1; ++ij5)
1985 {
1986 if( !j5valid[ij5] )
1987 {
1988  continue;
1989 }
1990 _ij5[0] = ij5; _ij5[1] = -1;
1991 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1992 {
1993 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1994 {
1995  j5valid[iij5]=false; _ij5[1] = iij5; break;
1996 }
1997 }
1998 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1999 {
2000 IkReal evalcond[8];
2001 IkReal x180=IKcos(j5);
2002 IkReal x181=IKsin(j5);
2003 IkReal x182=((1.0)*gconst25);
2004 if((((1.0)+(((-1.0)*gconst25*x182)))) < -0.00001)
2005 continue;
2006 IkReal x183=IKsqrt(((1.0)+(((-1.0)*gconst25*x182))));
2007 evalcond[0]=x181;
2008 evalcond[1]=((-1.0)*x180);
2009 evalcond[2]=((((-1.0)*x180*x182))+new_r11);
2010 evalcond[3]=(new_r10+(((-1.0)*x181*x182)));
2011 evalcond[4]=(((x180*x183))+new_r01);
2012 evalcond[5]=(new_r00+((x181*x183)));
2013 evalcond[6]=(((new_r00*x183))+x181+(((-1.0)*new_r10*x182)));
2014 evalcond[7]=(((new_r01*x183))+x180+(((-1.0)*new_r11*x182)));
2015 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2016 {
2017 continue;
2018 }
2019 }
2020 
2021 {
2022 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2023 vinfos[0].jointtype = 1;
2024 vinfos[0].foffset = j0;
2025 vinfos[0].indices[0] = _ij0[0];
2026 vinfos[0].indices[1] = _ij0[1];
2027 vinfos[0].maxsolutions = _nj0;
2028 vinfos[1].jointtype = 1;
2029 vinfos[1].foffset = j1;
2030 vinfos[1].indices[0] = _ij1[0];
2031 vinfos[1].indices[1] = _ij1[1];
2032 vinfos[1].maxsolutions = _nj1;
2033 vinfos[2].jointtype = 1;
2034 vinfos[2].foffset = j2;
2035 vinfos[2].indices[0] = _ij2[0];
2036 vinfos[2].indices[1] = _ij2[1];
2037 vinfos[2].maxsolutions = _nj2;
2038 vinfos[3].jointtype = 1;
2039 vinfos[3].foffset = j3;
2040 vinfos[3].indices[0] = _ij3[0];
2041 vinfos[3].indices[1] = _ij3[1];
2042 vinfos[3].maxsolutions = _nj3;
2043 vinfos[4].jointtype = 1;
2044 vinfos[4].foffset = j4;
2045 vinfos[4].indices[0] = _ij4[0];
2046 vinfos[4].indices[1] = _ij4[1];
2047 vinfos[4].maxsolutions = _nj4;
2048 vinfos[5].jointtype = 1;
2049 vinfos[5].foffset = j5;
2050 vinfos[5].indices[0] = _ij5[0];
2051 vinfos[5].indices[1] = _ij5[1];
2052 vinfos[5].maxsolutions = _nj5;
2053 std::vector<int> vfree(0);
2054 solutions.AddSolution(vinfos,vfree);
2055 }
2056 }
2057 }
2058 
2059 }
2060 
2061 }
2062 
2063 }
2064 } while(0);
2065 if( bgotonextstatement )
2066 {
2067 bool bgotonextstatement = true;
2068 do
2069 {
2070 CheckValue<IkReal> x184=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2071 if(!x184.valid){
2072 continue;
2073 }
2074 if((x184.value) < -0.00001)
2075 continue;
2076 IkReal gconst25=IKsqrt(x184.value);
2077 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst25)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959)));
2078 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2079 {
2080 bgotonextstatement=false;
2081 {
2082 IkReal j5eval[1];
2083 new_r21=0;
2084 new_r20=0;
2085 new_r02=0;
2086 new_r12=0;
2087 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2088 continue;
2089 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
2090 cj3=gconst25;
2091 if( (gconst25) < -1-IKFAST_SINCOS_THRESH || (gconst25) > 1+IKFAST_SINCOS_THRESH )
2092  continue;
2093 j3=((-1.0)*(IKacos(gconst25)));
2094 CheckValue<IkReal> x185=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2095 if(!x185.valid){
2096 continue;
2097 }
2098 if((x185.value) < -0.00001)
2099 continue;
2100 IkReal gconst25=IKsqrt(x185.value);
2101 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2102 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2103 {
2104 {
2105 IkReal j5array[1], cj5array[1], sj5array[1];
2106 bool j5valid[1]={false};
2107 _nj5 = 1;
2108 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
2109 continue;
2110 CheckValue<IkReal> x186=IKPowWithIntegerCheck(gconst25,-1);
2111 if(!x186.valid){
2112 continue;
2113 }
2114 if( IKabs((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x186.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))))+IKsqr((new_r11*(x186.value)))-1) <= IKFAST_SINCOS_THRESH )
2115  continue;
2116 j5array[0]=IKatan2((((gconst25*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x186.value)));
2117 sj5array[0]=IKsin(j5array[0]);
2118 cj5array[0]=IKcos(j5array[0]);
2119 if( j5array[0] > IKPI )
2120 {
2121  j5array[0]-=IK2PI;
2122 }
2123 else if( j5array[0] < -IKPI )
2124 { j5array[0]+=IK2PI;
2125 }
2126 j5valid[0] = true;
2127 for(int ij5 = 0; ij5 < 1; ++ij5)
2128 {
2129 if( !j5valid[ij5] )
2130 {
2131  continue;
2132 }
2133 _ij5[0] = ij5; _ij5[1] = -1;
2134 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2135 {
2136 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2137 {
2138  j5valid[iij5]=false; _ij5[1] = iij5; break;
2139 }
2140 }
2141 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2142 {
2143 IkReal evalcond[8];
2144 IkReal x187=IKcos(j5);
2145 IkReal x188=IKsin(j5);
2146 IkReal x189=((1.0)*gconst25);
2147 if((((1.0)+(((-1.0)*gconst25*x189)))) < -0.00001)
2148 continue;
2149 IkReal x190=IKsqrt(((1.0)+(((-1.0)*gconst25*x189))));
2150 IkReal x191=((1.0)*x190);
2151 evalcond[0]=x188;
2152 evalcond[1]=((-1.0)*x187);
2153 evalcond[2]=((((-1.0)*x187*x189))+new_r11);
2154 evalcond[3]=((((-1.0)*x188*x189))+new_r10);
2155 evalcond[4]=((((-1.0)*x187*x191))+new_r01);
2156 evalcond[5]=((((-1.0)*x188*x191))+new_r00);
2157 evalcond[6]=(x188+(((-1.0)*new_r10*x189))+(((-1.0)*new_r00*x191)));
2158 evalcond[7]=(x187+(((-1.0)*new_r11*x189))+(((-1.0)*new_r01*x191)));
2159 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2160 {
2161 continue;
2162 }
2163 }
2164 
2165 {
2166 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2167 vinfos[0].jointtype = 1;
2168 vinfos[0].foffset = j0;
2169 vinfos[0].indices[0] = _ij0[0];
2170 vinfos[0].indices[1] = _ij0[1];
2171 vinfos[0].maxsolutions = _nj0;
2172 vinfos[1].jointtype = 1;
2173 vinfos[1].foffset = j1;
2174 vinfos[1].indices[0] = _ij1[0];
2175 vinfos[1].indices[1] = _ij1[1];
2176 vinfos[1].maxsolutions = _nj1;
2177 vinfos[2].jointtype = 1;
2178 vinfos[2].foffset = j2;
2179 vinfos[2].indices[0] = _ij2[0];
2180 vinfos[2].indices[1] = _ij2[1];
2181 vinfos[2].maxsolutions = _nj2;
2182 vinfos[3].jointtype = 1;
2183 vinfos[3].foffset = j3;
2184 vinfos[3].indices[0] = _ij3[0];
2185 vinfos[3].indices[1] = _ij3[1];
2186 vinfos[3].maxsolutions = _nj3;
2187 vinfos[4].jointtype = 1;
2188 vinfos[4].foffset = j4;
2189 vinfos[4].indices[0] = _ij4[0];
2190 vinfos[4].indices[1] = _ij4[1];
2191 vinfos[4].maxsolutions = _nj4;
2192 vinfos[5].jointtype = 1;
2193 vinfos[5].foffset = j5;
2194 vinfos[5].indices[0] = _ij5[0];
2195 vinfos[5].indices[1] = _ij5[1];
2196 vinfos[5].maxsolutions = _nj5;
2197 std::vector<int> vfree(0);
2198 solutions.AddSolution(vinfos,vfree);
2199 }
2200 }
2201 }
2202 
2203 } else
2204 {
2205 {
2206 IkReal j5array[1], cj5array[1], sj5array[1];
2207 bool j5valid[1]={false};
2208 _nj5 = 1;
2209 CheckValue<IkReal> x192=IKPowWithIntegerCheck(IKsign(gconst25),-1);
2210 if(!x192.valid){
2211 continue;
2212 }
2213 CheckValue<IkReal> x193 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2214 if(!x193.valid){
2215 continue;
2216 }
2217 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.value)))+(x193.value));
2218 sj5array[0]=IKsin(j5array[0]);
2219 cj5array[0]=IKcos(j5array[0]);
2220 if( j5array[0] > IKPI )
2221 {
2222  j5array[0]-=IK2PI;
2223 }
2224 else if( j5array[0] < -IKPI )
2225 { j5array[0]+=IK2PI;
2226 }
2227 j5valid[0] = true;
2228 for(int ij5 = 0; ij5 < 1; ++ij5)
2229 {
2230 if( !j5valid[ij5] )
2231 {
2232  continue;
2233 }
2234 _ij5[0] = ij5; _ij5[1] = -1;
2235 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2236 {
2237 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2238 {
2239  j5valid[iij5]=false; _ij5[1] = iij5; break;
2240 }
2241 }
2242 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2243 {
2244 IkReal evalcond[8];
2245 IkReal x194=IKcos(j5);
2246 IkReal x195=IKsin(j5);
2247 IkReal x196=((1.0)*gconst25);
2248 if((((1.0)+(((-1.0)*gconst25*x196)))) < -0.00001)
2249 continue;
2250 IkReal x197=IKsqrt(((1.0)+(((-1.0)*gconst25*x196))));
2251 IkReal x198=((1.0)*x197);
2252 evalcond[0]=x195;
2253 evalcond[1]=((-1.0)*x194);
2254 evalcond[2]=((((-1.0)*x194*x196))+new_r11);
2255 evalcond[3]=((((-1.0)*x195*x196))+new_r10);
2256 evalcond[4]=((((-1.0)*x194*x198))+new_r01);
2257 evalcond[5]=((((-1.0)*x195*x198))+new_r00);
2258 evalcond[6]=(x195+(((-1.0)*new_r10*x196))+(((-1.0)*new_r00*x198)));
2259 evalcond[7]=(x194+(((-1.0)*new_r11*x196))+(((-1.0)*new_r01*x198)));
2260 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2261 {
2262 continue;
2263 }
2264 }
2265 
2266 {
2267 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2268 vinfos[0].jointtype = 1;
2269 vinfos[0].foffset = j0;
2270 vinfos[0].indices[0] = _ij0[0];
2271 vinfos[0].indices[1] = _ij0[1];
2272 vinfos[0].maxsolutions = _nj0;
2273 vinfos[1].jointtype = 1;
2274 vinfos[1].foffset = j1;
2275 vinfos[1].indices[0] = _ij1[0];
2276 vinfos[1].indices[1] = _ij1[1];
2277 vinfos[1].maxsolutions = _nj1;
2278 vinfos[2].jointtype = 1;
2279 vinfos[2].foffset = j2;
2280 vinfos[2].indices[0] = _ij2[0];
2281 vinfos[2].indices[1] = _ij2[1];
2282 vinfos[2].maxsolutions = _nj2;
2283 vinfos[3].jointtype = 1;
2284 vinfos[3].foffset = j3;
2285 vinfos[3].indices[0] = _ij3[0];
2286 vinfos[3].indices[1] = _ij3[1];
2287 vinfos[3].maxsolutions = _nj3;
2288 vinfos[4].jointtype = 1;
2289 vinfos[4].foffset = j4;
2290 vinfos[4].indices[0] = _ij4[0];
2291 vinfos[4].indices[1] = _ij4[1];
2292 vinfos[4].maxsolutions = _nj4;
2293 vinfos[5].jointtype = 1;
2294 vinfos[5].foffset = j5;
2295 vinfos[5].indices[0] = _ij5[0];
2296 vinfos[5].indices[1] = _ij5[1];
2297 vinfos[5].maxsolutions = _nj5;
2298 std::vector<int> vfree(0);
2299 solutions.AddSolution(vinfos,vfree);
2300 }
2301 }
2302 }
2303 
2304 }
2305 
2306 }
2307 
2308 }
2309 } while(0);
2310 if( bgotonextstatement )
2311 {
2312 bool bgotonextstatement = true;
2313 do
2314 {
2315 if( 1 )
2316 {
2317 bgotonextstatement=false;
2318 continue; // branch miss [j5]
2319 
2320 }
2321 } while(0);
2322 if( bgotonextstatement )
2323 {
2324 }
2325 }
2326 }
2327 }
2328 }
2329 }
2330 }
2331 }
2332 
2333 } else
2334 {
2335 {
2336 IkReal j5array[1], cj5array[1], sj5array[1];
2337 bool j5valid[1]={false};
2338 _nj5 = 1;
2339 IkReal x199=(new_r00*sj3);
2340 CheckValue<IkReal> x200=IKPowWithIntegerCheck(sj3,-1);
2341 if(!x200.valid){
2342 continue;
2343 }
2344 if( IKabs(((((-1.0)*x199))+((cj3*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x200.value)*((((cj3*new_r22*x199))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x199))+((cj3*new_r10))))+IKsqr(((x200.value)*((((cj3*new_r22*x199))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2345  continue;
2346 j5array[0]=IKatan2(((((-1.0)*x199))+((cj3*new_r10))), ((x200.value)*((((cj3*new_r22*x199))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2347 sj5array[0]=IKsin(j5array[0]);
2348 cj5array[0]=IKcos(j5array[0]);
2349 if( j5array[0] > IKPI )
2350 {
2351  j5array[0]-=IK2PI;
2352 }
2353 else if( j5array[0] < -IKPI )
2354 { j5array[0]+=IK2PI;
2355 }
2356 j5valid[0] = true;
2357 for(int ij5 = 0; ij5 < 1; ++ij5)
2358 {
2359 if( !j5valid[ij5] )
2360 {
2361  continue;
2362 }
2363 _ij5[0] = ij5; _ij5[1] = -1;
2364 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2365 {
2366 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2367 {
2368  j5valid[iij5]=false; _ij5[1] = iij5; break;
2369 }
2370 }
2371 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2372 {
2373 IkReal evalcond[10];
2374 IkReal x201=IKsin(j5);
2375 IkReal x202=IKcos(j5);
2376 IkReal x203=((1.0)*cj3);
2377 IkReal x204=(cj3*new_r00);
2378 IkReal x205=(cj3*new_r22);
2379 IkReal x206=(new_r22*sj3);
2380 IkReal x207=(new_r22*x202);
2381 IkReal x208=(sj3*x201);
2382 evalcond[0]=(((new_r00*sj3))+x201+(((-1.0)*new_r10*x203)));
2383 evalcond[1]=(((new_r01*sj3))+x202+(((-1.0)*new_r11*x203)));
2384 evalcond[2]=(((new_r11*sj3))+((new_r22*x201))+((cj3*new_r01)));
2385 evalcond[3]=(((new_r11*x206))+((new_r01*x205))+x201);
2386 evalcond[4]=(((sj3*x202))+((x201*x205))+new_r01);
2387 evalcond[5]=(((new_r10*sj3))+x204+(((-1.0)*x207)));
2388 evalcond[6]=((((-1.0)*x203*x207))+x208+new_r00);
2389 evalcond[7]=(((x201*x206))+(((-1.0)*x202*x203))+new_r11);
2390 evalcond[8]=(((new_r10*x206))+((new_r22*x204))+(((-1.0)*x202)));
2391 evalcond[9]=((((-1.0)*x201*x203))+(((-1.0)*x202*x206))+new_r10);
2392 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2393 {
2394 continue;
2395 }
2396 }
2397 
2398 {
2399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2400 vinfos[0].jointtype = 1;
2401 vinfos[0].foffset = j0;
2402 vinfos[0].indices[0] = _ij0[0];
2403 vinfos[0].indices[1] = _ij0[1];
2404 vinfos[0].maxsolutions = _nj0;
2405 vinfos[1].jointtype = 1;
2406 vinfos[1].foffset = j1;
2407 vinfos[1].indices[0] = _ij1[0];
2408 vinfos[1].indices[1] = _ij1[1];
2409 vinfos[1].maxsolutions = _nj1;
2410 vinfos[2].jointtype = 1;
2411 vinfos[2].foffset = j2;
2412 vinfos[2].indices[0] = _ij2[0];
2413 vinfos[2].indices[1] = _ij2[1];
2414 vinfos[2].maxsolutions = _nj2;
2415 vinfos[3].jointtype = 1;
2416 vinfos[3].foffset = j3;
2417 vinfos[3].indices[0] = _ij3[0];
2418 vinfos[3].indices[1] = _ij3[1];
2419 vinfos[3].maxsolutions = _nj3;
2420 vinfos[4].jointtype = 1;
2421 vinfos[4].foffset = j4;
2422 vinfos[4].indices[0] = _ij4[0];
2423 vinfos[4].indices[1] = _ij4[1];
2424 vinfos[4].maxsolutions = _nj4;
2425 vinfos[5].jointtype = 1;
2426 vinfos[5].foffset = j5;
2427 vinfos[5].indices[0] = _ij5[0];
2428 vinfos[5].indices[1] = _ij5[1];
2429 vinfos[5].maxsolutions = _nj5;
2430 std::vector<int> vfree(0);
2431 solutions.AddSolution(vinfos,vfree);
2432 }
2433 }
2434 }
2435 
2436 }
2437 
2438 }
2439 
2440 } else
2441 {
2442 {
2443 IkReal j5array[1], cj5array[1], sj5array[1];
2444 bool j5valid[1]={false};
2445 _nj5 = 1;
2446 IkReal x209=((1.0)*new_r01);
2447 CheckValue<IkReal> x210=IKPowWithIntegerCheck(new_r22,-1);
2448 if(!x210.valid){
2449 continue;
2450 }
2451 if( IKabs(((x210.value)*(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x209)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r11))+(((-1.0)*sj3*x209)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x210.value)*(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x209))))))+IKsqr((((cj3*new_r11))+(((-1.0)*sj3*x209))))-1) <= IKFAST_SINCOS_THRESH )
2452  continue;
2453 j5array[0]=IKatan2(((x210.value)*(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x209))))), (((cj3*new_r11))+(((-1.0)*sj3*x209))));
2454 sj5array[0]=IKsin(j5array[0]);
2455 cj5array[0]=IKcos(j5array[0]);
2456 if( j5array[0] > IKPI )
2457 {
2458  j5array[0]-=IK2PI;
2459 }
2460 else if( j5array[0] < -IKPI )
2461 { j5array[0]+=IK2PI;
2462 }
2463 j5valid[0] = true;
2464 for(int ij5 = 0; ij5 < 1; ++ij5)
2465 {
2466 if( !j5valid[ij5] )
2467 {
2468  continue;
2469 }
2470 _ij5[0] = ij5; _ij5[1] = -1;
2471 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2472 {
2473 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2474 {
2475  j5valid[iij5]=false; _ij5[1] = iij5; break;
2476 }
2477 }
2478 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2479 {
2480 IkReal evalcond[10];
2481 IkReal x211=IKsin(j5);
2482 IkReal x212=IKcos(j5);
2483 IkReal x213=((1.0)*cj3);
2484 IkReal x214=(cj3*new_r00);
2485 IkReal x215=(cj3*new_r22);
2486 IkReal x216=(new_r22*sj3);
2487 IkReal x217=(new_r22*x212);
2488 IkReal x218=(sj3*x211);
2489 evalcond[0]=(((new_r00*sj3))+(((-1.0)*new_r10*x213))+x211);
2490 evalcond[1]=((((-1.0)*new_r11*x213))+((new_r01*sj3))+x212);
2491 evalcond[2]=(((new_r22*x211))+((new_r11*sj3))+((cj3*new_r01)));
2492 evalcond[3]=(x211+((new_r01*x215))+((new_r11*x216)));
2493 evalcond[4]=(((x211*x215))+((sj3*x212))+new_r01);
2494 evalcond[5]=(((new_r10*sj3))+x214+(((-1.0)*x217)));
2495 evalcond[6]=(x218+(((-1.0)*x213*x217))+new_r00);
2496 evalcond[7]=(((x211*x216))+new_r11+(((-1.0)*x212*x213)));
2497 evalcond[8]=(((new_r22*x214))+(((-1.0)*x212))+((new_r10*x216)));
2498 evalcond[9]=((((-1.0)*x212*x216))+(((-1.0)*x211*x213))+new_r10);
2499 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2500 {
2501 continue;
2502 }
2503 }
2504 
2505 {
2506 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2507 vinfos[0].jointtype = 1;
2508 vinfos[0].foffset = j0;
2509 vinfos[0].indices[0] = _ij0[0];
2510 vinfos[0].indices[1] = _ij0[1];
2511 vinfos[0].maxsolutions = _nj0;
2512 vinfos[1].jointtype = 1;
2513 vinfos[1].foffset = j1;
2514 vinfos[1].indices[0] = _ij1[0];
2515 vinfos[1].indices[1] = _ij1[1];
2516 vinfos[1].maxsolutions = _nj1;
2517 vinfos[2].jointtype = 1;
2518 vinfos[2].foffset = j2;
2519 vinfos[2].indices[0] = _ij2[0];
2520 vinfos[2].indices[1] = _ij2[1];
2521 vinfos[2].maxsolutions = _nj2;
2522 vinfos[3].jointtype = 1;
2523 vinfos[3].foffset = j3;
2524 vinfos[3].indices[0] = _ij3[0];
2525 vinfos[3].indices[1] = _ij3[1];
2526 vinfos[3].maxsolutions = _nj3;
2527 vinfos[4].jointtype = 1;
2528 vinfos[4].foffset = j4;
2529 vinfos[4].indices[0] = _ij4[0];
2530 vinfos[4].indices[1] = _ij4[1];
2531 vinfos[4].maxsolutions = _nj4;
2532 vinfos[5].jointtype = 1;
2533 vinfos[5].foffset = j5;
2534 vinfos[5].indices[0] = _ij5[0];
2535 vinfos[5].indices[1] = _ij5[1];
2536 vinfos[5].maxsolutions = _nj5;
2537 std::vector<int> vfree(0);
2538 solutions.AddSolution(vinfos,vfree);
2539 }
2540 }
2541 }
2542 
2543 }
2544 
2545 }
2546 
2547 } else
2548 {
2549 {
2550 IkReal j5array[1], cj5array[1], sj5array[1];
2551 bool j5valid[1]={false};
2552 _nj5 = 1;
2553 IkReal x219=cj3*cj3;
2554 IkReal x220=(cj3*new_r22);
2555 CheckValue<IkReal> x221 = IKatan2WithCheck(IkReal((((new_r01*x220))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x220)))),IKFAST_ATAN2_MAGTHRESH);
2556 if(!x221.valid){
2557 continue;
2558 }
2559 CheckValue<IkReal> x222=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x219*(new_r22*new_r22)))+x219)),-1);
2560 if(!x222.valid){
2561 continue;
2562 }
2563 j5array[0]=((-1.5707963267949)+(x221.value)+(((1.5707963267949)*(x222.value))));
2564 sj5array[0]=IKsin(j5array[0]);
2565 cj5array[0]=IKcos(j5array[0]);
2566 if( j5array[0] > IKPI )
2567 {
2568  j5array[0]-=IK2PI;
2569 }
2570 else if( j5array[0] < -IKPI )
2571 { j5array[0]+=IK2PI;
2572 }
2573 j5valid[0] = true;
2574 for(int ij5 = 0; ij5 < 1; ++ij5)
2575 {
2576 if( !j5valid[ij5] )
2577 {
2578  continue;
2579 }
2580 _ij5[0] = ij5; _ij5[1] = -1;
2581 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2582 {
2583 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2584 {
2585  j5valid[iij5]=false; _ij5[1] = iij5; break;
2586 }
2587 }
2588 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2589 {
2590 IkReal evalcond[10];
2591 IkReal x223=IKsin(j5);
2592 IkReal x224=IKcos(j5);
2593 IkReal x225=((1.0)*cj3);
2594 IkReal x226=(cj3*new_r00);
2595 IkReal x227=(cj3*new_r22);
2596 IkReal x228=(new_r22*sj3);
2597 IkReal x229=(new_r22*x224);
2598 IkReal x230=(sj3*x223);
2599 evalcond[0]=((((-1.0)*new_r10*x225))+((new_r00*sj3))+x223);
2600 evalcond[1]=((((-1.0)*new_r11*x225))+((new_r01*sj3))+x224);
2601 evalcond[2]=(((new_r11*sj3))+((new_r22*x223))+((cj3*new_r01)));
2602 evalcond[3]=(((new_r01*x227))+x223+((new_r11*x228)));
2603 evalcond[4]=(((x223*x227))+((sj3*x224))+new_r01);
2604 evalcond[5]=(((new_r10*sj3))+x226+(((-1.0)*x229)));
2605 evalcond[6]=((((-1.0)*x225*x229))+x230+new_r00);
2606 evalcond[7]=((((-1.0)*x224*x225))+((x223*x228))+new_r11);
2607 evalcond[8]=(((new_r22*x226))+((new_r10*x228))+(((-1.0)*x224)));
2608 evalcond[9]=((((-1.0)*x223*x225))+new_r10+(((-1.0)*x224*x228)));
2609 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2610 {
2611 continue;
2612 }
2613 }
2614 
2615 {
2616 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2617 vinfos[0].jointtype = 1;
2618 vinfos[0].foffset = j0;
2619 vinfos[0].indices[0] = _ij0[0];
2620 vinfos[0].indices[1] = _ij0[1];
2621 vinfos[0].maxsolutions = _nj0;
2622 vinfos[1].jointtype = 1;
2623 vinfos[1].foffset = j1;
2624 vinfos[1].indices[0] = _ij1[0];
2625 vinfos[1].indices[1] = _ij1[1];
2626 vinfos[1].maxsolutions = _nj1;
2627 vinfos[2].jointtype = 1;
2628 vinfos[2].foffset = j2;
2629 vinfos[2].indices[0] = _ij2[0];
2630 vinfos[2].indices[1] = _ij2[1];
2631 vinfos[2].maxsolutions = _nj2;
2632 vinfos[3].jointtype = 1;
2633 vinfos[3].foffset = j3;
2634 vinfos[3].indices[0] = _ij3[0];
2635 vinfos[3].indices[1] = _ij3[1];
2636 vinfos[3].maxsolutions = _nj3;
2637 vinfos[4].jointtype = 1;
2638 vinfos[4].foffset = j4;
2639 vinfos[4].indices[0] = _ij4[0];
2640 vinfos[4].indices[1] = _ij4[1];
2641 vinfos[4].maxsolutions = _nj4;
2642 vinfos[5].jointtype = 1;
2643 vinfos[5].foffset = j5;
2644 vinfos[5].indices[0] = _ij5[0];
2645 vinfos[5].indices[1] = _ij5[1];
2646 vinfos[5].maxsolutions = _nj5;
2647 std::vector<int> vfree(0);
2648 solutions.AddSolution(vinfos,vfree);
2649 }
2650 }
2651 }
2652 
2653 }
2654 
2655 }
2656  }
2657 
2658 }
2659 
2660 }
2661 
2662 }
2663 } while(0);
2664 if( bgotonextstatement )
2665 {
2666 bool bgotonextstatement = true;
2667 do
2668 {
2669 if( 1 )
2670 {
2671 bgotonextstatement=false;
2672 continue; // branch miss [j3, j5]
2673 
2674 }
2675 } while(0);
2676 if( bgotonextstatement )
2677 {
2678 }
2679 }
2680 }
2681 }
2682 }
2683 
2684 } else
2685 {
2686 {
2687 IkReal j3array[1], cj3array[1], sj3array[1];
2688 bool j3valid[1]={false};
2689 _nj3 = 1;
2690 CheckValue<IkReal> x232=IKPowWithIntegerCheck(sj4,-1);
2691 if(!x232.valid){
2692 continue;
2693 }
2694 IkReal x231=x232.value;
2695 CheckValue<IkReal> x233=IKPowWithIntegerCheck(new_r12,-1);
2696 if(!x233.valid){
2697 continue;
2698 }
2699 if( IKabs((x231*(x233.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x231)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x231*(x233.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x231))-1) <= IKFAST_SINCOS_THRESH )
2700  continue;
2701 j3array[0]=IKatan2((x231*(x233.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x231));
2702 sj3array[0]=IKsin(j3array[0]);
2703 cj3array[0]=IKcos(j3array[0]);
2704 if( j3array[0] > IKPI )
2705 {
2706  j3array[0]-=IK2PI;
2707 }
2708 else if( j3array[0] < -IKPI )
2709 { j3array[0]+=IK2PI;
2710 }
2711 j3valid[0] = true;
2712 for(int ij3 = 0; ij3 < 1; ++ij3)
2713 {
2714 if( !j3valid[ij3] )
2715 {
2716  continue;
2717 }
2718 _ij3[0] = ij3; _ij3[1] = -1;
2719 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2720 {
2721 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2722 {
2723  j3valid[iij3]=false; _ij3[1] = iij3; break;
2724 }
2725 }
2726 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2727 {
2728 IkReal evalcond[8];
2729 IkReal x234=IKcos(j3);
2730 IkReal x235=IKsin(j3);
2731 IkReal x236=((1.0)*cj4);
2732 IkReal x237=(new_r02*x234);
2733 IkReal x238=(sj4*x235);
2734 IkReal x239=(sj4*x234);
2735 IkReal x240=(new_r12*x235);
2736 evalcond[0]=(x239+new_r02);
2737 evalcond[1]=(x238+new_r12);
2738 evalcond[2]=((((-1.0)*new_r02*x235))+((new_r12*x234)));
2739 evalcond[3]=(sj4+x237+x240);
2740 evalcond[4]=(((cj4*x240))+((new_r22*sj4))+((cj4*x237)));
2741 evalcond[5]=(((new_r10*x238))+(((-1.0)*new_r20*x236))+((new_r00*x239)));
2742 evalcond[6]=((((-1.0)*new_r21*x236))+((new_r11*x238))+((new_r01*x239)));
2743 evalcond[7]=((1.0)+(((-1.0)*new_r22*x236))+((new_r12*x238))+((sj4*x237)));
2744 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2745 {
2746 continue;
2747 }
2748 }
2749 
2750 {
2751 IkReal j5eval[3];
2752 j5eval[0]=sj4;
2753 j5eval[1]=IKsign(sj4);
2754 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2755 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2756 {
2757 {
2758 IkReal j5eval[2];
2759 j5eval[0]=sj4;
2760 j5eval[1]=sj3;
2761 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2762 {
2763 {
2764 IkReal j5eval[2];
2765 j5eval[0]=sj4;
2766 j5eval[1]=cj3;
2767 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2768 {
2769 {
2770 IkReal evalcond[5];
2771 bool bgotonextstatement = true;
2772 do
2773 {
2774 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2775 evalcond[1]=new_r02;
2776 evalcond[2]=new_r12;
2777 evalcond[3]=new_r21;
2778 evalcond[4]=new_r20;
2779 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 )
2780 {
2781 bgotonextstatement=false;
2782 {
2783 IkReal j5array[1], cj5array[1], sj5array[1];
2784 bool j5valid[1]={false};
2785 _nj5 = 1;
2786 IkReal x241=((1.0)*new_r01);
2787 if( IKabs(((((-1.0)*cj3*x241))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x241))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x241))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x241))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2788  continue;
2789 j5array[0]=IKatan2(((((-1.0)*cj3*x241))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x241))+((cj3*new_r00))));
2790 sj5array[0]=IKsin(j5array[0]);
2791 cj5array[0]=IKcos(j5array[0]);
2792 if( j5array[0] > IKPI )
2793 {
2794  j5array[0]-=IK2PI;
2795 }
2796 else if( j5array[0] < -IKPI )
2797 { j5array[0]+=IK2PI;
2798 }
2799 j5valid[0] = true;
2800 for(int ij5 = 0; ij5 < 1; ++ij5)
2801 {
2802 if( !j5valid[ij5] )
2803 {
2804  continue;
2805 }
2806 _ij5[0] = ij5; _ij5[1] = -1;
2807 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2808 {
2809 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2810 {
2811  j5valid[iij5]=false; _ij5[1] = iij5; break;
2812 }
2813 }
2814 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2815 {
2816 IkReal evalcond[8];
2817 IkReal x242=IKsin(j5);
2818 IkReal x243=IKcos(j5);
2819 IkReal x244=((1.0)*cj3);
2820 IkReal x245=(sj3*x242);
2821 IkReal x246=((1.0)*x243);
2822 IkReal x247=(x243*x244);
2823 evalcond[0]=(((new_r11*sj3))+x242+((cj3*new_r01)));
2824 evalcond[1]=(((new_r00*sj3))+x242+(((-1.0)*new_r10*x244)));
2825 evalcond[2]=(((new_r01*sj3))+x243+(((-1.0)*new_r11*x244)));
2826 evalcond[3]=(((sj3*x243))+new_r01+((cj3*x242)));
2827 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x246)));
2828 evalcond[5]=(x245+(((-1.0)*x247))+new_r00);
2829 evalcond[6]=(x245+(((-1.0)*x247))+new_r11);
2830 evalcond[7]=((((-1.0)*x242*x244))+(((-1.0)*sj3*x246))+new_r10);
2831 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2832 {
2833 continue;
2834 }
2835 }
2836 
2837 {
2838 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2839 vinfos[0].jointtype = 1;
2840 vinfos[0].foffset = j0;
2841 vinfos[0].indices[0] = _ij0[0];
2842 vinfos[0].indices[1] = _ij0[1];
2843 vinfos[0].maxsolutions = _nj0;
2844 vinfos[1].jointtype = 1;
2845 vinfos[1].foffset = j1;
2846 vinfos[1].indices[0] = _ij1[0];
2847 vinfos[1].indices[1] = _ij1[1];
2848 vinfos[1].maxsolutions = _nj1;
2849 vinfos[2].jointtype = 1;
2850 vinfos[2].foffset = j2;
2851 vinfos[2].indices[0] = _ij2[0];
2852 vinfos[2].indices[1] = _ij2[1];
2853 vinfos[2].maxsolutions = _nj2;
2854 vinfos[3].jointtype = 1;
2855 vinfos[3].foffset = j3;
2856 vinfos[3].indices[0] = _ij3[0];
2857 vinfos[3].indices[1] = _ij3[1];
2858 vinfos[3].maxsolutions = _nj3;
2859 vinfos[4].jointtype = 1;
2860 vinfos[4].foffset = j4;
2861 vinfos[4].indices[0] = _ij4[0];
2862 vinfos[4].indices[1] = _ij4[1];
2863 vinfos[4].maxsolutions = _nj4;
2864 vinfos[5].jointtype = 1;
2865 vinfos[5].foffset = j5;
2866 vinfos[5].indices[0] = _ij5[0];
2867 vinfos[5].indices[1] = _ij5[1];
2868 vinfos[5].maxsolutions = _nj5;
2869 std::vector<int> vfree(0);
2870 solutions.AddSolution(vinfos,vfree);
2871 }
2872 }
2873 }
2874 
2875 }
2876 } while(0);
2877 if( bgotonextstatement )
2878 {
2879 bool bgotonextstatement = true;
2880 do
2881 {
2882 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2883 evalcond[1]=new_r02;
2884 evalcond[2]=new_r12;
2885 evalcond[3]=new_r21;
2886 evalcond[4]=new_r20;
2887 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 )
2888 {
2889 bgotonextstatement=false;
2890 {
2891 IkReal j5array[1], cj5array[1], sj5array[1];
2892 bool j5valid[1]={false};
2893 _nj5 = 1;
2894 IkReal x248=((1.0)*sj3);
2895 if( IKabs(((((-1.0)*new_r00*x248))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x248))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x248))+((cj3*new_r01))))+IKsqr(((((-1.0)*new_r01*x248))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2896  continue;
2897 j5array[0]=IKatan2(((((-1.0)*new_r00*x248))+((cj3*new_r01))), ((((-1.0)*new_r01*x248))+(((-1.0)*cj3*new_r00))));
2898 sj5array[0]=IKsin(j5array[0]);
2899 cj5array[0]=IKcos(j5array[0]);
2900 if( j5array[0] > IKPI )
2901 {
2902  j5array[0]-=IK2PI;
2903 }
2904 else if( j5array[0] < -IKPI )
2905 { j5array[0]+=IK2PI;
2906 }
2907 j5valid[0] = true;
2908 for(int ij5 = 0; ij5 < 1; ++ij5)
2909 {
2910 if( !j5valid[ij5] )
2911 {
2912  continue;
2913 }
2914 _ij5[0] = ij5; _ij5[1] = -1;
2915 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2916 {
2917 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2918 {
2919  j5valid[iij5]=false; _ij5[1] = iij5; break;
2920 }
2921 }
2922 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2923 {
2924 IkReal evalcond[8];
2925 IkReal x249=IKcos(j5);
2926 IkReal x250=IKsin(j5);
2927 IkReal x251=((1.0)*cj3);
2928 IkReal x252=(sj3*x249);
2929 IkReal x253=((1.0)*x250);
2930 IkReal x254=(x250*x251);
2931 evalcond[0]=(((new_r10*sj3))+x249+((cj3*new_r00)));
2932 evalcond[1]=((((-1.0)*new_r10*x251))+((new_r00*sj3))+x250);
2933 evalcond[2]=((((-1.0)*new_r11*x251))+((new_r01*sj3))+x249);
2934 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x253)));
2935 evalcond[4]=(((sj3*x250))+new_r00+((cj3*x249)));
2936 evalcond[5]=(x252+new_r01+(((-1.0)*x254)));
2937 evalcond[6]=(x252+new_r10+(((-1.0)*x254)));
2938 evalcond[7]=((((-1.0)*sj3*x253))+(((-1.0)*x249*x251))+new_r11);
2939 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2940 {
2941 continue;
2942 }
2943 }
2944 
2945 {
2946 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2947 vinfos[0].jointtype = 1;
2948 vinfos[0].foffset = j0;
2949 vinfos[0].indices[0] = _ij0[0];
2950 vinfos[0].indices[1] = _ij0[1];
2951 vinfos[0].maxsolutions = _nj0;
2952 vinfos[1].jointtype = 1;
2953 vinfos[1].foffset = j1;
2954 vinfos[1].indices[0] = _ij1[0];
2955 vinfos[1].indices[1] = _ij1[1];
2956 vinfos[1].maxsolutions = _nj1;
2957 vinfos[2].jointtype = 1;
2958 vinfos[2].foffset = j2;
2959 vinfos[2].indices[0] = _ij2[0];
2960 vinfos[2].indices[1] = _ij2[1];
2961 vinfos[2].maxsolutions = _nj2;
2962 vinfos[3].jointtype = 1;
2963 vinfos[3].foffset = j3;
2964 vinfos[3].indices[0] = _ij3[0];
2965 vinfos[3].indices[1] = _ij3[1];
2966 vinfos[3].maxsolutions = _nj3;
2967 vinfos[4].jointtype = 1;
2968 vinfos[4].foffset = j4;
2969 vinfos[4].indices[0] = _ij4[0];
2970 vinfos[4].indices[1] = _ij4[1];
2971 vinfos[4].maxsolutions = _nj4;
2972 vinfos[5].jointtype = 1;
2973 vinfos[5].foffset = j5;
2974 vinfos[5].indices[0] = _ij5[0];
2975 vinfos[5].indices[1] = _ij5[1];
2976 vinfos[5].maxsolutions = _nj5;
2977 std::vector<int> vfree(0);
2978 solutions.AddSolution(vinfos,vfree);
2979 }
2980 }
2981 }
2982 
2983 }
2984 } while(0);
2985 if( bgotonextstatement )
2986 {
2987 bool bgotonextstatement = true;
2988 do
2989 {
2990 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
2991 evalcond[1]=new_r02;
2992 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
2993 {
2994 bgotonextstatement=false;
2995 {
2996 IkReal j5array[1], cj5array[1], sj5array[1];
2997 bool j5valid[1]={false};
2998 _nj5 = 1;
2999 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
3000  continue;
3001 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3002 sj5array[0]=IKsin(j5array[0]);
3003 cj5array[0]=IKcos(j5array[0]);
3004 if( j5array[0] > IKPI )
3005 {
3006  j5array[0]-=IK2PI;
3007 }
3008 else if( j5array[0] < -IKPI )
3009 { j5array[0]+=IK2PI;
3010 }
3011 j5valid[0] = true;
3012 for(int ij5 = 0; ij5 < 1; ++ij5)
3013 {
3014 if( !j5valid[ij5] )
3015 {
3016  continue;
3017 }
3018 _ij5[0] = ij5; _ij5[1] = -1;
3019 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3020 {
3021 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3022 {
3023  j5valid[iij5]=false; _ij5[1] = iij5; break;
3024 }
3025 }
3026 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3027 {
3028 IkReal evalcond[8];
3029 IkReal x255=IKsin(j5);
3030 IkReal x256=IKcos(j5);
3031 IkReal x257=((1.0)*x256);
3032 evalcond[0]=(x255+new_r00);
3033 evalcond[1]=(x256+new_r01);
3034 evalcond[2]=(((sj4*x255))+new_r21);
3035 evalcond[3]=(new_r11+((cj4*x255)));
3036 evalcond[4]=((((-1.0)*sj4*x257))+new_r20);
3037 evalcond[5]=((((-1.0)*cj4*x257))+new_r10);
3038 evalcond[6]=(((cj4*new_r11))+x255+((new_r21*sj4)));
3039 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x257)));
3040 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3041 {
3042 continue;
3043 }
3044 }
3045 
3046 {
3047 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3048 vinfos[0].jointtype = 1;
3049 vinfos[0].foffset = j0;
3050 vinfos[0].indices[0] = _ij0[0];
3051 vinfos[0].indices[1] = _ij0[1];
3052 vinfos[0].maxsolutions = _nj0;
3053 vinfos[1].jointtype = 1;
3054 vinfos[1].foffset = j1;
3055 vinfos[1].indices[0] = _ij1[0];
3056 vinfos[1].indices[1] = _ij1[1];
3057 vinfos[1].maxsolutions = _nj1;
3058 vinfos[2].jointtype = 1;
3059 vinfos[2].foffset = j2;
3060 vinfos[2].indices[0] = _ij2[0];
3061 vinfos[2].indices[1] = _ij2[1];
3062 vinfos[2].maxsolutions = _nj2;
3063 vinfos[3].jointtype = 1;
3064 vinfos[3].foffset = j3;
3065 vinfos[3].indices[0] = _ij3[0];
3066 vinfos[3].indices[1] = _ij3[1];
3067 vinfos[3].maxsolutions = _nj3;
3068 vinfos[4].jointtype = 1;
3069 vinfos[4].foffset = j4;
3070 vinfos[4].indices[0] = _ij4[0];
3071 vinfos[4].indices[1] = _ij4[1];
3072 vinfos[4].maxsolutions = _nj4;
3073 vinfos[5].jointtype = 1;
3074 vinfos[5].foffset = j5;
3075 vinfos[5].indices[0] = _ij5[0];
3076 vinfos[5].indices[1] = _ij5[1];
3077 vinfos[5].maxsolutions = _nj5;
3078 std::vector<int> vfree(0);
3079 solutions.AddSolution(vinfos,vfree);
3080 }
3081 }
3082 }
3083 
3084 }
3085 } while(0);
3086 if( bgotonextstatement )
3087 {
3088 bool bgotonextstatement = true;
3089 do
3090 {
3091 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3092 evalcond[1]=new_r02;
3093 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3094 {
3095 bgotonextstatement=false;
3096 {
3097 IkReal j5array[1], cj5array[1], sj5array[1];
3098 bool j5valid[1]={false};
3099 _nj5 = 1;
3100 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3101  continue;
3102 j5array[0]=IKatan2(new_r00, new_r01);
3103 sj5array[0]=IKsin(j5array[0]);
3104 cj5array[0]=IKcos(j5array[0]);
3105 if( j5array[0] > IKPI )
3106 {
3107  j5array[0]-=IK2PI;
3108 }
3109 else if( j5array[0] < -IKPI )
3110 { j5array[0]+=IK2PI;
3111 }
3112 j5valid[0] = true;
3113 for(int ij5 = 0; ij5 < 1; ++ij5)
3114 {
3115 if( !j5valid[ij5] )
3116 {
3117  continue;
3118 }
3119 _ij5[0] = ij5; _ij5[1] = -1;
3120 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3121 {
3122 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3123 {
3124  j5valid[iij5]=false; _ij5[1] = iij5; break;
3125 }
3126 }
3127 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3128 {
3129 IkReal evalcond[8];
3130 IkReal x258=IKsin(j5);
3131 IkReal x259=IKcos(j5);
3132 IkReal x260=((1.0)*cj4);
3133 IkReal x261=((1.0)*x259);
3134 evalcond[0]=(((sj4*x258))+new_r21);
3135 evalcond[1]=(x258+(((-1.0)*new_r00)));
3136 evalcond[2]=(x259+(((-1.0)*new_r01)));
3137 evalcond[3]=((((-1.0)*sj4*x261))+new_r20);
3138 evalcond[4]=((((-1.0)*new_r11))+((cj4*x258)));
3139 evalcond[5]=((((-1.0)*x259*x260))+(((-1.0)*new_r10)));
3140 evalcond[6]=(x258+(((-1.0)*new_r11*x260))+((new_r21*sj4)));
3141 evalcond[7]=(((new_r20*sj4))+(((-1.0)*new_r10*x260))+(((-1.0)*x261)));
3142 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3143 {
3144 continue;
3145 }
3146 }
3147 
3148 {
3149 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3150 vinfos[0].jointtype = 1;
3151 vinfos[0].foffset = j0;
3152 vinfos[0].indices[0] = _ij0[0];
3153 vinfos[0].indices[1] = _ij0[1];
3154 vinfos[0].maxsolutions = _nj0;
3155 vinfos[1].jointtype = 1;
3156 vinfos[1].foffset = j1;
3157 vinfos[1].indices[0] = _ij1[0];
3158 vinfos[1].indices[1] = _ij1[1];
3159 vinfos[1].maxsolutions = _nj1;
3160 vinfos[2].jointtype = 1;
3161 vinfos[2].foffset = j2;
3162 vinfos[2].indices[0] = _ij2[0];
3163 vinfos[2].indices[1] = _ij2[1];
3164 vinfos[2].maxsolutions = _nj2;
3165 vinfos[3].jointtype = 1;
3166 vinfos[3].foffset = j3;
3167 vinfos[3].indices[0] = _ij3[0];
3168 vinfos[3].indices[1] = _ij3[1];
3169 vinfos[3].maxsolutions = _nj3;
3170 vinfos[4].jointtype = 1;
3171 vinfos[4].foffset = j4;
3172 vinfos[4].indices[0] = _ij4[0];
3173 vinfos[4].indices[1] = _ij4[1];
3174 vinfos[4].maxsolutions = _nj4;
3175 vinfos[5].jointtype = 1;
3176 vinfos[5].foffset = j5;
3177 vinfos[5].indices[0] = _ij5[0];
3178 vinfos[5].indices[1] = _ij5[1];
3179 vinfos[5].maxsolutions = _nj5;
3180 std::vector<int> vfree(0);
3181 solutions.AddSolution(vinfos,vfree);
3182 }
3183 }
3184 }
3185 
3186 }
3187 } while(0);
3188 if( bgotonextstatement )
3189 {
3190 bool bgotonextstatement = true;
3191 do
3192 {
3193 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3194 evalcond[1]=new_r12;
3195 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3196 {
3197 bgotonextstatement=false;
3198 {
3199 IkReal j5array[1], cj5array[1], sj5array[1];
3200 bool j5valid[1]={false};
3201 _nj5 = 1;
3202 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3203  continue;
3204 j5array[0]=IKatan2(new_r10, new_r11);
3205 sj5array[0]=IKsin(j5array[0]);
3206 cj5array[0]=IKcos(j5array[0]);
3207 if( j5array[0] > IKPI )
3208 {
3209  j5array[0]-=IK2PI;
3210 }
3211 else if( j5array[0] < -IKPI )
3212 { j5array[0]+=IK2PI;
3213 }
3214 j5valid[0] = true;
3215 for(int ij5 = 0; ij5 < 1; ++ij5)
3216 {
3217 if( !j5valid[ij5] )
3218 {
3219  continue;
3220 }
3221 _ij5[0] = ij5; _ij5[1] = -1;
3222 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3223 {
3224 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3225 {
3226  j5valid[iij5]=false; _ij5[1] = iij5; break;
3227 }
3228 }
3229 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3230 {
3231 IkReal evalcond[8];
3232 IkReal x262=IKcos(j5);
3233 IkReal x263=IKsin(j5);
3234 IkReal x264=((1.0)*x262);
3235 evalcond[0]=(new_r20+((new_r02*x262)));
3236 evalcond[1]=(x263+(((-1.0)*new_r10)));
3237 evalcond[2]=(x262+(((-1.0)*new_r11)));
3238 evalcond[3]=(((cj4*x263))+new_r01);
3239 evalcond[4]=((((-1.0)*new_r02*x263))+new_r21);
3240 evalcond[5]=((((-1.0)*cj4*x264))+new_r00);
3241 evalcond[6]=(((cj4*new_r01))+x263+((new_r21*sj4)));
3242 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x264)));
3243 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3244 {
3245 continue;
3246 }
3247 }
3248 
3249 {
3250 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3251 vinfos[0].jointtype = 1;
3252 vinfos[0].foffset = j0;
3253 vinfos[0].indices[0] = _ij0[0];
3254 vinfos[0].indices[1] = _ij0[1];
3255 vinfos[0].maxsolutions = _nj0;
3256 vinfos[1].jointtype = 1;
3257 vinfos[1].foffset = j1;
3258 vinfos[1].indices[0] = _ij1[0];
3259 vinfos[1].indices[1] = _ij1[1];
3260 vinfos[1].maxsolutions = _nj1;
3261 vinfos[2].jointtype = 1;
3262 vinfos[2].foffset = j2;
3263 vinfos[2].indices[0] = _ij2[0];
3264 vinfos[2].indices[1] = _ij2[1];
3265 vinfos[2].maxsolutions = _nj2;
3266 vinfos[3].jointtype = 1;
3267 vinfos[3].foffset = j3;
3268 vinfos[3].indices[0] = _ij3[0];
3269 vinfos[3].indices[1] = _ij3[1];
3270 vinfos[3].maxsolutions = _nj3;
3271 vinfos[4].jointtype = 1;
3272 vinfos[4].foffset = j4;
3273 vinfos[4].indices[0] = _ij4[0];
3274 vinfos[4].indices[1] = _ij4[1];
3275 vinfos[4].maxsolutions = _nj4;
3276 vinfos[5].jointtype = 1;
3277 vinfos[5].foffset = j5;
3278 vinfos[5].indices[0] = _ij5[0];
3279 vinfos[5].indices[1] = _ij5[1];
3280 vinfos[5].maxsolutions = _nj5;
3281 std::vector<int> vfree(0);
3282 solutions.AddSolution(vinfos,vfree);
3283 }
3284 }
3285 }
3286 
3287 }
3288 } while(0);
3289 if( bgotonextstatement )
3290 {
3291 bool bgotonextstatement = true;
3292 do
3293 {
3294 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3295 evalcond[1]=new_r12;
3296 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3297 {
3298 bgotonextstatement=false;
3299 {
3300 IkReal j5array[1], cj5array[1], sj5array[1];
3301 bool j5valid[1]={false};
3302 _nj5 = 1;
3303 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
3304  continue;
3305 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3306 sj5array[0]=IKsin(j5array[0]);
3307 cj5array[0]=IKcos(j5array[0]);
3308 if( j5array[0] > IKPI )
3309 {
3310  j5array[0]-=IK2PI;
3311 }
3312 else if( j5array[0] < -IKPI )
3313 { j5array[0]+=IK2PI;
3314 }
3315 j5valid[0] = true;
3316 for(int ij5 = 0; ij5 < 1; ++ij5)
3317 {
3318 if( !j5valid[ij5] )
3319 {
3320  continue;
3321 }
3322 _ij5[0] = ij5; _ij5[1] = -1;
3323 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3324 {
3325 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3326 {
3327  j5valid[iij5]=false; _ij5[1] = iij5; break;
3328 }
3329 }
3330 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3331 {
3332 IkReal evalcond[8];
3333 IkReal x265=IKsin(j5);
3334 IkReal x266=IKcos(j5);
3335 IkReal x267=((1.0)*new_r00);
3336 IkReal x268=((1.0)*new_r01);
3337 IkReal x269=((1.0)*x266);
3338 evalcond[0]=(x265+new_r10);
3339 evalcond[1]=(x266+new_r11);
3340 evalcond[2]=(new_r21+((new_r02*x265)));
3341 evalcond[3]=(new_r20+(((-1.0)*new_r02*x269)));
3342 evalcond[4]=(((cj4*x265))+(((-1.0)*x268)));
3343 evalcond[5]=((((-1.0)*cj4*x269))+(((-1.0)*x267)));
3344 evalcond[6]=((((-1.0)*cj4*x268))+x265+((new_r21*sj4)));
3345 evalcond[7]=((((-1.0)*cj4*x267))+((new_r20*sj4))+(((-1.0)*x269)));
3346 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3347 {
3348 continue;
3349 }
3350 }
3351 
3352 {
3353 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3354 vinfos[0].jointtype = 1;
3355 vinfos[0].foffset = j0;
3356 vinfos[0].indices[0] = _ij0[0];
3357 vinfos[0].indices[1] = _ij0[1];
3358 vinfos[0].maxsolutions = _nj0;
3359 vinfos[1].jointtype = 1;
3360 vinfos[1].foffset = j1;
3361 vinfos[1].indices[0] = _ij1[0];
3362 vinfos[1].indices[1] = _ij1[1];
3363 vinfos[1].maxsolutions = _nj1;
3364 vinfos[2].jointtype = 1;
3365 vinfos[2].foffset = j2;
3366 vinfos[2].indices[0] = _ij2[0];
3367 vinfos[2].indices[1] = _ij2[1];
3368 vinfos[2].maxsolutions = _nj2;
3369 vinfos[3].jointtype = 1;
3370 vinfos[3].foffset = j3;
3371 vinfos[3].indices[0] = _ij3[0];
3372 vinfos[3].indices[1] = _ij3[1];
3373 vinfos[3].maxsolutions = _nj3;
3374 vinfos[4].jointtype = 1;
3375 vinfos[4].foffset = j4;
3376 vinfos[4].indices[0] = _ij4[0];
3377 vinfos[4].indices[1] = _ij4[1];
3378 vinfos[4].maxsolutions = _nj4;
3379 vinfos[5].jointtype = 1;
3380 vinfos[5].foffset = j5;
3381 vinfos[5].indices[0] = _ij5[0];
3382 vinfos[5].indices[1] = _ij5[1];
3383 vinfos[5].maxsolutions = _nj5;
3384 std::vector<int> vfree(0);
3385 solutions.AddSolution(vinfos,vfree);
3386 }
3387 }
3388 }
3389 
3390 }
3391 } while(0);
3392 if( bgotonextstatement )
3393 {
3394 bool bgotonextstatement = true;
3395 do
3396 {
3397 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3398 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3399 {
3400 bgotonextstatement=false;
3401 {
3402 IkReal j5eval[1];
3403 new_r21=0;
3404 new_r20=0;
3405 new_r02=0;
3406 new_r12=0;
3407 j5eval[0]=1.0;
3408 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3409 {
3410 continue; // no branches [j5]
3411 
3412 } else
3413 {
3414 IkReal op[2+1], zeror[2];
3415 int numroots;
3416 op[0]=-1.0;
3417 op[1]=0;
3418 op[2]=1.0;
3419 polyroots2(op,zeror,numroots);
3420 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3421 int numsolutions = 0;
3422 for(int ij5 = 0; ij5 < numroots; ++ij5)
3423 {
3424 IkReal htj5 = zeror[ij5];
3425 tempj5array[0]=((2.0)*(atan(htj5)));
3426 for(int kj5 = 0; kj5 < 1; ++kj5)
3427 {
3428 j5array[numsolutions] = tempj5array[kj5];
3429 if( j5array[numsolutions] > IKPI )
3430 {
3431  j5array[numsolutions]-=IK2PI;
3432 }
3433 else if( j5array[numsolutions] < -IKPI )
3434 {
3435  j5array[numsolutions]+=IK2PI;
3436 }
3437 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3438 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3439 numsolutions++;
3440 }
3441 }
3442 bool j5valid[2]={true,true};
3443 _nj5 = 2;
3444 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3445  {
3446 if( !j5valid[ij5] )
3447 {
3448  continue;
3449 }
3450  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3451 htj5 = IKtan(j5/2);
3452 
3453 _ij5[0] = ij5; _ij5[1] = -1;
3454 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3455 {
3456 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3457 {
3458  j5valid[iij5]=false; _ij5[1] = iij5; break;
3459 }
3460 }
3461 {
3462 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3463 vinfos[0].jointtype = 1;
3464 vinfos[0].foffset = j0;
3465 vinfos[0].indices[0] = _ij0[0];
3466 vinfos[0].indices[1] = _ij0[1];
3467 vinfos[0].maxsolutions = _nj0;
3468 vinfos[1].jointtype = 1;
3469 vinfos[1].foffset = j1;
3470 vinfos[1].indices[0] = _ij1[0];
3471 vinfos[1].indices[1] = _ij1[1];
3472 vinfos[1].maxsolutions = _nj1;
3473 vinfos[2].jointtype = 1;
3474 vinfos[2].foffset = j2;
3475 vinfos[2].indices[0] = _ij2[0];
3476 vinfos[2].indices[1] = _ij2[1];
3477 vinfos[2].maxsolutions = _nj2;
3478 vinfos[3].jointtype = 1;
3479 vinfos[3].foffset = j3;
3480 vinfos[3].indices[0] = _ij3[0];
3481 vinfos[3].indices[1] = _ij3[1];
3482 vinfos[3].maxsolutions = _nj3;
3483 vinfos[4].jointtype = 1;
3484 vinfos[4].foffset = j4;
3485 vinfos[4].indices[0] = _ij4[0];
3486 vinfos[4].indices[1] = _ij4[1];
3487 vinfos[4].maxsolutions = _nj4;
3488 vinfos[5].jointtype = 1;
3489 vinfos[5].foffset = j5;
3490 vinfos[5].indices[0] = _ij5[0];
3491 vinfos[5].indices[1] = _ij5[1];
3492 vinfos[5].maxsolutions = _nj5;
3493 std::vector<int> vfree(0);
3494 solutions.AddSolution(vinfos,vfree);
3495 }
3496  }
3497 
3498 }
3499 
3500 }
3501 
3502 }
3503 } while(0);
3504 if( bgotonextstatement )
3505 {
3506 bool bgotonextstatement = true;
3507 do
3508 {
3509 if( 1 )
3510 {
3511 bgotonextstatement=false;
3512 continue; // branch miss [j5]
3513 
3514 }
3515 } while(0);
3516 if( bgotonextstatement )
3517 {
3518 }
3519 }
3520 }
3521 }
3522 }
3523 }
3524 }
3525 }
3526 }
3527 
3528 } else
3529 {
3530 {
3531 IkReal j5array[1], cj5array[1], sj5array[1];
3532 bool j5valid[1]={false};
3533 _nj5 = 1;
3534 CheckValue<IkReal> x271=IKPowWithIntegerCheck(sj4,-1);
3535 if(!x271.valid){
3536 continue;
3537 }
3538 IkReal x270=x271.value;
3539 CheckValue<IkReal> x272=IKPowWithIntegerCheck(cj3,-1);
3540 if(!x272.valid){
3541 continue;
3542 }
3543 if( IKabs(((-1.0)*new_r21*x270)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x270*(x272.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x270))+IKsqr((x270*(x272.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3544  continue;
3545 j5array[0]=IKatan2(((-1.0)*new_r21*x270), (x270*(x272.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3546 sj5array[0]=IKsin(j5array[0]);
3547 cj5array[0]=IKcos(j5array[0]);
3548 if( j5array[0] > IKPI )
3549 {
3550  j5array[0]-=IK2PI;
3551 }
3552 else if( j5array[0] < -IKPI )
3553 { j5array[0]+=IK2PI;
3554 }
3555 j5valid[0] = true;
3556 for(int ij5 = 0; ij5 < 1; ++ij5)
3557 {
3558 if( !j5valid[ij5] )
3559 {
3560  continue;
3561 }
3562 _ij5[0] = ij5; _ij5[1] = -1;
3563 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3564 {
3565 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3566 {
3567  j5valid[iij5]=false; _ij5[1] = iij5; break;
3568 }
3569 }
3570 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3571 {
3572 IkReal evalcond[12];
3573 IkReal x273=IKsin(j5);
3574 IkReal x274=IKcos(j5);
3575 IkReal x275=(cj3*new_r00);
3576 IkReal x276=(cj3*cj4);
3577 IkReal x277=(cj4*sj3);
3578 IkReal x278=((1.0)*cj3);
3579 IkReal x279=(sj3*x273);
3580 IkReal x280=((1.0)*x274);
3581 evalcond[0]=(new_r21+((sj4*x273)));
3582 evalcond[1]=((((-1.0)*sj4*x280))+new_r20);
3583 evalcond[2]=(((new_r00*sj3))+x273+(((-1.0)*new_r10*x278)));
3584 evalcond[3]=(((new_r01*sj3))+x274+(((-1.0)*new_r11*x278)));
3585 evalcond[4]=(((cj4*x273))+((new_r11*sj3))+((cj3*new_r01)));
3586 evalcond[5]=(((x273*x276))+((sj3*x274))+new_r01);
3587 evalcond[6]=(((new_r10*sj3))+x275+(((-1.0)*cj4*x280)));
3588 evalcond[7]=((((-1.0)*x276*x280))+x279+new_r00);
3589 evalcond[8]=(((x273*x277))+(((-1.0)*x274*x278))+new_r11);
3590 evalcond[9]=((((-1.0)*x273*x278))+(((-1.0)*x277*x280))+new_r10);
3591 evalcond[10]=(x273+((new_r01*x276))+((new_r21*sj4))+((new_r11*x277)));
3592 evalcond[11]=(((new_r20*sj4))+((cj4*x275))+(((-1.0)*x280))+((new_r10*x277)));
3593 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3594 {
3595 continue;
3596 }
3597 }
3598 
3599 {
3600 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3601 vinfos[0].jointtype = 1;
3602 vinfos[0].foffset = j0;
3603 vinfos[0].indices[0] = _ij0[0];
3604 vinfos[0].indices[1] = _ij0[1];
3605 vinfos[0].maxsolutions = _nj0;
3606 vinfos[1].jointtype = 1;
3607 vinfos[1].foffset = j1;
3608 vinfos[1].indices[0] = _ij1[0];
3609 vinfos[1].indices[1] = _ij1[1];
3610 vinfos[1].maxsolutions = _nj1;
3611 vinfos[2].jointtype = 1;
3612 vinfos[2].foffset = j2;
3613 vinfos[2].indices[0] = _ij2[0];
3614 vinfos[2].indices[1] = _ij2[1];
3615 vinfos[2].maxsolutions = _nj2;
3616 vinfos[3].jointtype = 1;
3617 vinfos[3].foffset = j3;
3618 vinfos[3].indices[0] = _ij3[0];
3619 vinfos[3].indices[1] = _ij3[1];
3620 vinfos[3].maxsolutions = _nj3;
3621 vinfos[4].jointtype = 1;
3622 vinfos[4].foffset = j4;
3623 vinfos[4].indices[0] = _ij4[0];
3624 vinfos[4].indices[1] = _ij4[1];
3625 vinfos[4].maxsolutions = _nj4;
3626 vinfos[5].jointtype = 1;
3627 vinfos[5].foffset = j5;
3628 vinfos[5].indices[0] = _ij5[0];
3629 vinfos[5].indices[1] = _ij5[1];
3630 vinfos[5].maxsolutions = _nj5;
3631 std::vector<int> vfree(0);
3632 solutions.AddSolution(vinfos,vfree);
3633 }
3634 }
3635 }
3636 
3637 }
3638 
3639 }
3640 
3641 } else
3642 {
3643 {
3644 IkReal j5array[1], cj5array[1], sj5array[1];
3645 bool j5valid[1]={false};
3646 _nj5 = 1;
3647 CheckValue<IkReal> x282=IKPowWithIntegerCheck(sj4,-1);
3648 if(!x282.valid){
3649 continue;
3650 }
3651 IkReal x281=x282.value;
3652 CheckValue<IkReal> x283=IKPowWithIntegerCheck(sj3,-1);
3653 if(!x283.valid){
3654 continue;
3655 }
3656 if( IKabs(((-1.0)*new_r21*x281)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x281*(x283.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x281))+IKsqr((x281*(x283.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3657  continue;
3658 j5array[0]=IKatan2(((-1.0)*new_r21*x281), (x281*(x283.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3659 sj5array[0]=IKsin(j5array[0]);
3660 cj5array[0]=IKcos(j5array[0]);
3661 if( j5array[0] > IKPI )
3662 {
3663  j5array[0]-=IK2PI;
3664 }
3665 else if( j5array[0] < -IKPI )
3666 { j5array[0]+=IK2PI;
3667 }
3668 j5valid[0] = true;
3669 for(int ij5 = 0; ij5 < 1; ++ij5)
3670 {
3671 if( !j5valid[ij5] )
3672 {
3673  continue;
3674 }
3675 _ij5[0] = ij5; _ij5[1] = -1;
3676 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3677 {
3678 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3679 {
3680  j5valid[iij5]=false; _ij5[1] = iij5; break;
3681 }
3682 }
3683 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3684 {
3685 IkReal evalcond[12];
3686 IkReal x284=IKsin(j5);
3687 IkReal x285=IKcos(j5);
3688 IkReal x286=(cj3*new_r00);
3689 IkReal x287=(cj3*cj4);
3690 IkReal x288=(cj4*sj3);
3691 IkReal x289=((1.0)*cj3);
3692 IkReal x290=(sj3*x284);
3693 IkReal x291=((1.0)*x285);
3694 evalcond[0]=(new_r21+((sj4*x284)));
3695 evalcond[1]=(new_r20+(((-1.0)*sj4*x291)));
3696 evalcond[2]=(((new_r00*sj3))+x284+(((-1.0)*new_r10*x289)));
3697 evalcond[3]=(((new_r01*sj3))+x285+(((-1.0)*new_r11*x289)));
3698 evalcond[4]=(((new_r11*sj3))+((cj3*new_r01))+((cj4*x284)));
3699 evalcond[5]=(((x284*x287))+((sj3*x285))+new_r01);
3700 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x291))+x286);
3701 evalcond[7]=(x290+(((-1.0)*x287*x291))+new_r00);
3702 evalcond[8]=(((x284*x288))+(((-1.0)*x285*x289))+new_r11);
3703 evalcond[9]=((((-1.0)*x284*x289))+(((-1.0)*x288*x291))+new_r10);
3704 evalcond[10]=(x284+((new_r21*sj4))+((new_r11*x288))+((new_r01*x287)));
3705 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x291))+((cj4*x286))+((new_r10*x288)));
3706 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3707 {
3708 continue;
3709 }
3710 }
3711 
3712 {
3713 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3714 vinfos[0].jointtype = 1;
3715 vinfos[0].foffset = j0;
3716 vinfos[0].indices[0] = _ij0[0];
3717 vinfos[0].indices[1] = _ij0[1];
3718 vinfos[0].maxsolutions = _nj0;
3719 vinfos[1].jointtype = 1;
3720 vinfos[1].foffset = j1;
3721 vinfos[1].indices[0] = _ij1[0];
3722 vinfos[1].indices[1] = _ij1[1];
3723 vinfos[1].maxsolutions = _nj1;
3724 vinfos[2].jointtype = 1;
3725 vinfos[2].foffset = j2;
3726 vinfos[2].indices[0] = _ij2[0];
3727 vinfos[2].indices[1] = _ij2[1];
3728 vinfos[2].maxsolutions = _nj2;
3729 vinfos[3].jointtype = 1;
3730 vinfos[3].foffset = j3;
3731 vinfos[3].indices[0] = _ij3[0];
3732 vinfos[3].indices[1] = _ij3[1];
3733 vinfos[3].maxsolutions = _nj3;
3734 vinfos[4].jointtype = 1;
3735 vinfos[4].foffset = j4;
3736 vinfos[4].indices[0] = _ij4[0];
3737 vinfos[4].indices[1] = _ij4[1];
3738 vinfos[4].maxsolutions = _nj4;
3739 vinfos[5].jointtype = 1;
3740 vinfos[5].foffset = j5;
3741 vinfos[5].indices[0] = _ij5[0];
3742 vinfos[5].indices[1] = _ij5[1];
3743 vinfos[5].maxsolutions = _nj5;
3744 std::vector<int> vfree(0);
3745 solutions.AddSolution(vinfos,vfree);
3746 }
3747 }
3748 }
3749 
3750 }
3751 
3752 }
3753 
3754 } else
3755 {
3756 {
3757 IkReal j5array[1], cj5array[1], sj5array[1];
3758 bool j5valid[1]={false};
3759 _nj5 = 1;
3760 CheckValue<IkReal> x292 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3761 if(!x292.valid){
3762 continue;
3763 }
3764 CheckValue<IkReal> x293=IKPowWithIntegerCheck(IKsign(sj4),-1);
3765 if(!x293.valid){
3766 continue;
3767 }
3768 j5array[0]=((-1.5707963267949)+(x292.value)+(((1.5707963267949)*(x293.value))));
3769 sj5array[0]=IKsin(j5array[0]);
3770 cj5array[0]=IKcos(j5array[0]);
3771 if( j5array[0] > IKPI )
3772 {
3773  j5array[0]-=IK2PI;
3774 }
3775 else if( j5array[0] < -IKPI )
3776 { j5array[0]+=IK2PI;
3777 }
3778 j5valid[0] = true;
3779 for(int ij5 = 0; ij5 < 1; ++ij5)
3780 {
3781 if( !j5valid[ij5] )
3782 {
3783  continue;
3784 }
3785 _ij5[0] = ij5; _ij5[1] = -1;
3786 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3787 {
3788 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3789 {
3790  j5valid[iij5]=false; _ij5[1] = iij5; break;
3791 }
3792 }
3793 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3794 {
3795 IkReal evalcond[12];
3796 IkReal x294=IKsin(j5);
3797 IkReal x295=IKcos(j5);
3798 IkReal x296=(cj3*new_r00);
3799 IkReal x297=(cj3*cj4);
3800 IkReal x298=(cj4*sj3);
3801 IkReal x299=((1.0)*cj3);
3802 IkReal x300=(sj3*x294);
3803 IkReal x301=((1.0)*x295);
3804 evalcond[0]=(((sj4*x294))+new_r21);
3805 evalcond[1]=((((-1.0)*sj4*x301))+new_r20);
3806 evalcond[2]=((((-1.0)*new_r10*x299))+((new_r00*sj3))+x294);
3807 evalcond[3]=((((-1.0)*new_r11*x299))+((new_r01*sj3))+x295);
3808 evalcond[4]=(((new_r11*sj3))+((cj4*x294))+((cj3*new_r01)));
3809 evalcond[5]=(((x294*x297))+new_r01+((sj3*x295)));
3810 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x301))+x296);
3811 evalcond[7]=((((-1.0)*x297*x301))+x300+new_r00);
3812 evalcond[8]=((((-1.0)*x295*x299))+((x294*x298))+new_r11);
3813 evalcond[9]=((((-1.0)*x298*x301))+new_r10+(((-1.0)*x294*x299)));
3814 evalcond[10]=(((new_r11*x298))+x294+((new_r01*x297))+((new_r21*sj4)));
3815 evalcond[11]=(((new_r20*sj4))+((new_r10*x298))+((cj4*x296))+(((-1.0)*x301)));
3816 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3817 {
3818 continue;
3819 }
3820 }
3821 
3822 {
3823 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3824 vinfos[0].jointtype = 1;
3825 vinfos[0].foffset = j0;
3826 vinfos[0].indices[0] = _ij0[0];
3827 vinfos[0].indices[1] = _ij0[1];
3828 vinfos[0].maxsolutions = _nj0;
3829 vinfos[1].jointtype = 1;
3830 vinfos[1].foffset = j1;
3831 vinfos[1].indices[0] = _ij1[0];
3832 vinfos[1].indices[1] = _ij1[1];
3833 vinfos[1].maxsolutions = _nj1;
3834 vinfos[2].jointtype = 1;
3835 vinfos[2].foffset = j2;
3836 vinfos[2].indices[0] = _ij2[0];
3837 vinfos[2].indices[1] = _ij2[1];
3838 vinfos[2].maxsolutions = _nj2;
3839 vinfos[3].jointtype = 1;
3840 vinfos[3].foffset = j3;
3841 vinfos[3].indices[0] = _ij3[0];
3842 vinfos[3].indices[1] = _ij3[1];
3843 vinfos[3].maxsolutions = _nj3;
3844 vinfos[4].jointtype = 1;
3845 vinfos[4].foffset = j4;
3846 vinfos[4].indices[0] = _ij4[0];
3847 vinfos[4].indices[1] = _ij4[1];
3848 vinfos[4].maxsolutions = _nj4;
3849 vinfos[5].jointtype = 1;
3850 vinfos[5].foffset = j5;
3851 vinfos[5].indices[0] = _ij5[0];
3852 vinfos[5].indices[1] = _ij5[1];
3853 vinfos[5].maxsolutions = _nj5;
3854 std::vector<int> vfree(0);
3855 solutions.AddSolution(vinfos,vfree);
3856 }
3857 }
3858 }
3859 
3860 }
3861 
3862 }
3863 }
3864 }
3865 
3866 }
3867 
3868 }
3869 
3870 } else
3871 {
3872 {
3873 IkReal j3array[1], cj3array[1], sj3array[1];
3874 bool j3valid[1]={false};
3875 _nj3 = 1;
3876 CheckValue<IkReal> x302=IKPowWithIntegerCheck(IKsign(sj4),-1);
3877 if(!x302.valid){
3878 continue;
3879 }
3880 CheckValue<IkReal> x303 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3881 if(!x303.valid){
3882 continue;
3883 }
3884 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x302.value)))+(x303.value));
3885 sj3array[0]=IKsin(j3array[0]);
3886 cj3array[0]=IKcos(j3array[0]);
3887 if( j3array[0] > IKPI )
3888 {
3889  j3array[0]-=IK2PI;
3890 }
3891 else if( j3array[0] < -IKPI )
3892 { j3array[0]+=IK2PI;
3893 }
3894 j3valid[0] = true;
3895 for(int ij3 = 0; ij3 < 1; ++ij3)
3896 {
3897 if( !j3valid[ij3] )
3898 {
3899  continue;
3900 }
3901 _ij3[0] = ij3; _ij3[1] = -1;
3902 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3903 {
3904 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3905 {
3906  j3valid[iij3]=false; _ij3[1] = iij3; break;
3907 }
3908 }
3909 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3910 {
3911 IkReal evalcond[8];
3912 IkReal x304=IKcos(j3);
3913 IkReal x305=IKsin(j3);
3914 IkReal x306=((1.0)*cj4);
3915 IkReal x307=(new_r02*x304);
3916 IkReal x308=(sj4*x305);
3917 IkReal x309=(sj4*x304);
3918 IkReal x310=(new_r12*x305);
3919 evalcond[0]=(x309+new_r02);
3920 evalcond[1]=(x308+new_r12);
3921 evalcond[2]=((((-1.0)*new_r02*x305))+((new_r12*x304)));
3922 evalcond[3]=(sj4+x307+x310);
3923 evalcond[4]=(((cj4*x310))+((new_r22*sj4))+((cj4*x307)));
3924 evalcond[5]=(((new_r00*x309))+((new_r10*x308))+(((-1.0)*new_r20*x306)));
3925 evalcond[6]=(((new_r01*x309))+((new_r11*x308))+(((-1.0)*new_r21*x306)));
3926 evalcond[7]=((1.0)+(((-1.0)*new_r22*x306))+((new_r12*x308))+((sj4*x307)));
3927 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3928 {
3929 continue;
3930 }
3931 }
3932 
3933 {
3934 IkReal j5eval[3];
3935 j5eval[0]=sj4;
3936 j5eval[1]=IKsign(sj4);
3937 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
3938 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
3939 {
3940 {
3941 IkReal j5eval[2];
3942 j5eval[0]=sj4;
3943 j5eval[1]=sj3;
3944 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3945 {
3946 {
3947 IkReal j5eval[2];
3948 j5eval[0]=sj4;
3949 j5eval[1]=cj3;
3950 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
3951 {
3952 {
3953 IkReal evalcond[5];
3954 bool bgotonextstatement = true;
3955 do
3956 {
3957 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
3958 evalcond[1]=new_r02;
3959 evalcond[2]=new_r12;
3960 evalcond[3]=new_r21;
3961 evalcond[4]=new_r20;
3962 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 )
3963 {
3964 bgotonextstatement=false;
3965 {
3966 IkReal j5array[1], cj5array[1], sj5array[1];
3967 bool j5valid[1]={false};
3968 _nj5 = 1;
3969 IkReal x311=((1.0)*new_r01);
3970 if( IKabs(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x311)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x311))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x311))))+IKsqr(((((-1.0)*sj3*x311))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
3971  continue;
3972 j5array[0]=IKatan2(((((-1.0)*new_r00*sj3))+(((-1.0)*cj3*x311))), ((((-1.0)*sj3*x311))+((cj3*new_r00))));
3973 sj5array[0]=IKsin(j5array[0]);
3974 cj5array[0]=IKcos(j5array[0]);
3975 if( j5array[0] > IKPI )
3976 {
3977  j5array[0]-=IK2PI;
3978 }
3979 else if( j5array[0] < -IKPI )
3980 { j5array[0]+=IK2PI;
3981 }
3982 j5valid[0] = true;
3983 for(int ij5 = 0; ij5 < 1; ++ij5)
3984 {
3985 if( !j5valid[ij5] )
3986 {
3987  continue;
3988 }
3989 _ij5[0] = ij5; _ij5[1] = -1;
3990 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3991 {
3992 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3993 {
3994  j5valid[iij5]=false; _ij5[1] = iij5; break;
3995 }
3996 }
3997 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3998 {
3999 IkReal evalcond[8];
4000 IkReal x312=IKsin(j5);
4001 IkReal x313=IKcos(j5);
4002 IkReal x314=((1.0)*cj3);
4003 IkReal x315=(sj3*x312);
4004 IkReal x316=((1.0)*x313);
4005 IkReal x317=(x313*x314);
4006 evalcond[0]=(((new_r11*sj3))+x312+((cj3*new_r01)));
4007 evalcond[1]=(((new_r00*sj3))+x312+(((-1.0)*new_r10*x314)));
4008 evalcond[2]=(((new_r01*sj3))+(((-1.0)*new_r11*x314))+x313);
4009 evalcond[3]=(((cj3*x312))+new_r01+((sj3*x313)));
4010 evalcond[4]=(((new_r10*sj3))+(((-1.0)*x316))+((cj3*new_r00)));
4011 evalcond[5]=(x315+(((-1.0)*x317))+new_r00);
4012 evalcond[6]=(x315+(((-1.0)*x317))+new_r11);
4013 evalcond[7]=((((-1.0)*x312*x314))+(((-1.0)*sj3*x316))+new_r10);
4014 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4015 {
4016 continue;
4017 }
4018 }
4019 
4020 {
4021 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4022 vinfos[0].jointtype = 1;
4023 vinfos[0].foffset = j0;
4024 vinfos[0].indices[0] = _ij0[0];
4025 vinfos[0].indices[1] = _ij0[1];
4026 vinfos[0].maxsolutions = _nj0;
4027 vinfos[1].jointtype = 1;
4028 vinfos[1].foffset = j1;
4029 vinfos[1].indices[0] = _ij1[0];
4030 vinfos[1].indices[1] = _ij1[1];
4031 vinfos[1].maxsolutions = _nj1;
4032 vinfos[2].jointtype = 1;
4033 vinfos[2].foffset = j2;
4034 vinfos[2].indices[0] = _ij2[0];
4035 vinfos[2].indices[1] = _ij2[1];
4036 vinfos[2].maxsolutions = _nj2;
4037 vinfos[3].jointtype = 1;
4038 vinfos[3].foffset = j3;
4039 vinfos[3].indices[0] = _ij3[0];
4040 vinfos[3].indices[1] = _ij3[1];
4041 vinfos[3].maxsolutions = _nj3;
4042 vinfos[4].jointtype = 1;
4043 vinfos[4].foffset = j4;
4044 vinfos[4].indices[0] = _ij4[0];
4045 vinfos[4].indices[1] = _ij4[1];
4046 vinfos[4].maxsolutions = _nj4;
4047 vinfos[5].jointtype = 1;
4048 vinfos[5].foffset = j5;
4049 vinfos[5].indices[0] = _ij5[0];
4050 vinfos[5].indices[1] = _ij5[1];
4051 vinfos[5].maxsolutions = _nj5;
4052 std::vector<int> vfree(0);
4053 solutions.AddSolution(vinfos,vfree);
4054 }
4055 }
4056 }
4057 
4058 }
4059 } while(0);
4060 if( bgotonextstatement )
4061 {
4062 bool bgotonextstatement = true;
4063 do
4064 {
4065 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4066 evalcond[1]=new_r02;
4067 evalcond[2]=new_r12;
4068 evalcond[3]=new_r21;
4069 evalcond[4]=new_r20;
4070 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 )
4071 {
4072 bgotonextstatement=false;
4073 {
4074 IkReal j5array[1], cj5array[1], sj5array[1];
4075 bool j5valid[1]={false};
4076 _nj5 = 1;
4077 IkReal x318=((1.0)*sj3);
4078 if( IKabs(((((-1.0)*new_r00*x318))+((cj3*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x318)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r00*x318))+((cj3*new_r01))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x318))))-1) <= IKFAST_SINCOS_THRESH )
4079  continue;
4080 j5array[0]=IKatan2(((((-1.0)*new_r00*x318))+((cj3*new_r01))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x318))));
4081 sj5array[0]=IKsin(j5array[0]);
4082 cj5array[0]=IKcos(j5array[0]);
4083 if( j5array[0] > IKPI )
4084 {
4085  j5array[0]-=IK2PI;
4086 }
4087 else if( j5array[0] < -IKPI )
4088 { j5array[0]+=IK2PI;
4089 }
4090 j5valid[0] = true;
4091 for(int ij5 = 0; ij5 < 1; ++ij5)
4092 {
4093 if( !j5valid[ij5] )
4094 {
4095  continue;
4096 }
4097 _ij5[0] = ij5; _ij5[1] = -1;
4098 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4099 {
4100 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4101 {
4102  j5valid[iij5]=false; _ij5[1] = iij5; break;
4103 }
4104 }
4105 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4106 {
4107 IkReal evalcond[8];
4108 IkReal x319=IKcos(j5);
4109 IkReal x320=IKsin(j5);
4110 IkReal x321=((1.0)*cj3);
4111 IkReal x322=(sj3*x319);
4112 IkReal x323=((1.0)*x320);
4113 IkReal x324=(x320*x321);
4114 evalcond[0]=(((new_r10*sj3))+x319+((cj3*new_r00)));
4115 evalcond[1]=(((new_r00*sj3))+x320+(((-1.0)*new_r10*x321)));
4116 evalcond[2]=(((new_r01*sj3))+x319+(((-1.0)*new_r11*x321)));
4117 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x323))+((cj3*new_r01)));
4118 evalcond[4]=(((sj3*x320))+((cj3*x319))+new_r00);
4119 evalcond[5]=((((-1.0)*x324))+x322+new_r01);
4120 evalcond[6]=((((-1.0)*x324))+x322+new_r10);
4121 evalcond[7]=((((-1.0)*x319*x321))+new_r11+(((-1.0)*sj3*x323)));
4122 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4123 {
4124 continue;
4125 }
4126 }
4127 
4128 {
4129 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4130 vinfos[0].jointtype = 1;
4131 vinfos[0].foffset = j0;
4132 vinfos[0].indices[0] = _ij0[0];
4133 vinfos[0].indices[1] = _ij0[1];
4134 vinfos[0].maxsolutions = _nj0;
4135 vinfos[1].jointtype = 1;
4136 vinfos[1].foffset = j1;
4137 vinfos[1].indices[0] = _ij1[0];
4138 vinfos[1].indices[1] = _ij1[1];
4139 vinfos[1].maxsolutions = _nj1;
4140 vinfos[2].jointtype = 1;
4141 vinfos[2].foffset = j2;
4142 vinfos[2].indices[0] = _ij2[0];
4143 vinfos[2].indices[1] = _ij2[1];
4144 vinfos[2].maxsolutions = _nj2;
4145 vinfos[3].jointtype = 1;
4146 vinfos[3].foffset = j3;
4147 vinfos[3].indices[0] = _ij3[0];
4148 vinfos[3].indices[1] = _ij3[1];
4149 vinfos[3].maxsolutions = _nj3;
4150 vinfos[4].jointtype = 1;
4151 vinfos[4].foffset = j4;
4152 vinfos[4].indices[0] = _ij4[0];
4153 vinfos[4].indices[1] = _ij4[1];
4154 vinfos[4].maxsolutions = _nj4;
4155 vinfos[5].jointtype = 1;
4156 vinfos[5].foffset = j5;
4157 vinfos[5].indices[0] = _ij5[0];
4158 vinfos[5].indices[1] = _ij5[1];
4159 vinfos[5].maxsolutions = _nj5;
4160 std::vector<int> vfree(0);
4161 solutions.AddSolution(vinfos,vfree);
4162 }
4163 }
4164 }
4165 
4166 }
4167 } while(0);
4168 if( bgotonextstatement )
4169 {
4170 bool bgotonextstatement = true;
4171 do
4172 {
4173 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4174 evalcond[1]=new_r02;
4175 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4176 {
4177 bgotonextstatement=false;
4178 {
4179 IkReal j5array[1], cj5array[1], sj5array[1];
4180 bool j5valid[1]={false};
4181 _nj5 = 1;
4182 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
4183  continue;
4184 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4185 sj5array[0]=IKsin(j5array[0]);
4186 cj5array[0]=IKcos(j5array[0]);
4187 if( j5array[0] > IKPI )
4188 {
4189  j5array[0]-=IK2PI;
4190 }
4191 else if( j5array[0] < -IKPI )
4192 { j5array[0]+=IK2PI;
4193 }
4194 j5valid[0] = true;
4195 for(int ij5 = 0; ij5 < 1; ++ij5)
4196 {
4197 if( !j5valid[ij5] )
4198 {
4199  continue;
4200 }
4201 _ij5[0] = ij5; _ij5[1] = -1;
4202 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4203 {
4204 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4205 {
4206  j5valid[iij5]=false; _ij5[1] = iij5; break;
4207 }
4208 }
4209 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4210 {
4211 IkReal evalcond[8];
4212 IkReal x325=IKsin(j5);
4213 IkReal x326=IKcos(j5);
4214 IkReal x327=((1.0)*x326);
4215 evalcond[0]=(x325+new_r00);
4216 evalcond[1]=(x326+new_r01);
4217 evalcond[2]=(((sj4*x325))+new_r21);
4218 evalcond[3]=(((cj4*x325))+new_r11);
4219 evalcond[4]=(new_r20+(((-1.0)*sj4*x327)));
4220 evalcond[5]=(new_r10+(((-1.0)*cj4*x327)));
4221 evalcond[6]=(((cj4*new_r11))+x325+((new_r21*sj4)));
4222 evalcond[7]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x327)));
4223 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4224 {
4225 continue;
4226 }
4227 }
4228 
4229 {
4230 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4231 vinfos[0].jointtype = 1;
4232 vinfos[0].foffset = j0;
4233 vinfos[0].indices[0] = _ij0[0];
4234 vinfos[0].indices[1] = _ij0[1];
4235 vinfos[0].maxsolutions = _nj0;
4236 vinfos[1].jointtype = 1;
4237 vinfos[1].foffset = j1;
4238 vinfos[1].indices[0] = _ij1[0];
4239 vinfos[1].indices[1] = _ij1[1];
4240 vinfos[1].maxsolutions = _nj1;
4241 vinfos[2].jointtype = 1;
4242 vinfos[2].foffset = j2;
4243 vinfos[2].indices[0] = _ij2[0];
4244 vinfos[2].indices[1] = _ij2[1];
4245 vinfos[2].maxsolutions = _nj2;
4246 vinfos[3].jointtype = 1;
4247 vinfos[3].foffset = j3;
4248 vinfos[3].indices[0] = _ij3[0];
4249 vinfos[3].indices[1] = _ij3[1];
4250 vinfos[3].maxsolutions = _nj3;
4251 vinfos[4].jointtype = 1;
4252 vinfos[4].foffset = j4;
4253 vinfos[4].indices[0] = _ij4[0];
4254 vinfos[4].indices[1] = _ij4[1];
4255 vinfos[4].maxsolutions = _nj4;
4256 vinfos[5].jointtype = 1;
4257 vinfos[5].foffset = j5;
4258 vinfos[5].indices[0] = _ij5[0];
4259 vinfos[5].indices[1] = _ij5[1];
4260 vinfos[5].maxsolutions = _nj5;
4261 std::vector<int> vfree(0);
4262 solutions.AddSolution(vinfos,vfree);
4263 }
4264 }
4265 }
4266 
4267 }
4268 } while(0);
4269 if( bgotonextstatement )
4270 {
4271 bool bgotonextstatement = true;
4272 do
4273 {
4274 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4275 evalcond[1]=new_r02;
4276 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4277 {
4278 bgotonextstatement=false;
4279 {
4280 IkReal j5array[1], cj5array[1], sj5array[1];
4281 bool j5valid[1]={false};
4282 _nj5 = 1;
4283 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4284  continue;
4285 j5array[0]=IKatan2(new_r00, new_r01);
4286 sj5array[0]=IKsin(j5array[0]);
4287 cj5array[0]=IKcos(j5array[0]);
4288 if( j5array[0] > IKPI )
4289 {
4290  j5array[0]-=IK2PI;
4291 }
4292 else if( j5array[0] < -IKPI )
4293 { j5array[0]+=IK2PI;
4294 }
4295 j5valid[0] = true;
4296 for(int ij5 = 0; ij5 < 1; ++ij5)
4297 {
4298 if( !j5valid[ij5] )
4299 {
4300  continue;
4301 }
4302 _ij5[0] = ij5; _ij5[1] = -1;
4303 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4304 {
4305 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4306 {
4307  j5valid[iij5]=false; _ij5[1] = iij5; break;
4308 }
4309 }
4310 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4311 {
4312 IkReal evalcond[8];
4313 IkReal x328=IKsin(j5);
4314 IkReal x329=IKcos(j5);
4315 IkReal x330=((1.0)*cj4);
4316 IkReal x331=((1.0)*x329);
4317 evalcond[0]=(((sj4*x328))+new_r21);
4318 evalcond[1]=(x328+(((-1.0)*new_r00)));
4319 evalcond[2]=(x329+(((-1.0)*new_r01)));
4320 evalcond[3]=((((-1.0)*sj4*x331))+new_r20);
4321 evalcond[4]=(((cj4*x328))+(((-1.0)*new_r11)));
4322 evalcond[5]=((((-1.0)*x329*x330))+(((-1.0)*new_r10)));
4323 evalcond[6]=((((-1.0)*new_r11*x330))+x328+((new_r21*sj4)));
4324 evalcond[7]=((((-1.0)*new_r10*x330))+((new_r20*sj4))+(((-1.0)*x331)));
4325 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4326 {
4327 continue;
4328 }
4329 }
4330 
4331 {
4332 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4333 vinfos[0].jointtype = 1;
4334 vinfos[0].foffset = j0;
4335 vinfos[0].indices[0] = _ij0[0];
4336 vinfos[0].indices[1] = _ij0[1];
4337 vinfos[0].maxsolutions = _nj0;
4338 vinfos[1].jointtype = 1;
4339 vinfos[1].foffset = j1;
4340 vinfos[1].indices[0] = _ij1[0];
4341 vinfos[1].indices[1] = _ij1[1];
4342 vinfos[1].maxsolutions = _nj1;
4343 vinfos[2].jointtype = 1;
4344 vinfos[2].foffset = j2;
4345 vinfos[2].indices[0] = _ij2[0];
4346 vinfos[2].indices[1] = _ij2[1];
4347 vinfos[2].maxsolutions = _nj2;
4348 vinfos[3].jointtype = 1;
4349 vinfos[3].foffset = j3;
4350 vinfos[3].indices[0] = _ij3[0];
4351 vinfos[3].indices[1] = _ij3[1];
4352 vinfos[3].maxsolutions = _nj3;
4353 vinfos[4].jointtype = 1;
4354 vinfos[4].foffset = j4;
4355 vinfos[4].indices[0] = _ij4[0];
4356 vinfos[4].indices[1] = _ij4[1];
4357 vinfos[4].maxsolutions = _nj4;
4358 vinfos[5].jointtype = 1;
4359 vinfos[5].foffset = j5;
4360 vinfos[5].indices[0] = _ij5[0];
4361 vinfos[5].indices[1] = _ij5[1];
4362 vinfos[5].maxsolutions = _nj5;
4363 std::vector<int> vfree(0);
4364 solutions.AddSolution(vinfos,vfree);
4365 }
4366 }
4367 }
4368 
4369 }
4370 } while(0);
4371 if( bgotonextstatement )
4372 {
4373 bool bgotonextstatement = true;
4374 do
4375 {
4376 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4377 evalcond[1]=new_r12;
4378 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4379 {
4380 bgotonextstatement=false;
4381 {
4382 IkReal j5array[1], cj5array[1], sj5array[1];
4383 bool j5valid[1]={false};
4384 _nj5 = 1;
4385 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4386  continue;
4387 j5array[0]=IKatan2(new_r10, new_r11);
4388 sj5array[0]=IKsin(j5array[0]);
4389 cj5array[0]=IKcos(j5array[0]);
4390 if( j5array[0] > IKPI )
4391 {
4392  j5array[0]-=IK2PI;
4393 }
4394 else if( j5array[0] < -IKPI )
4395 { j5array[0]+=IK2PI;
4396 }
4397 j5valid[0] = true;
4398 for(int ij5 = 0; ij5 < 1; ++ij5)
4399 {
4400 if( !j5valid[ij5] )
4401 {
4402  continue;
4403 }
4404 _ij5[0] = ij5; _ij5[1] = -1;
4405 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4406 {
4407 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4408 {
4409  j5valid[iij5]=false; _ij5[1] = iij5; break;
4410 }
4411 }
4412 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4413 {
4414 IkReal evalcond[8];
4415 IkReal x332=IKcos(j5);
4416 IkReal x333=IKsin(j5);
4417 IkReal x334=((1.0)*x332);
4418 evalcond[0]=(((new_r02*x332))+new_r20);
4419 evalcond[1]=(x333+(((-1.0)*new_r10)));
4420 evalcond[2]=(x332+(((-1.0)*new_r11)));
4421 evalcond[3]=(((cj4*x333))+new_r01);
4422 evalcond[4]=((((-1.0)*new_r02*x333))+new_r21);
4423 evalcond[5]=((((-1.0)*cj4*x334))+new_r00);
4424 evalcond[6]=(((cj4*new_r01))+x333+((new_r21*sj4)));
4425 evalcond[7]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x334)));
4426 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4427 {
4428 continue;
4429 }
4430 }
4431 
4432 {
4433 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4434 vinfos[0].jointtype = 1;
4435 vinfos[0].foffset = j0;
4436 vinfos[0].indices[0] = _ij0[0];
4437 vinfos[0].indices[1] = _ij0[1];
4438 vinfos[0].maxsolutions = _nj0;
4439 vinfos[1].jointtype = 1;
4440 vinfos[1].foffset = j1;
4441 vinfos[1].indices[0] = _ij1[0];
4442 vinfos[1].indices[1] = _ij1[1];
4443 vinfos[1].maxsolutions = _nj1;
4444 vinfos[2].jointtype = 1;
4445 vinfos[2].foffset = j2;
4446 vinfos[2].indices[0] = _ij2[0];
4447 vinfos[2].indices[1] = _ij2[1];
4448 vinfos[2].maxsolutions = _nj2;
4449 vinfos[3].jointtype = 1;
4450 vinfos[3].foffset = j3;
4451 vinfos[3].indices[0] = _ij3[0];
4452 vinfos[3].indices[1] = _ij3[1];
4453 vinfos[3].maxsolutions = _nj3;
4454 vinfos[4].jointtype = 1;
4455 vinfos[4].foffset = j4;
4456 vinfos[4].indices[0] = _ij4[0];
4457 vinfos[4].indices[1] = _ij4[1];
4458 vinfos[4].maxsolutions = _nj4;
4459 vinfos[5].jointtype = 1;
4460 vinfos[5].foffset = j5;
4461 vinfos[5].indices[0] = _ij5[0];
4462 vinfos[5].indices[1] = _ij5[1];
4463 vinfos[5].maxsolutions = _nj5;
4464 std::vector<int> vfree(0);
4465 solutions.AddSolution(vinfos,vfree);
4466 }
4467 }
4468 }
4469 
4470 }
4471 } while(0);
4472 if( bgotonextstatement )
4473 {
4474 bool bgotonextstatement = true;
4475 do
4476 {
4477 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4478 evalcond[1]=new_r12;
4479 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4480 {
4481 bgotonextstatement=false;
4482 {
4483 IkReal j5array[1], cj5array[1], sj5array[1];
4484 bool j5valid[1]={false};
4485 _nj5 = 1;
4486 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
4487  continue;
4488 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4489 sj5array[0]=IKsin(j5array[0]);
4490 cj5array[0]=IKcos(j5array[0]);
4491 if( j5array[0] > IKPI )
4492 {
4493  j5array[0]-=IK2PI;
4494 }
4495 else if( j5array[0] < -IKPI )
4496 { j5array[0]+=IK2PI;
4497 }
4498 j5valid[0] = true;
4499 for(int ij5 = 0; ij5 < 1; ++ij5)
4500 {
4501 if( !j5valid[ij5] )
4502 {
4503  continue;
4504 }
4505 _ij5[0] = ij5; _ij5[1] = -1;
4506 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4507 {
4508 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4509 {
4510  j5valid[iij5]=false; _ij5[1] = iij5; break;
4511 }
4512 }
4513 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4514 {
4515 IkReal evalcond[8];
4516 IkReal x335=IKsin(j5);
4517 IkReal x336=IKcos(j5);
4518 IkReal x337=((1.0)*new_r00);
4519 IkReal x338=((1.0)*new_r01);
4520 IkReal x339=((1.0)*x336);
4521 evalcond[0]=(x335+new_r10);
4522 evalcond[1]=(x336+new_r11);
4523 evalcond[2]=(((new_r02*x335))+new_r21);
4524 evalcond[3]=(new_r20+(((-1.0)*new_r02*x339)));
4525 evalcond[4]=((((-1.0)*x338))+((cj4*x335)));
4526 evalcond[5]=((((-1.0)*cj4*x339))+(((-1.0)*x337)));
4527 evalcond[6]=((((-1.0)*cj4*x338))+x335+((new_r21*sj4)));
4528 evalcond[7]=(((new_r20*sj4))+(((-1.0)*cj4*x337))+(((-1.0)*x339)));
4529 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4530 {
4531 continue;
4532 }
4533 }
4534 
4535 {
4536 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4537 vinfos[0].jointtype = 1;
4538 vinfos[0].foffset = j0;
4539 vinfos[0].indices[0] = _ij0[0];
4540 vinfos[0].indices[1] = _ij0[1];
4541 vinfos[0].maxsolutions = _nj0;
4542 vinfos[1].jointtype = 1;
4543 vinfos[1].foffset = j1;
4544 vinfos[1].indices[0] = _ij1[0];
4545 vinfos[1].indices[1] = _ij1[1];
4546 vinfos[1].maxsolutions = _nj1;
4547 vinfos[2].jointtype = 1;
4548 vinfos[2].foffset = j2;
4549 vinfos[2].indices[0] = _ij2[0];
4550 vinfos[2].indices[1] = _ij2[1];
4551 vinfos[2].maxsolutions = _nj2;
4552 vinfos[3].jointtype = 1;
4553 vinfos[3].foffset = j3;
4554 vinfos[3].indices[0] = _ij3[0];
4555 vinfos[3].indices[1] = _ij3[1];
4556 vinfos[3].maxsolutions = _nj3;
4557 vinfos[4].jointtype = 1;
4558 vinfos[4].foffset = j4;
4559 vinfos[4].indices[0] = _ij4[0];
4560 vinfos[4].indices[1] = _ij4[1];
4561 vinfos[4].maxsolutions = _nj4;
4562 vinfos[5].jointtype = 1;
4563 vinfos[5].foffset = j5;
4564 vinfos[5].indices[0] = _ij5[0];
4565 vinfos[5].indices[1] = _ij5[1];
4566 vinfos[5].maxsolutions = _nj5;
4567 std::vector<int> vfree(0);
4568 solutions.AddSolution(vinfos,vfree);
4569 }
4570 }
4571 }
4572 
4573 }
4574 } while(0);
4575 if( bgotonextstatement )
4576 {
4577 bool bgotonextstatement = true;
4578 do
4579 {
4580 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4581 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4582 {
4583 bgotonextstatement=false;
4584 {
4585 IkReal j5eval[1];
4586 new_r21=0;
4587 new_r20=0;
4588 new_r02=0;
4589 new_r12=0;
4590 j5eval[0]=1.0;
4591 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4592 {
4593 continue; // no branches [j5]
4594 
4595 } else
4596 {
4597 IkReal op[2+1], zeror[2];
4598 int numroots;
4599 op[0]=-1.0;
4600 op[1]=0;
4601 op[2]=1.0;
4602 polyroots2(op,zeror,numroots);
4603 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4604 int numsolutions = 0;
4605 for(int ij5 = 0; ij5 < numroots; ++ij5)
4606 {
4607 IkReal htj5 = zeror[ij5];
4608 tempj5array[0]=((2.0)*(atan(htj5)));
4609 for(int kj5 = 0; kj5 < 1; ++kj5)
4610 {
4611 j5array[numsolutions] = tempj5array[kj5];
4612 if( j5array[numsolutions] > IKPI )
4613 {
4614  j5array[numsolutions]-=IK2PI;
4615 }
4616 else if( j5array[numsolutions] < -IKPI )
4617 {
4618  j5array[numsolutions]+=IK2PI;
4619 }
4620 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4621 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4622 numsolutions++;
4623 }
4624 }
4625 bool j5valid[2]={true,true};
4626 _nj5 = 2;
4627 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4628  {
4629 if( !j5valid[ij5] )
4630 {
4631  continue;
4632 }
4633  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4634 htj5 = IKtan(j5/2);
4635 
4636 _ij5[0] = ij5; _ij5[1] = -1;
4637 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4638 {
4639 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4640 {
4641  j5valid[iij5]=false; _ij5[1] = iij5; break;
4642 }
4643 }
4644 {
4645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4646 vinfos[0].jointtype = 1;
4647 vinfos[0].foffset = j0;
4648 vinfos[0].indices[0] = _ij0[0];
4649 vinfos[0].indices[1] = _ij0[1];
4650 vinfos[0].maxsolutions = _nj0;
4651 vinfos[1].jointtype = 1;
4652 vinfos[1].foffset = j1;
4653 vinfos[1].indices[0] = _ij1[0];
4654 vinfos[1].indices[1] = _ij1[1];
4655 vinfos[1].maxsolutions = _nj1;
4656 vinfos[2].jointtype = 1;
4657 vinfos[2].foffset = j2;
4658 vinfos[2].indices[0] = _ij2[0];
4659 vinfos[2].indices[1] = _ij2[1];
4660 vinfos[2].maxsolutions = _nj2;
4661 vinfos[3].jointtype = 1;
4662 vinfos[3].foffset = j3;
4663 vinfos[3].indices[0] = _ij3[0];
4664 vinfos[3].indices[1] = _ij3[1];
4665 vinfos[3].maxsolutions = _nj3;
4666 vinfos[4].jointtype = 1;
4667 vinfos[4].foffset = j4;
4668 vinfos[4].indices[0] = _ij4[0];
4669 vinfos[4].indices[1] = _ij4[1];
4670 vinfos[4].maxsolutions = _nj4;
4671 vinfos[5].jointtype = 1;
4672 vinfos[5].foffset = j5;
4673 vinfos[5].indices[0] = _ij5[0];
4674 vinfos[5].indices[1] = _ij5[1];
4675 vinfos[5].maxsolutions = _nj5;
4676 std::vector<int> vfree(0);
4677 solutions.AddSolution(vinfos,vfree);
4678 }
4679  }
4680 
4681 }
4682 
4683 }
4684 
4685 }
4686 } while(0);
4687 if( bgotonextstatement )
4688 {
4689 bool bgotonextstatement = true;
4690 do
4691 {
4692 if( 1 )
4693 {
4694 bgotonextstatement=false;
4695 continue; // branch miss [j5]
4696 
4697 }
4698 } while(0);
4699 if( bgotonextstatement )
4700 {
4701 }
4702 }
4703 }
4704 }
4705 }
4706 }
4707 }
4708 }
4709 }
4710 
4711 } else
4712 {
4713 {
4714 IkReal j5array[1], cj5array[1], sj5array[1];
4715 bool j5valid[1]={false};
4716 _nj5 = 1;
4717 CheckValue<IkReal> x341=IKPowWithIntegerCheck(sj4,-1);
4718 if(!x341.valid){
4719 continue;
4720 }
4721 IkReal x340=x341.value;
4722 CheckValue<IkReal> x342=IKPowWithIntegerCheck(cj3,-1);
4723 if(!x342.valid){
4724 continue;
4725 }
4726 if( IKabs(((-1.0)*new_r21*x340)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x340*(x342.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x340))+IKsqr((x340*(x342.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4727  continue;
4728 j5array[0]=IKatan2(((-1.0)*new_r21*x340), (x340*(x342.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4729 sj5array[0]=IKsin(j5array[0]);
4730 cj5array[0]=IKcos(j5array[0]);
4731 if( j5array[0] > IKPI )
4732 {
4733  j5array[0]-=IK2PI;
4734 }
4735 else if( j5array[0] < -IKPI )
4736 { j5array[0]+=IK2PI;
4737 }
4738 j5valid[0] = true;
4739 for(int ij5 = 0; ij5 < 1; ++ij5)
4740 {
4741 if( !j5valid[ij5] )
4742 {
4743  continue;
4744 }
4745 _ij5[0] = ij5; _ij5[1] = -1;
4746 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4747 {
4748 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4749 {
4750  j5valid[iij5]=false; _ij5[1] = iij5; break;
4751 }
4752 }
4753 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4754 {
4755 IkReal evalcond[12];
4756 IkReal x343=IKsin(j5);
4757 IkReal x344=IKcos(j5);
4758 IkReal x345=(cj3*new_r00);
4759 IkReal x346=(cj3*cj4);
4760 IkReal x347=(cj4*sj3);
4761 IkReal x348=((1.0)*cj3);
4762 IkReal x349=(sj3*x343);
4763 IkReal x350=((1.0)*x344);
4764 evalcond[0]=(((sj4*x343))+new_r21);
4765 evalcond[1]=(new_r20+(((-1.0)*sj4*x350)));
4766 evalcond[2]=((((-1.0)*new_r10*x348))+((new_r00*sj3))+x343);
4767 evalcond[3]=((((-1.0)*new_r11*x348))+((new_r01*sj3))+x344);
4768 evalcond[4]=(((new_r11*sj3))+((cj4*x343))+((cj3*new_r01)));
4769 evalcond[5]=(((sj3*x344))+new_r01+((x343*x346)));
4770 evalcond[6]=(((new_r10*sj3))+x345+(((-1.0)*cj4*x350)));
4771 evalcond[7]=((((-1.0)*x346*x350))+x349+new_r00);
4772 evalcond[8]=((((-1.0)*x344*x348))+new_r11+((x343*x347)));
4773 evalcond[9]=((((-1.0)*x347*x350))+(((-1.0)*x343*x348))+new_r10);
4774 evalcond[10]=(((new_r11*x347))+((new_r01*x346))+x343+((new_r21*sj4)));
4775 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x350))+((new_r10*x347))+((cj4*x345)));
4776 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4777 {
4778 continue;
4779 }
4780 }
4781 
4782 {
4783 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4784 vinfos[0].jointtype = 1;
4785 vinfos[0].foffset = j0;
4786 vinfos[0].indices[0] = _ij0[0];
4787 vinfos[0].indices[1] = _ij0[1];
4788 vinfos[0].maxsolutions = _nj0;
4789 vinfos[1].jointtype = 1;
4790 vinfos[1].foffset = j1;
4791 vinfos[1].indices[0] = _ij1[0];
4792 vinfos[1].indices[1] = _ij1[1];
4793 vinfos[1].maxsolutions = _nj1;
4794 vinfos[2].jointtype = 1;
4795 vinfos[2].foffset = j2;
4796 vinfos[2].indices[0] = _ij2[0];
4797 vinfos[2].indices[1] = _ij2[1];
4798 vinfos[2].maxsolutions = _nj2;
4799 vinfos[3].jointtype = 1;
4800 vinfos[3].foffset = j3;
4801 vinfos[3].indices[0] = _ij3[0];
4802 vinfos[3].indices[1] = _ij3[1];
4803 vinfos[3].maxsolutions = _nj3;
4804 vinfos[4].jointtype = 1;
4805 vinfos[4].foffset = j4;
4806 vinfos[4].indices[0] = _ij4[0];
4807 vinfos[4].indices[1] = _ij4[1];
4808 vinfos[4].maxsolutions = _nj4;
4809 vinfos[5].jointtype = 1;
4810 vinfos[5].foffset = j5;
4811 vinfos[5].indices[0] = _ij5[0];
4812 vinfos[5].indices[1] = _ij5[1];
4813 vinfos[5].maxsolutions = _nj5;
4814 std::vector<int> vfree(0);
4815 solutions.AddSolution(vinfos,vfree);
4816 }
4817 }
4818 }
4819 
4820 }
4821 
4822 }
4823 
4824 } else
4825 {
4826 {
4827 IkReal j5array[1], cj5array[1], sj5array[1];
4828 bool j5valid[1]={false};
4829 _nj5 = 1;
4830 CheckValue<IkReal> x352=IKPowWithIntegerCheck(sj4,-1);
4831 if(!x352.valid){
4832 continue;
4833 }
4834 IkReal x351=x352.value;
4835 CheckValue<IkReal> x353=IKPowWithIntegerCheck(sj3,-1);
4836 if(!x353.valid){
4837 continue;
4838 }
4839 if( IKabs(((-1.0)*new_r21*x351)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x351*(x353.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x351))+IKsqr((x351*(x353.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4840  continue;
4841 j5array[0]=IKatan2(((-1.0)*new_r21*x351), (x351*(x353.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4842 sj5array[0]=IKsin(j5array[0]);
4843 cj5array[0]=IKcos(j5array[0]);
4844 if( j5array[0] > IKPI )
4845 {
4846  j5array[0]-=IK2PI;
4847 }
4848 else if( j5array[0] < -IKPI )
4849 { j5array[0]+=IK2PI;
4850 }
4851 j5valid[0] = true;
4852 for(int ij5 = 0; ij5 < 1; ++ij5)
4853 {
4854 if( !j5valid[ij5] )
4855 {
4856  continue;
4857 }
4858 _ij5[0] = ij5; _ij5[1] = -1;
4859 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4860 {
4861 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4862 {
4863  j5valid[iij5]=false; _ij5[1] = iij5; break;
4864 }
4865 }
4866 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4867 {
4868 IkReal evalcond[12];
4869 IkReal x354=IKsin(j5);
4870 IkReal x355=IKcos(j5);
4871 IkReal x356=(cj3*new_r00);
4872 IkReal x357=(cj3*cj4);
4873 IkReal x358=(cj4*sj3);
4874 IkReal x359=((1.0)*cj3);
4875 IkReal x360=(sj3*x354);
4876 IkReal x361=((1.0)*x355);
4877 evalcond[0]=(((sj4*x354))+new_r21);
4878 evalcond[1]=((((-1.0)*sj4*x361))+new_r20);
4879 evalcond[2]=(((new_r00*sj3))+x354+(((-1.0)*new_r10*x359)));
4880 evalcond[3]=(((new_r01*sj3))+x355+(((-1.0)*new_r11*x359)));
4881 evalcond[4]=(((new_r11*sj3))+((cj4*x354))+((cj3*new_r01)));
4882 evalcond[5]=(((x354*x357))+((sj3*x355))+new_r01);
4883 evalcond[6]=((((-1.0)*cj4*x361))+((new_r10*sj3))+x356);
4884 evalcond[7]=(x360+new_r00+(((-1.0)*x357*x361)));
4885 evalcond[8]=(((x354*x358))+new_r11+(((-1.0)*x355*x359)));
4886 evalcond[9]=((((-1.0)*x358*x361))+new_r10+(((-1.0)*x354*x359)));
4887 evalcond[10]=(((new_r01*x357))+x354+((new_r21*sj4))+((new_r11*x358)));
4888 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x361))+((cj4*x356))+((new_r10*x358)));
4889 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4890 {
4891 continue;
4892 }
4893 }
4894 
4895 {
4896 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4897 vinfos[0].jointtype = 1;
4898 vinfos[0].foffset = j0;
4899 vinfos[0].indices[0] = _ij0[0];
4900 vinfos[0].indices[1] = _ij0[1];
4901 vinfos[0].maxsolutions = _nj0;
4902 vinfos[1].jointtype = 1;
4903 vinfos[1].foffset = j1;
4904 vinfos[1].indices[0] = _ij1[0];
4905 vinfos[1].indices[1] = _ij1[1];
4906 vinfos[1].maxsolutions = _nj1;
4907 vinfos[2].jointtype = 1;
4908 vinfos[2].foffset = j2;
4909 vinfos[2].indices[0] = _ij2[0];
4910 vinfos[2].indices[1] = _ij2[1];
4911 vinfos[2].maxsolutions = _nj2;
4912 vinfos[3].jointtype = 1;
4913 vinfos[3].foffset = j3;
4914 vinfos[3].indices[0] = _ij3[0];
4915 vinfos[3].indices[1] = _ij3[1];
4916 vinfos[3].maxsolutions = _nj3;
4917 vinfos[4].jointtype = 1;
4918 vinfos[4].foffset = j4;
4919 vinfos[4].indices[0] = _ij4[0];
4920 vinfos[4].indices[1] = _ij4[1];
4921 vinfos[4].maxsolutions = _nj4;
4922 vinfos[5].jointtype = 1;
4923 vinfos[5].foffset = j5;
4924 vinfos[5].indices[0] = _ij5[0];
4925 vinfos[5].indices[1] = _ij5[1];
4926 vinfos[5].maxsolutions = _nj5;
4927 std::vector<int> vfree(0);
4928 solutions.AddSolution(vinfos,vfree);
4929 }
4930 }
4931 }
4932 
4933 }
4934 
4935 }
4936 
4937 } else
4938 {
4939 {
4940 IkReal j5array[1], cj5array[1], sj5array[1];
4941 bool j5valid[1]={false};
4942 _nj5 = 1;
4943 CheckValue<IkReal> x362 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
4944 if(!x362.valid){
4945 continue;
4946 }
4947 CheckValue<IkReal> x363=IKPowWithIntegerCheck(IKsign(sj4),-1);
4948 if(!x363.valid){
4949 continue;
4950 }
4951 j5array[0]=((-1.5707963267949)+(x362.value)+(((1.5707963267949)*(x363.value))));
4952 sj5array[0]=IKsin(j5array[0]);
4953 cj5array[0]=IKcos(j5array[0]);
4954 if( j5array[0] > IKPI )
4955 {
4956  j5array[0]-=IK2PI;
4957 }
4958 else if( j5array[0] < -IKPI )
4959 { j5array[0]+=IK2PI;
4960 }
4961 j5valid[0] = true;
4962 for(int ij5 = 0; ij5 < 1; ++ij5)
4963 {
4964 if( !j5valid[ij5] )
4965 {
4966  continue;
4967 }
4968 _ij5[0] = ij5; _ij5[1] = -1;
4969 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4970 {
4971 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4972 {
4973  j5valid[iij5]=false; _ij5[1] = iij5; break;
4974 }
4975 }
4976 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4977 {
4978 IkReal evalcond[12];
4979 IkReal x364=IKsin(j5);
4980 IkReal x365=IKcos(j5);
4981 IkReal x366=(cj3*new_r00);
4982 IkReal x367=(cj3*cj4);
4983 IkReal x368=(cj4*sj3);
4984 IkReal x369=((1.0)*cj3);
4985 IkReal x370=(sj3*x364);
4986 IkReal x371=((1.0)*x365);
4987 evalcond[0]=(((sj4*x364))+new_r21);
4988 evalcond[1]=((((-1.0)*sj4*x371))+new_r20);
4989 evalcond[2]=(((new_r00*sj3))+x364+(((-1.0)*new_r10*x369)));
4990 evalcond[3]=(((new_r01*sj3))+x365+(((-1.0)*new_r11*x369)));
4991 evalcond[4]=(((new_r11*sj3))+((cj4*x364))+((cj3*new_r01)));
4992 evalcond[5]=(((x364*x367))+((sj3*x365))+new_r01);
4993 evalcond[6]=(((new_r10*sj3))+(((-1.0)*cj4*x371))+x366);
4994 evalcond[7]=((((-1.0)*x367*x371))+x370+new_r00);
4995 evalcond[8]=(((x364*x368))+new_r11+(((-1.0)*x365*x369)));
4996 evalcond[9]=((((-1.0)*x368*x371))+(((-1.0)*x364*x369))+new_r10);
4997 evalcond[10]=(x364+((new_r01*x367))+((new_r21*sj4))+((new_r11*x368)));
4998 evalcond[11]=(((new_r20*sj4))+(((-1.0)*x371))+((cj4*x366))+((new_r10*x368)));
4999 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
5000 {
5001 continue;
5002 }
5003 }
5004 
5005 {
5006 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5007 vinfos[0].jointtype = 1;
5008 vinfos[0].foffset = j0;
5009 vinfos[0].indices[0] = _ij0[0];
5010 vinfos[0].indices[1] = _ij0[1];
5011 vinfos[0].maxsolutions = _nj0;
5012 vinfos[1].jointtype = 1;
5013 vinfos[1].foffset = j1;
5014 vinfos[1].indices[0] = _ij1[0];
5015 vinfos[1].indices[1] = _ij1[1];
5016 vinfos[1].maxsolutions = _nj1;
5017 vinfos[2].jointtype = 1;
5018 vinfos[2].foffset = j2;
5019 vinfos[2].indices[0] = _ij2[0];
5020 vinfos[2].indices[1] = _ij2[1];
5021 vinfos[2].maxsolutions = _nj2;
5022 vinfos[3].jointtype = 1;
5023 vinfos[3].foffset = j3;
5024 vinfos[3].indices[0] = _ij3[0];
5025 vinfos[3].indices[1] = _ij3[1];
5026 vinfos[3].maxsolutions = _nj3;
5027 vinfos[4].jointtype = 1;
5028 vinfos[4].foffset = j4;
5029 vinfos[4].indices[0] = _ij4[0];
5030 vinfos[4].indices[1] = _ij4[1];
5031 vinfos[4].maxsolutions = _nj4;
5032 vinfos[5].jointtype = 1;
5033 vinfos[5].foffset = j5;
5034 vinfos[5].indices[0] = _ij5[0];
5035 vinfos[5].indices[1] = _ij5[1];
5036 vinfos[5].maxsolutions = _nj5;
5037 std::vector<int> vfree(0);
5038 solutions.AddSolution(vinfos,vfree);
5039 }
5040 }
5041 }
5042 
5043 }
5044 
5045 }
5046 }
5047 }
5048 
5049 }
5050 
5051 }
5052 
5053 } else
5054 {
5055 {
5056 IkReal j5array[1], cj5array[1], sj5array[1];
5057 bool j5valid[1]={false};
5058 _nj5 = 1;
5059 CheckValue<IkReal> x372 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5060 if(!x372.valid){
5061 continue;
5062 }
5063 CheckValue<IkReal> x373=IKPowWithIntegerCheck(IKsign(sj4),-1);
5064 if(!x373.valid){
5065 continue;
5066 }
5067 j5array[0]=((-1.5707963267949)+(x372.value)+(((1.5707963267949)*(x373.value))));
5068 sj5array[0]=IKsin(j5array[0]);
5069 cj5array[0]=IKcos(j5array[0]);
5070 if( j5array[0] > IKPI )
5071 {
5072  j5array[0]-=IK2PI;
5073 }
5074 else if( j5array[0] < -IKPI )
5075 { j5array[0]+=IK2PI;
5076 }
5077 j5valid[0] = true;
5078 for(int ij5 = 0; ij5 < 1; ++ij5)
5079 {
5080 if( !j5valid[ij5] )
5081 {
5082  continue;
5083 }
5084 _ij5[0] = ij5; _ij5[1] = -1;
5085 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5086 {
5087 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5088 {
5089  j5valid[iij5]=false; _ij5[1] = iij5; break;
5090 }
5091 }
5092 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5093 {
5094 IkReal evalcond[2];
5095 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5096 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5097 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5098 {
5099 continue;
5100 }
5101 }
5102 
5103 {
5104 IkReal j3eval[3];
5105 j3eval[0]=sj4;
5106 j3eval[1]=IKsign(sj4);
5107 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5108 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5109 {
5110 {
5111 IkReal j3eval[2];
5112 j3eval[0]=new_r12;
5113 j3eval[1]=sj4;
5114 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5115 {
5116 {
5117 IkReal evalcond[5];
5118 bool bgotonextstatement = true;
5119 do
5120 {
5121 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5122 evalcond[1]=new_r02;
5123 evalcond[2]=new_r12;
5124 evalcond[3]=new_r21;
5125 evalcond[4]=new_r20;
5126 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 )
5127 {
5128 bgotonextstatement=false;
5129 {
5130 IkReal j3eval[3];
5131 sj4=0;
5132 cj4=1.0;
5133 j4=0;
5134 IkReal x374=((1.0)*new_r11);
5135 IkReal x375=((((-1.0)*new_r10*x374))+(((-1.0)*new_r00*new_r01)));
5136 j3eval[0]=x375;
5137 j3eval[1]=((IKabs(((((-1.0)*sj5*x374))+((new_r00*sj5)))))+(IKabs((((new_r10*sj5))+((new_r01*sj5))))));
5138 j3eval[2]=IKsign(x375);
5139 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5140 {
5141 {
5142 IkReal j3eval[3];
5143 sj4=0;
5144 cj4=1.0;
5145 j4=0;
5146 IkReal x376=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5147 j3eval[0]=x376;
5148 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5149 j3eval[2]=IKsign(x376);
5150 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5151 {
5152 {
5153 IkReal j3eval[3];
5154 sj4=0;
5155 cj4=1.0;
5156 j4=0;
5157 IkReal x377=((1.0)*new_r11);
5158 IkReal x378=((((-1.0)*cj5*x377))+(((-1.0)*new_r01*sj5)));
5159 j3eval[0]=x378;
5160 j3eval[1]=IKsign(x378);
5161 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x377))+(((-1.0)*(cj5*cj5)))))));
5162 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5163 {
5164 {
5165 IkReal evalcond[1];
5166 bool bgotonextstatement = true;
5167 do
5168 {
5169 IkReal x379=((-1.0)*new_r01);
5170 IkReal x381 = ((new_r01*new_r01)+(new_r11*new_r11));
5171 if(IKabs(x381)==0){
5172 continue;
5173 }
5174 IkReal x380=pow(x381,-0.5);
5175 CheckValue<IkReal> x382 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x379),IKFAST_ATAN2_MAGTHRESH);
5176 if(!x382.valid){
5177 continue;
5178 }
5179 IkReal gconst6=((-1.0)*(x382.value));
5180 IkReal gconst7=(new_r11*x380);
5181 IkReal gconst8=(x379*x380);
5182 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5183 if(!x383.valid){
5184 continue;
5185 }
5186 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x383.value)+j5)))), 6.28318530717959)));
5187 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5188 {
5189 bgotonextstatement=false;
5190 {
5191 IkReal j3eval[3];
5192 IkReal x384=((-1.0)*new_r01);
5193 CheckValue<IkReal> x387 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x384),IKFAST_ATAN2_MAGTHRESH);
5194 if(!x387.valid){
5195 continue;
5196 }
5197 IkReal x385=((-1.0)*(x387.value));
5198 IkReal x386=x380;
5199 sj4=0;
5200 cj4=1.0;
5201 j4=0;
5202 sj5=gconst7;
5203 cj5=gconst8;
5204 j5=x385;
5205 IkReal gconst6=x385;
5206 IkReal gconst7=(new_r11*x386);
5207 IkReal gconst8=(x384*x386);
5208 IkReal x388=new_r11*new_r11;
5209 IkReal x389=(new_r10*new_r11);
5210 IkReal x390=((((-1.0)*x389))+(((-1.0)*new_r00*new_r01)));
5211 IkReal x391=x380;
5212 IkReal x392=(new_r11*x391);
5213 j3eval[0]=x390;
5214 j3eval[1]=((IKabs(((((-1.0)*x388*x391))+((new_r00*x392)))))+(IKabs((((new_r01*x392))+((x389*x391))))));
5215 j3eval[2]=IKsign(x390);
5216 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5217 {
5218 {
5219 IkReal j3eval[1];
5220 IkReal x393=((-1.0)*new_r01);
5221 CheckValue<IkReal> x396 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x393),IKFAST_ATAN2_MAGTHRESH);
5222 if(!x396.valid){
5223 continue;
5224 }
5225 IkReal x394=((-1.0)*(x396.value));
5226 IkReal x395=x380;
5227 sj4=0;
5228 cj4=1.0;
5229 j4=0;
5230 sj5=gconst7;
5231 cj5=gconst8;
5232 j5=x394;
5233 IkReal gconst6=x394;
5234 IkReal gconst7=(new_r11*x395);
5235 IkReal gconst8=(x393*x395);
5236 IkReal x397=new_r11*new_r11;
5237 CheckValue<IkReal> x400=IKPowWithIntegerCheck(((new_r01*new_r01)+x397),-1);
5238 if(!x400.valid){
5239 continue;
5240 }
5241 IkReal x398=x400.value;
5242 IkReal x399=(x397*x398);
5243 j3eval[0]=((IKabs((((new_r00*new_r01*x399))+((new_r00*x398*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x398)))))+(IKabs((((new_r01*new_r10))+x399))));
5244 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5245 {
5246 {
5247 IkReal j3eval[1];
5248 IkReal x401=((-1.0)*new_r01);
5249 CheckValue<IkReal> x404 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x401),IKFAST_ATAN2_MAGTHRESH);
5250 if(!x404.valid){
5251 continue;
5252 }
5253 IkReal x402=((-1.0)*(x404.value));
5254 IkReal x403=x380;
5255 sj4=0;
5256 cj4=1.0;
5257 j4=0;
5258 sj5=gconst7;
5259 cj5=gconst8;
5260 j5=x402;
5261 IkReal gconst6=x402;
5262 IkReal gconst7=(new_r11*x403);
5263 IkReal gconst8=(x401*x403);
5264 IkReal x405=new_r01*new_r01;
5265 IkReal x406=new_r11*new_r11;
5266 CheckValue<IkReal> x413=IKPowWithIntegerCheck((x405+x406),-1);
5267 if(!x413.valid){
5268 continue;
5269 }
5270 IkReal x407=x413.value;
5271 IkReal x408=(x406*x407);
5272 CheckValue<IkReal> x414=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x405))),-1);
5273 if(!x414.valid){
5274 continue;
5275 }
5276 IkReal x409=x414.value;
5277 IkReal x410=((1.0)*x409);
5278 IkReal x411=(new_r11*x410);
5279 IkReal x412=(new_r01*x410);
5280 j3eval[0]=((IKabs(((((-1.0)*new_r01*x411*(new_r11*new_r11)))+(((-1.0)*x411*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x411)))))+(IKabs((((x405*x408))+((x407*(x405*x405)))+(((-1.0)*x408))))));
5281 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5282 {
5283 {
5284 IkReal evalcond[2];
5285 bool bgotonextstatement = true;
5286 do
5287 {
5288 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5289 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5290 {
5291 bgotonextstatement=false;
5292 {
5293 IkReal j3array[2], cj3array[2], sj3array[2];
5294 bool j3valid[2]={false};
5295 _nj3 = 2;
5296 CheckValue<IkReal> x415=IKPowWithIntegerCheck(gconst8,-1);
5297 if(!x415.valid){
5298 continue;
5299 }
5300 sj3array[0]=(new_r10*(x415.value));
5301 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5302 {
5303  j3valid[0] = j3valid[1] = true;
5304  j3array[0] = IKasin(sj3array[0]);
5305  cj3array[0] = IKcos(j3array[0]);
5306  sj3array[1] = sj3array[0];
5307  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5308  cj3array[1] = -cj3array[0];
5309 }
5310 else if( isnan(sj3array[0]) )
5311 {
5312  // probably any value will work
5313  j3valid[0] = true;
5314  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5315 }
5316 for(int ij3 = 0; ij3 < 2; ++ij3)
5317 {
5318 if( !j3valid[ij3] )
5319 {
5320  continue;
5321 }
5322 _ij3[0] = ij3; _ij3[1] = -1;
5323 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5324 {
5325 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5326 {
5327  j3valid[iij3]=false; _ij3[1] = iij3; break;
5328 }
5329 }
5330 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5331 {
5332 IkReal evalcond[6];
5333 IkReal x416=IKcos(j3);
5334 IkReal x417=IKsin(j3);
5335 IkReal x418=((-1.0)*x416);
5336 evalcond[0]=(new_r01*x416);
5337 evalcond[1]=(new_r10*x418);
5338 evalcond[2]=(gconst8*x418);
5339 evalcond[3]=(gconst8+((new_r01*x417)));
5340 evalcond[4]=(((gconst8*x417))+new_r01);
5341 evalcond[5]=((((-1.0)*gconst8))+((new_r10*x417)));
5342 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5343 {
5344 continue;
5345 }
5346 }
5347 
5348 {
5349 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5350 vinfos[0].jointtype = 1;
5351 vinfos[0].foffset = j0;
5352 vinfos[0].indices[0] = _ij0[0];
5353 vinfos[0].indices[1] = _ij0[1];
5354 vinfos[0].maxsolutions = _nj0;
5355 vinfos[1].jointtype = 1;
5356 vinfos[1].foffset = j1;
5357 vinfos[1].indices[0] = _ij1[0];
5358 vinfos[1].indices[1] = _ij1[1];
5359 vinfos[1].maxsolutions = _nj1;
5360 vinfos[2].jointtype = 1;
5361 vinfos[2].foffset = j2;
5362 vinfos[2].indices[0] = _ij2[0];
5363 vinfos[2].indices[1] = _ij2[1];
5364 vinfos[2].maxsolutions = _nj2;
5365 vinfos[3].jointtype = 1;
5366 vinfos[3].foffset = j3;
5367 vinfos[3].indices[0] = _ij3[0];
5368 vinfos[3].indices[1] = _ij3[1];
5369 vinfos[3].maxsolutions = _nj3;
5370 vinfos[4].jointtype = 1;
5371 vinfos[4].foffset = j4;
5372 vinfos[4].indices[0] = _ij4[0];
5373 vinfos[4].indices[1] = _ij4[1];
5374 vinfos[4].maxsolutions = _nj4;
5375 vinfos[5].jointtype = 1;
5376 vinfos[5].foffset = j5;
5377 vinfos[5].indices[0] = _ij5[0];
5378 vinfos[5].indices[1] = _ij5[1];
5379 vinfos[5].maxsolutions = _nj5;
5380 std::vector<int> vfree(0);
5381 solutions.AddSolution(vinfos,vfree);
5382 }
5383 }
5384 }
5385 
5386 }
5387 } while(0);
5388 if( bgotonextstatement )
5389 {
5390 bool bgotonextstatement = true;
5391 do
5392 {
5393 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5394 evalcond[1]=gconst7;
5395 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
5396 {
5397 bgotonextstatement=false;
5398 {
5399 IkReal j3eval[3];
5400 IkReal x419=((-1.0)*new_r01);
5401 CheckValue<IkReal> x421 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x419),IKFAST_ATAN2_MAGTHRESH);
5402 if(!x421.valid){
5403 continue;
5404 }
5405 IkReal x420=((-1.0)*(x421.value));
5406 sj4=0;
5407 cj4=1.0;
5408 j4=0;
5409 sj5=gconst7;
5410 cj5=gconst8;
5411 j5=x420;
5412 new_r00=0;
5413 new_r10=0;
5414 new_r21=0;
5415 new_r22=0;
5416 IkReal gconst6=x420;
5417 IkReal gconst7=new_r11;
5418 IkReal gconst8=x419;
5419 j3eval[0]=-1.0;
5420 j3eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5421 j3eval[2]=-1.0;
5422 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5423 {
5424 {
5425 IkReal j3eval[3];
5426 IkReal x422=((-1.0)*new_r01);
5427 CheckValue<IkReal> x424 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x422),IKFAST_ATAN2_MAGTHRESH);
5428 if(!x424.valid){
5429 continue;
5430 }
5431 IkReal x423=((-1.0)*(x424.value));
5432 sj4=0;
5433 cj4=1.0;
5434 j4=0;
5435 sj5=gconst7;
5436 cj5=gconst8;
5437 j5=x423;
5438 new_r00=0;
5439 new_r10=0;
5440 new_r21=0;
5441 new_r22=0;
5442 IkReal gconst6=x423;
5443 IkReal gconst7=new_r11;
5444 IkReal gconst8=x422;
5445 j3eval[0]=-1.0;
5446 j3eval[1]=-1.0;
5447 j3eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5448 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5449 {
5450 {
5451 IkReal j3eval[3];
5452 IkReal x425=((-1.0)*new_r01);
5453 CheckValue<IkReal> x427 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x425),IKFAST_ATAN2_MAGTHRESH);
5454 if(!x427.valid){
5455 continue;
5456 }
5457 IkReal x426=((-1.0)*(x427.value));
5458 sj4=0;
5459 cj4=1.0;
5460 j4=0;
5461 sj5=gconst7;
5462 cj5=gconst8;
5463 j5=x426;
5464 new_r00=0;
5465 new_r10=0;
5466 new_r21=0;
5467 new_r22=0;
5468 IkReal gconst6=x426;
5469 IkReal gconst7=new_r11;
5470 IkReal gconst8=x425;
5471 j3eval[0]=1.0;
5472 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5473 j3eval[2]=1.0;
5474 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5475 {
5476 continue; // 3 cases reached
5477 
5478 } else
5479 {
5480 {
5481 IkReal j3array[1], cj3array[1], sj3array[1];
5482 bool j3valid[1]={false};
5483 _nj3 = 1;
5484 IkReal x428=((1.0)*new_r01);
5485 CheckValue<IkReal> x429 = IKatan2WithCheck(IkReal(((((-1.0)*(gconst7*gconst7)))+(new_r01*new_r01))),IkReal(((((-1.0)*new_r11*x428))+((gconst7*gconst8)))),IKFAST_ATAN2_MAGTHRESH);
5486 if(!x429.valid){
5487 continue;
5488 }
5489 CheckValue<IkReal> x430=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*x428))+((gconst7*new_r11)))),-1);
5490 if(!x430.valid){
5491 continue;
5492 }
5493 j3array[0]=((-1.5707963267949)+(x429.value)+(((1.5707963267949)*(x430.value))));
5494 sj3array[0]=IKsin(j3array[0]);
5495 cj3array[0]=IKcos(j3array[0]);
5496 if( j3array[0] > IKPI )
5497 {
5498  j3array[0]-=IK2PI;
5499 }
5500 else if( j3array[0] < -IKPI )
5501 { j3array[0]+=IK2PI;
5502 }
5503 j3valid[0] = true;
5504 for(int ij3 = 0; ij3 < 1; ++ij3)
5505 {
5506 if( !j3valid[ij3] )
5507 {
5508  continue;
5509 }
5510 _ij3[0] = ij3; _ij3[1] = -1;
5511 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5512 {
5513 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5514 {
5515  j3valid[iij3]=false; _ij3[1] = iij3; break;
5516 }
5517 }
5518 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5519 {
5520 IkReal evalcond[6];
5521 IkReal x431=IKsin(j3);
5522 IkReal x432=IKcos(j3);
5523 IkReal x433=(gconst7*x431);
5524 IkReal x434=((1.0)*x432);
5525 IkReal x435=(gconst8*x431);
5526 IkReal x436=(gconst8*x434);
5527 evalcond[0]=(((new_r01*x432))+gconst7+((new_r11*x431)));
5528 evalcond[1]=(((gconst7*x432))+x435+new_r01);
5529 evalcond[2]=((((-1.0)*x436))+x433);
5530 evalcond[3]=(((new_r01*x431))+gconst8+(((-1.0)*new_r11*x434)));
5531 evalcond[4]=((((-1.0)*x436))+x433+new_r11);
5532 evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst7*x434)));
5533 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5534 {
5535 continue;
5536 }
5537 }
5538 
5539 {
5540 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5541 vinfos[0].jointtype = 1;
5542 vinfos[0].foffset = j0;
5543 vinfos[0].indices[0] = _ij0[0];
5544 vinfos[0].indices[1] = _ij0[1];
5545 vinfos[0].maxsolutions = _nj0;
5546 vinfos[1].jointtype = 1;
5547 vinfos[1].foffset = j1;
5548 vinfos[1].indices[0] = _ij1[0];
5549 vinfos[1].indices[1] = _ij1[1];
5550 vinfos[1].maxsolutions = _nj1;
5551 vinfos[2].jointtype = 1;
5552 vinfos[2].foffset = j2;
5553 vinfos[2].indices[0] = _ij2[0];
5554 vinfos[2].indices[1] = _ij2[1];
5555 vinfos[2].maxsolutions = _nj2;
5556 vinfos[3].jointtype = 1;
5557 vinfos[3].foffset = j3;
5558 vinfos[3].indices[0] = _ij3[0];
5559 vinfos[3].indices[1] = _ij3[1];
5560 vinfos[3].maxsolutions = _nj3;
5561 vinfos[4].jointtype = 1;
5562 vinfos[4].foffset = j4;
5563 vinfos[4].indices[0] = _ij4[0];
5564 vinfos[4].indices[1] = _ij4[1];
5565 vinfos[4].maxsolutions = _nj4;
5566 vinfos[5].jointtype = 1;
5567 vinfos[5].foffset = j5;
5568 vinfos[5].indices[0] = _ij5[0];
5569 vinfos[5].indices[1] = _ij5[1];
5570 vinfos[5].maxsolutions = _nj5;
5571 std::vector<int> vfree(0);
5572 solutions.AddSolution(vinfos,vfree);
5573 }
5574 }
5575 }
5576 
5577 }
5578 
5579 }
5580 
5581 } else
5582 {
5583 {
5584 IkReal j3array[1], cj3array[1], sj3array[1];
5585 bool j3valid[1]={false};
5586 _nj3 = 1;
5587 CheckValue<IkReal> x437=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
5588 if(!x437.valid){
5589 continue;
5590 }
5591 CheckValue<IkReal> x438 = IKatan2WithCheck(IkReal((gconst8*new_r01)),IkReal((gconst7*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5592 if(!x438.valid){
5593 continue;
5594 }
5595 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x437.value)))+(x438.value));
5596 sj3array[0]=IKsin(j3array[0]);
5597 cj3array[0]=IKcos(j3array[0]);
5598 if( j3array[0] > IKPI )
5599 {
5600  j3array[0]-=IK2PI;
5601 }
5602 else if( j3array[0] < -IKPI )
5603 { j3array[0]+=IK2PI;
5604 }
5605 j3valid[0] = true;
5606 for(int ij3 = 0; ij3 < 1; ++ij3)
5607 {
5608 if( !j3valid[ij3] )
5609 {
5610  continue;
5611 }
5612 _ij3[0] = ij3; _ij3[1] = -1;
5613 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5614 {
5615 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5616 {
5617  j3valid[iij3]=false; _ij3[1] = iij3; break;
5618 }
5619 }
5620 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5621 {
5622 IkReal evalcond[6];
5623 IkReal x439=IKsin(j3);
5624 IkReal x440=IKcos(j3);
5625 IkReal x441=(gconst7*x439);
5626 IkReal x442=((1.0)*x440);
5627 IkReal x443=(gconst8*x439);
5628 IkReal x444=(gconst8*x442);
5629 evalcond[0]=(((new_r01*x440))+gconst7+((new_r11*x439)));
5630 evalcond[1]=(((gconst7*x440))+x443+new_r01);
5631 evalcond[2]=((((-1.0)*x444))+x441);
5632 evalcond[3]=(((new_r01*x439))+gconst8+(((-1.0)*new_r11*x442)));
5633 evalcond[4]=((((-1.0)*x444))+x441+new_r11);
5634 evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst7*x442)));
5635 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5636 {
5637 continue;
5638 }
5639 }
5640 
5641 {
5642 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5643 vinfos[0].jointtype = 1;
5644 vinfos[0].foffset = j0;
5645 vinfos[0].indices[0] = _ij0[0];
5646 vinfos[0].indices[1] = _ij0[1];
5647 vinfos[0].maxsolutions = _nj0;
5648 vinfos[1].jointtype = 1;
5649 vinfos[1].foffset = j1;
5650 vinfos[1].indices[0] = _ij1[0];
5651 vinfos[1].indices[1] = _ij1[1];
5652 vinfos[1].maxsolutions = _nj1;
5653 vinfos[2].jointtype = 1;
5654 vinfos[2].foffset = j2;
5655 vinfos[2].indices[0] = _ij2[0];
5656 vinfos[2].indices[1] = _ij2[1];
5657 vinfos[2].maxsolutions = _nj2;
5658 vinfos[3].jointtype = 1;
5659 vinfos[3].foffset = j3;
5660 vinfos[3].indices[0] = _ij3[0];
5661 vinfos[3].indices[1] = _ij3[1];
5662 vinfos[3].maxsolutions = _nj3;
5663 vinfos[4].jointtype = 1;
5664 vinfos[4].foffset = j4;
5665 vinfos[4].indices[0] = _ij4[0];
5666 vinfos[4].indices[1] = _ij4[1];
5667 vinfos[4].maxsolutions = _nj4;
5668 vinfos[5].jointtype = 1;
5669 vinfos[5].foffset = j5;
5670 vinfos[5].indices[0] = _ij5[0];
5671 vinfos[5].indices[1] = _ij5[1];
5672 vinfos[5].maxsolutions = _nj5;
5673 std::vector<int> vfree(0);
5674 solutions.AddSolution(vinfos,vfree);
5675 }
5676 }
5677 }
5678 
5679 }
5680 
5681 }
5682 
5683 } else
5684 {
5685 {
5686 IkReal j3array[1], cj3array[1], sj3array[1];
5687 bool j3valid[1]={false};
5688 _nj3 = 1;
5689 CheckValue<IkReal> x445=IKPowWithIntegerCheck(IKsign((((gconst8*new_r01))+(((-1.0)*gconst7*new_r11)))),-1);
5690 if(!x445.valid){
5691 continue;
5692 }
5693 CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(gconst7*gconst7),IkReal(((-1.0)*gconst7*gconst8)),IKFAST_ATAN2_MAGTHRESH);
5694 if(!x446.valid){
5695 continue;
5696 }
5697 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x445.value)))+(x446.value));
5698 sj3array[0]=IKsin(j3array[0]);
5699 cj3array[0]=IKcos(j3array[0]);
5700 if( j3array[0] > IKPI )
5701 {
5702  j3array[0]-=IK2PI;
5703 }
5704 else if( j3array[0] < -IKPI )
5705 { j3array[0]+=IK2PI;
5706 }
5707 j3valid[0] = true;
5708 for(int ij3 = 0; ij3 < 1; ++ij3)
5709 {
5710 if( !j3valid[ij3] )
5711 {
5712  continue;
5713 }
5714 _ij3[0] = ij3; _ij3[1] = -1;
5715 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5716 {
5717 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5718 {
5719  j3valid[iij3]=false; _ij3[1] = iij3; break;
5720 }
5721 }
5722 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5723 {
5724 IkReal evalcond[6];
5725 IkReal x447=IKsin(j3);
5726 IkReal x448=IKcos(j3);
5727 IkReal x449=(gconst7*x447);
5728 IkReal x450=((1.0)*x448);
5729 IkReal x451=(gconst8*x447);
5730 IkReal x452=(gconst8*x450);
5731 evalcond[0]=(((new_r01*x448))+gconst7+((new_r11*x447)));
5732 evalcond[1]=(((gconst7*x448))+x451+new_r01);
5733 evalcond[2]=((((-1.0)*x452))+x449);
5734 evalcond[3]=(((new_r01*x447))+gconst8+(((-1.0)*new_r11*x450)));
5735 evalcond[4]=((((-1.0)*x452))+x449+new_r11);
5736 evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst7*x450)));
5737 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5738 {
5739 continue;
5740 }
5741 }
5742 
5743 {
5744 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5745 vinfos[0].jointtype = 1;
5746 vinfos[0].foffset = j0;
5747 vinfos[0].indices[0] = _ij0[0];
5748 vinfos[0].indices[1] = _ij0[1];
5749 vinfos[0].maxsolutions = _nj0;
5750 vinfos[1].jointtype = 1;
5751 vinfos[1].foffset = j1;
5752 vinfos[1].indices[0] = _ij1[0];
5753 vinfos[1].indices[1] = _ij1[1];
5754 vinfos[1].maxsolutions = _nj1;
5755 vinfos[2].jointtype = 1;
5756 vinfos[2].foffset = j2;
5757 vinfos[2].indices[0] = _ij2[0];
5758 vinfos[2].indices[1] = _ij2[1];
5759 vinfos[2].maxsolutions = _nj2;
5760 vinfos[3].jointtype = 1;
5761 vinfos[3].foffset = j3;
5762 vinfos[3].indices[0] = _ij3[0];
5763 vinfos[3].indices[1] = _ij3[1];
5764 vinfos[3].maxsolutions = _nj3;
5765 vinfos[4].jointtype = 1;
5766 vinfos[4].foffset = j4;
5767 vinfos[4].indices[0] = _ij4[0];
5768 vinfos[4].indices[1] = _ij4[1];
5769 vinfos[4].maxsolutions = _nj4;
5770 vinfos[5].jointtype = 1;
5771 vinfos[5].foffset = j5;
5772 vinfos[5].indices[0] = _ij5[0];
5773 vinfos[5].indices[1] = _ij5[1];
5774 vinfos[5].maxsolutions = _nj5;
5775 std::vector<int> vfree(0);
5776 solutions.AddSolution(vinfos,vfree);
5777 }
5778 }
5779 }
5780 
5781 }
5782 
5783 }
5784 
5785 }
5786 } while(0);
5787 if( bgotonextstatement )
5788 {
5789 bool bgotonextstatement = true;
5790 do
5791 {
5792 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5793 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5794 {
5795 bgotonextstatement=false;
5796 {
5797 IkReal j3eval[1];
5798 CheckValue<IkReal> x454 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5799 if(!x454.valid){
5800 continue;
5801 }
5802 IkReal x453=((-1.0)*(x454.value));
5803 sj4=0;
5804 cj4=1.0;
5805 j4=0;
5806 sj5=gconst7;
5807 cj5=gconst8;
5808 j5=x453;
5809 new_r01=0;
5810 new_r10=0;
5811 IkReal gconst6=x453;
5812 IkReal x455 = new_r11*new_r11;
5813 if(IKabs(x455)==0){
5814 continue;
5815 }
5816 IkReal gconst7=(new_r11*(pow(x455,-0.5)));
5817 IkReal gconst8=0;
5818 j3eval[0]=new_r00;
5819 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5820 {
5821 {
5822 IkReal j3eval[1];
5823 CheckValue<IkReal> x457 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5824 if(!x457.valid){
5825 continue;
5826 }
5827 IkReal x456=((-1.0)*(x457.value));
5828 sj4=0;
5829 cj4=1.0;
5830 j4=0;
5831 sj5=gconst7;
5832 cj5=gconst8;
5833 j5=x456;
5834 new_r01=0;
5835 new_r10=0;
5836 IkReal gconst6=x456;
5837 IkReal x458 = new_r11*new_r11;
5838 if(IKabs(x458)==0){
5839 continue;
5840 }
5841 IkReal gconst7=(new_r11*(pow(x458,-0.5)));
5842 IkReal gconst8=0;
5843 j3eval[0]=new_r11;
5844 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5845 {
5846 {
5847 IkReal j3array[2], cj3array[2], sj3array[2];
5848 bool j3valid[2]={false};
5849 _nj3 = 2;
5850 CheckValue<IkReal> x459=IKPowWithIntegerCheck(gconst7,-1);
5851 if(!x459.valid){
5852 continue;
5853 }
5854 sj3array[0]=((-1.0)*new_r00*(x459.value));
5855 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5856 {
5857  j3valid[0] = j3valid[1] = true;
5858  j3array[0] = IKasin(sj3array[0]);
5859  cj3array[0] = IKcos(j3array[0]);
5860  sj3array[1] = sj3array[0];
5861  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5862  cj3array[1] = -cj3array[0];
5863 }
5864 else if( isnan(sj3array[0]) )
5865 {
5866  // probably any value will work
5867  j3valid[0] = true;
5868  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5869 }
5870 for(int ij3 = 0; ij3 < 2; ++ij3)
5871 {
5872 if( !j3valid[ij3] )
5873 {
5874  continue;
5875 }
5876 _ij3[0] = ij3; _ij3[1] = -1;
5877 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5878 {
5879 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5880 {
5881  j3valid[iij3]=false; _ij3[1] = iij3; break;
5882 }
5883 }
5884 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5885 {
5886 IkReal evalcond[6];
5887 IkReal x460=IKcos(j3);
5888 IkReal x461=IKsin(j3);
5889 evalcond[0]=(gconst7*x460);
5890 evalcond[1]=(new_r00*x460);
5891 evalcond[2]=((-1.0)*new_r11*x460);
5892 evalcond[3]=(((new_r00*x461))+gconst7);
5893 evalcond[4]=(((new_r11*x461))+gconst7);
5894 evalcond[5]=(((gconst7*x461))+new_r11);
5895 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5896 {
5897 continue;
5898 }
5899 }
5900 
5901 {
5902 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5903 vinfos[0].jointtype = 1;
5904 vinfos[0].foffset = j0;
5905 vinfos[0].indices[0] = _ij0[0];
5906 vinfos[0].indices[1] = _ij0[1];
5907 vinfos[0].maxsolutions = _nj0;
5908 vinfos[1].jointtype = 1;
5909 vinfos[1].foffset = j1;
5910 vinfos[1].indices[0] = _ij1[0];
5911 vinfos[1].indices[1] = _ij1[1];
5912 vinfos[1].maxsolutions = _nj1;
5913 vinfos[2].jointtype = 1;
5914 vinfos[2].foffset = j2;
5915 vinfos[2].indices[0] = _ij2[0];
5916 vinfos[2].indices[1] = _ij2[1];
5917 vinfos[2].maxsolutions = _nj2;
5918 vinfos[3].jointtype = 1;
5919 vinfos[3].foffset = j3;
5920 vinfos[3].indices[0] = _ij3[0];
5921 vinfos[3].indices[1] = _ij3[1];
5922 vinfos[3].maxsolutions = _nj3;
5923 vinfos[4].jointtype = 1;
5924 vinfos[4].foffset = j4;
5925 vinfos[4].indices[0] = _ij4[0];
5926 vinfos[4].indices[1] = _ij4[1];
5927 vinfos[4].maxsolutions = _nj4;
5928 vinfos[5].jointtype = 1;
5929 vinfos[5].foffset = j5;
5930 vinfos[5].indices[0] = _ij5[0];
5931 vinfos[5].indices[1] = _ij5[1];
5932 vinfos[5].maxsolutions = _nj5;
5933 std::vector<int> vfree(0);
5934 solutions.AddSolution(vinfos,vfree);
5935 }
5936 }
5937 }
5938 
5939 } else
5940 {
5941 {
5942 IkReal j3array[2], cj3array[2], sj3array[2];
5943 bool j3valid[2]={false};
5944 _nj3 = 2;
5945 CheckValue<IkReal> x462=IKPowWithIntegerCheck(new_r11,-1);
5946 if(!x462.valid){
5947 continue;
5948 }
5949 sj3array[0]=((-1.0)*gconst7*(x462.value));
5950 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5951 {
5952  j3valid[0] = j3valid[1] = true;
5953  j3array[0] = IKasin(sj3array[0]);
5954  cj3array[0] = IKcos(j3array[0]);
5955  sj3array[1] = sj3array[0];
5956  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5957  cj3array[1] = -cj3array[0];
5958 }
5959 else if( isnan(sj3array[0]) )
5960 {
5961  // probably any value will work
5962  j3valid[0] = true;
5963  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5964 }
5965 for(int ij3 = 0; ij3 < 2; ++ij3)
5966 {
5967 if( !j3valid[ij3] )
5968 {
5969  continue;
5970 }
5971 _ij3[0] = ij3; _ij3[1] = -1;
5972 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5973 {
5974 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5975 {
5976  j3valid[iij3]=false; _ij3[1] = iij3; break;
5977 }
5978 }
5979 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5980 {
5981 IkReal evalcond[6];
5982 IkReal x463=IKcos(j3);
5983 IkReal x464=IKsin(j3);
5984 IkReal x465=(gconst7*x464);
5985 evalcond[0]=(gconst7*x463);
5986 evalcond[1]=(new_r00*x463);
5987 evalcond[2]=((-1.0)*new_r11*x463);
5988 evalcond[3]=(((new_r00*x464))+gconst7);
5989 evalcond[4]=(x465+new_r00);
5990 evalcond[5]=(x465+new_r11);
5991 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5992 {
5993 continue;
5994 }
5995 }
5996 
5997 {
5998 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5999 vinfos[0].jointtype = 1;
6000 vinfos[0].foffset = j0;
6001 vinfos[0].indices[0] = _ij0[0];
6002 vinfos[0].indices[1] = _ij0[1];
6003 vinfos[0].maxsolutions = _nj0;
6004 vinfos[1].jointtype = 1;
6005 vinfos[1].foffset = j1;
6006 vinfos[1].indices[0] = _ij1[0];
6007 vinfos[1].indices[1] = _ij1[1];
6008 vinfos[1].maxsolutions = _nj1;
6009 vinfos[2].jointtype = 1;
6010 vinfos[2].foffset = j2;
6011 vinfos[2].indices[0] = _ij2[0];
6012 vinfos[2].indices[1] = _ij2[1];
6013 vinfos[2].maxsolutions = _nj2;
6014 vinfos[3].jointtype = 1;
6015 vinfos[3].foffset = j3;
6016 vinfos[3].indices[0] = _ij3[0];
6017 vinfos[3].indices[1] = _ij3[1];
6018 vinfos[3].maxsolutions = _nj3;
6019 vinfos[4].jointtype = 1;
6020 vinfos[4].foffset = j4;
6021 vinfos[4].indices[0] = _ij4[0];
6022 vinfos[4].indices[1] = _ij4[1];
6023 vinfos[4].maxsolutions = _nj4;
6024 vinfos[5].jointtype = 1;
6025 vinfos[5].foffset = j5;
6026 vinfos[5].indices[0] = _ij5[0];
6027 vinfos[5].indices[1] = _ij5[1];
6028 vinfos[5].maxsolutions = _nj5;
6029 std::vector<int> vfree(0);
6030 solutions.AddSolution(vinfos,vfree);
6031 }
6032 }
6033 }
6034 
6035 }
6036 
6037 }
6038 
6039 } else
6040 {
6041 {
6042 IkReal j3array[2], cj3array[2], sj3array[2];
6043 bool j3valid[2]={false};
6044 _nj3 = 2;
6045 CheckValue<IkReal> x466=IKPowWithIntegerCheck(new_r00,-1);
6046 if(!x466.valid){
6047 continue;
6048 }
6049 sj3array[0]=((-1.0)*gconst7*(x466.value));
6050 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6051 {
6052  j3valid[0] = j3valid[1] = true;
6053  j3array[0] = IKasin(sj3array[0]);
6054  cj3array[0] = IKcos(j3array[0]);
6055  sj3array[1] = sj3array[0];
6056  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6057  cj3array[1] = -cj3array[0];
6058 }
6059 else if( isnan(sj3array[0]) )
6060 {
6061  // probably any value will work
6062  j3valid[0] = true;
6063  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6064 }
6065 for(int ij3 = 0; ij3 < 2; ++ij3)
6066 {
6067 if( !j3valid[ij3] )
6068 {
6069  continue;
6070 }
6071 _ij3[0] = ij3; _ij3[1] = -1;
6072 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6073 {
6074 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6075 {
6076  j3valid[iij3]=false; _ij3[1] = iij3; break;
6077 }
6078 }
6079 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6080 {
6081 IkReal evalcond[6];
6082 IkReal x467=IKcos(j3);
6083 IkReal x468=IKsin(j3);
6084 IkReal x469=(gconst7*x468);
6085 evalcond[0]=(gconst7*x467);
6086 evalcond[1]=(new_r00*x467);
6087 evalcond[2]=((-1.0)*new_r11*x467);
6088 evalcond[3]=(((new_r11*x468))+gconst7);
6089 evalcond[4]=(x469+new_r00);
6090 evalcond[5]=(x469+new_r11);
6091 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6092 {
6093 continue;
6094 }
6095 }
6096 
6097 {
6098 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6099 vinfos[0].jointtype = 1;
6100 vinfos[0].foffset = j0;
6101 vinfos[0].indices[0] = _ij0[0];
6102 vinfos[0].indices[1] = _ij0[1];
6103 vinfos[0].maxsolutions = _nj0;
6104 vinfos[1].jointtype = 1;
6105 vinfos[1].foffset = j1;
6106 vinfos[1].indices[0] = _ij1[0];
6107 vinfos[1].indices[1] = _ij1[1];
6108 vinfos[1].maxsolutions = _nj1;
6109 vinfos[2].jointtype = 1;
6110 vinfos[2].foffset = j2;
6111 vinfos[2].indices[0] = _ij2[0];
6112 vinfos[2].indices[1] = _ij2[1];
6113 vinfos[2].maxsolutions = _nj2;
6114 vinfos[3].jointtype = 1;
6115 vinfos[3].foffset = j3;
6116 vinfos[3].indices[0] = _ij3[0];
6117 vinfos[3].indices[1] = _ij3[1];
6118 vinfos[3].maxsolutions = _nj3;
6119 vinfos[4].jointtype = 1;
6120 vinfos[4].foffset = j4;
6121 vinfos[4].indices[0] = _ij4[0];
6122 vinfos[4].indices[1] = _ij4[1];
6123 vinfos[4].maxsolutions = _nj4;
6124 vinfos[5].jointtype = 1;
6125 vinfos[5].foffset = j5;
6126 vinfos[5].indices[0] = _ij5[0];
6127 vinfos[5].indices[1] = _ij5[1];
6128 vinfos[5].maxsolutions = _nj5;
6129 std::vector<int> vfree(0);
6130 solutions.AddSolution(vinfos,vfree);
6131 }
6132 }
6133 }
6134 
6135 }
6136 
6137 }
6138 
6139 }
6140 } while(0);
6141 if( bgotonextstatement )
6142 {
6143 bool bgotonextstatement = true;
6144 do
6145 {
6146 evalcond[0]=IKabs(new_r11);
6147 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6148 {
6149 bgotonextstatement=false;
6150 {
6151 IkReal j3eval[1];
6152 IkReal x470=((-1.0)*new_r01);
6153 CheckValue<IkReal> x472 = IKatan2WithCheck(IkReal(0),IkReal(x470),IKFAST_ATAN2_MAGTHRESH);
6154 if(!x472.valid){
6155 continue;
6156 }
6157 IkReal x471=((-1.0)*(x472.value));
6158 sj4=0;
6159 cj4=1.0;
6160 j4=0;
6161 sj5=gconst7;
6162 cj5=gconst8;
6163 j5=x471;
6164 new_r11=0;
6165 IkReal gconst6=x471;
6166 IkReal gconst7=0;
6167 IkReal x473 = new_r01*new_r01;
6168 if(IKabs(x473)==0){
6169 continue;
6170 }
6171 IkReal gconst8=(x470*(pow(x473,-0.5)));
6172 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6173 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6174 {
6175 {
6176 IkReal j3eval[1];
6177 IkReal x474=((-1.0)*new_r01);
6178 CheckValue<IkReal> x476 = IKatan2WithCheck(IkReal(0),IkReal(x474),IKFAST_ATAN2_MAGTHRESH);
6179 if(!x476.valid){
6180 continue;
6181 }
6182 IkReal x475=((-1.0)*(x476.value));
6183 sj4=0;
6184 cj4=1.0;
6185 j4=0;
6186 sj5=gconst7;
6187 cj5=gconst8;
6188 j5=x475;
6189 new_r11=0;
6190 IkReal gconst6=x475;
6191 IkReal gconst7=0;
6192 IkReal x477 = new_r01*new_r01;
6193 if(IKabs(x477)==0){
6194 continue;
6195 }
6196 IkReal gconst8=(x474*(pow(x477,-0.5)));
6197 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6198 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6199 {
6200 {
6201 IkReal j3eval[1];
6202 IkReal x478=((-1.0)*new_r01);
6203 CheckValue<IkReal> x480 = IKatan2WithCheck(IkReal(0),IkReal(x478),IKFAST_ATAN2_MAGTHRESH);
6204 if(!x480.valid){
6205 continue;
6206 }
6207 IkReal x479=((-1.0)*(x480.value));
6208 sj4=0;
6209 cj4=1.0;
6210 j4=0;
6211 sj5=gconst7;
6212 cj5=gconst8;
6213 j5=x479;
6214 new_r11=0;
6215 IkReal gconst6=x479;
6216 IkReal gconst7=0;
6217 IkReal x481 = new_r01*new_r01;
6218 if(IKabs(x481)==0){
6219 continue;
6220 }
6221 IkReal gconst8=(x478*(pow(x481,-0.5)));
6222 j3eval[0]=new_r01;
6223 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6224 {
6225 continue; // 3 cases reached
6226 
6227 } else
6228 {
6229 {
6230 IkReal j3array[1], cj3array[1], sj3array[1];
6231 bool j3valid[1]={false};
6232 _nj3 = 1;
6233 CheckValue<IkReal> x482=IKPowWithIntegerCheck(new_r01,-1);
6234 if(!x482.valid){
6235 continue;
6236 }
6237 CheckValue<IkReal> x483=IKPowWithIntegerCheck(gconst8,-1);
6238 if(!x483.valid){
6239 continue;
6240 }
6241 if( IKabs(((-1.0)*gconst8*(x482.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst8*(x482.value)))+IKsqr((new_r00*(x483.value)))-1) <= IKFAST_SINCOS_THRESH )
6242  continue;
6243 j3array[0]=IKatan2(((-1.0)*gconst8*(x482.value)), (new_r00*(x483.value)));
6244 sj3array[0]=IKsin(j3array[0]);
6245 cj3array[0]=IKcos(j3array[0]);
6246 if( j3array[0] > IKPI )
6247 {
6248  j3array[0]-=IK2PI;
6249 }
6250 else if( j3array[0] < -IKPI )
6251 { j3array[0]+=IK2PI;
6252 }
6253 j3valid[0] = true;
6254 for(int ij3 = 0; ij3 < 1; ++ij3)
6255 {
6256 if( !j3valid[ij3] )
6257 {
6258  continue;
6259 }
6260 _ij3[0] = ij3; _ij3[1] = -1;
6261 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6262 {
6263 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6264 {
6265  j3valid[iij3]=false; _ij3[1] = iij3; break;
6266 }
6267 }
6268 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6269 {
6270 IkReal evalcond[8];
6271 IkReal x484=IKcos(j3);
6272 IkReal x485=IKsin(j3);
6273 IkReal x486=(gconst8*x485);
6274 IkReal x487=(gconst8*x484);
6275 evalcond[0]=(new_r01*x484);
6276 evalcond[1]=((-1.0)*x487);
6277 evalcond[2]=(gconst8+((new_r01*x485)));
6278 evalcond[3]=(x486+new_r01);
6279 evalcond[4]=((((-1.0)*x487))+new_r00);
6280 evalcond[5]=((((-1.0)*x486))+new_r10);
6281 evalcond[6]=((((-1.0)*new_r10*x484))+((new_r00*x485)));
6282 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x485))+((new_r00*x484)));
6283 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6284 {
6285 continue;
6286 }
6287 }
6288 
6289 {
6290 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6291 vinfos[0].jointtype = 1;
6292 vinfos[0].foffset = j0;
6293 vinfos[0].indices[0] = _ij0[0];
6294 vinfos[0].indices[1] = _ij0[1];
6295 vinfos[0].maxsolutions = _nj0;
6296 vinfos[1].jointtype = 1;
6297 vinfos[1].foffset = j1;
6298 vinfos[1].indices[0] = _ij1[0];
6299 vinfos[1].indices[1] = _ij1[1];
6300 vinfos[1].maxsolutions = _nj1;
6301 vinfos[2].jointtype = 1;
6302 vinfos[2].foffset = j2;
6303 vinfos[2].indices[0] = _ij2[0];
6304 vinfos[2].indices[1] = _ij2[1];
6305 vinfos[2].maxsolutions = _nj2;
6306 vinfos[3].jointtype = 1;
6307 vinfos[3].foffset = j3;
6308 vinfos[3].indices[0] = _ij3[0];
6309 vinfos[3].indices[1] = _ij3[1];
6310 vinfos[3].maxsolutions = _nj3;
6311 vinfos[4].jointtype = 1;
6312 vinfos[4].foffset = j4;
6313 vinfos[4].indices[0] = _ij4[0];
6314 vinfos[4].indices[1] = _ij4[1];
6315 vinfos[4].maxsolutions = _nj4;
6316 vinfos[5].jointtype = 1;
6317 vinfos[5].foffset = j5;
6318 vinfos[5].indices[0] = _ij5[0];
6319 vinfos[5].indices[1] = _ij5[1];
6320 vinfos[5].maxsolutions = _nj5;
6321 std::vector<int> vfree(0);
6322 solutions.AddSolution(vinfos,vfree);
6323 }
6324 }
6325 }
6326 
6327 }
6328 
6329 }
6330 
6331 } else
6332 {
6333 {
6334 IkReal j3array[1], cj3array[1], sj3array[1];
6335 bool j3valid[1]={false};
6336 _nj3 = 1;
6337 CheckValue<IkReal> x488 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6338 if(!x488.valid){
6339 continue;
6340 }
6341 CheckValue<IkReal> x489=IKPowWithIntegerCheck(IKsign(gconst8),-1);
6342 if(!x489.valid){
6343 continue;
6344 }
6345 j3array[0]=((-1.5707963267949)+(x488.value)+(((1.5707963267949)*(x489.value))));
6346 sj3array[0]=IKsin(j3array[0]);
6347 cj3array[0]=IKcos(j3array[0]);
6348 if( j3array[0] > IKPI )
6349 {
6350  j3array[0]-=IK2PI;
6351 }
6352 else if( j3array[0] < -IKPI )
6353 { j3array[0]+=IK2PI;
6354 }
6355 j3valid[0] = true;
6356 for(int ij3 = 0; ij3 < 1; ++ij3)
6357 {
6358 if( !j3valid[ij3] )
6359 {
6360  continue;
6361 }
6362 _ij3[0] = ij3; _ij3[1] = -1;
6363 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6364 {
6365 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6366 {
6367  j3valid[iij3]=false; _ij3[1] = iij3; break;
6368 }
6369 }
6370 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6371 {
6372 IkReal evalcond[8];
6373 IkReal x490=IKcos(j3);
6374 IkReal x491=IKsin(j3);
6375 IkReal x492=(gconst8*x491);
6376 IkReal x493=(gconst8*x490);
6377 evalcond[0]=(new_r01*x490);
6378 evalcond[1]=((-1.0)*x493);
6379 evalcond[2]=(gconst8+((new_r01*x491)));
6380 evalcond[3]=(x492+new_r01);
6381 evalcond[4]=((((-1.0)*x493))+new_r00);
6382 evalcond[5]=((((-1.0)*x492))+new_r10);
6383 evalcond[6]=((((-1.0)*new_r10*x490))+((new_r00*x491)));
6384 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x491))+((new_r00*x490)));
6385 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6386 {
6387 continue;
6388 }
6389 }
6390 
6391 {
6392 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6393 vinfos[0].jointtype = 1;
6394 vinfos[0].foffset = j0;
6395 vinfos[0].indices[0] = _ij0[0];
6396 vinfos[0].indices[1] = _ij0[1];
6397 vinfos[0].maxsolutions = _nj0;
6398 vinfos[1].jointtype = 1;
6399 vinfos[1].foffset = j1;
6400 vinfos[1].indices[0] = _ij1[0];
6401 vinfos[1].indices[1] = _ij1[1];
6402 vinfos[1].maxsolutions = _nj1;
6403 vinfos[2].jointtype = 1;
6404 vinfos[2].foffset = j2;
6405 vinfos[2].indices[0] = _ij2[0];
6406 vinfos[2].indices[1] = _ij2[1];
6407 vinfos[2].maxsolutions = _nj2;
6408 vinfos[3].jointtype = 1;
6409 vinfos[3].foffset = j3;
6410 vinfos[3].indices[0] = _ij3[0];
6411 vinfos[3].indices[1] = _ij3[1];
6412 vinfos[3].maxsolutions = _nj3;
6413 vinfos[4].jointtype = 1;
6414 vinfos[4].foffset = j4;
6415 vinfos[4].indices[0] = _ij4[0];
6416 vinfos[4].indices[1] = _ij4[1];
6417 vinfos[4].maxsolutions = _nj4;
6418 vinfos[5].jointtype = 1;
6419 vinfos[5].foffset = j5;
6420 vinfos[5].indices[0] = _ij5[0];
6421 vinfos[5].indices[1] = _ij5[1];
6422 vinfos[5].maxsolutions = _nj5;
6423 std::vector<int> vfree(0);
6424 solutions.AddSolution(vinfos,vfree);
6425 }
6426 }
6427 }
6428 
6429 }
6430 
6431 }
6432 
6433 } else
6434 {
6435 {
6436 IkReal j3array[1], cj3array[1], sj3array[1];
6437 bool j3valid[1]={false};
6438 _nj3 = 1;
6439 CheckValue<IkReal> x494=IKPowWithIntegerCheck(IKsign(gconst8),-1);
6440 if(!x494.valid){
6441 continue;
6442 }
6443 CheckValue<IkReal> x495 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6444 if(!x495.valid){
6445 continue;
6446 }
6447 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x494.value)))+(x495.value));
6448 sj3array[0]=IKsin(j3array[0]);
6449 cj3array[0]=IKcos(j3array[0]);
6450 if( j3array[0] > IKPI )
6451 {
6452  j3array[0]-=IK2PI;
6453 }
6454 else if( j3array[0] < -IKPI )
6455 { j3array[0]+=IK2PI;
6456 }
6457 j3valid[0] = true;
6458 for(int ij3 = 0; ij3 < 1; ++ij3)
6459 {
6460 if( !j3valid[ij3] )
6461 {
6462  continue;
6463 }
6464 _ij3[0] = ij3; _ij3[1] = -1;
6465 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6466 {
6467 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6468 {
6469  j3valid[iij3]=false; _ij3[1] = iij3; break;
6470 }
6471 }
6472 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6473 {
6474 IkReal evalcond[8];
6475 IkReal x496=IKcos(j3);
6476 IkReal x497=IKsin(j3);
6477 IkReal x498=(gconst8*x497);
6478 IkReal x499=(gconst8*x496);
6479 evalcond[0]=(new_r01*x496);
6480 evalcond[1]=((-1.0)*x499);
6481 evalcond[2]=(gconst8+((new_r01*x497)));
6482 evalcond[3]=(x498+new_r01);
6483 evalcond[4]=((((-1.0)*x499))+new_r00);
6484 evalcond[5]=((((-1.0)*x498))+new_r10);
6485 evalcond[6]=((((-1.0)*new_r10*x496))+((new_r00*x497)));
6486 evalcond[7]=((((-1.0)*gconst8))+((new_r10*x497))+((new_r00*x496)));
6487 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6488 {
6489 continue;
6490 }
6491 }
6492 
6493 {
6494 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6495 vinfos[0].jointtype = 1;
6496 vinfos[0].foffset = j0;
6497 vinfos[0].indices[0] = _ij0[0];
6498 vinfos[0].indices[1] = _ij0[1];
6499 vinfos[0].maxsolutions = _nj0;
6500 vinfos[1].jointtype = 1;
6501 vinfos[1].foffset = j1;
6502 vinfos[1].indices[0] = _ij1[0];
6503 vinfos[1].indices[1] = _ij1[1];
6504 vinfos[1].maxsolutions = _nj1;
6505 vinfos[2].jointtype = 1;
6506 vinfos[2].foffset = j2;
6507 vinfos[2].indices[0] = _ij2[0];
6508 vinfos[2].indices[1] = _ij2[1];
6509 vinfos[2].maxsolutions = _nj2;
6510 vinfos[3].jointtype = 1;
6511 vinfos[3].foffset = j3;
6512 vinfos[3].indices[0] = _ij3[0];
6513 vinfos[3].indices[1] = _ij3[1];
6514 vinfos[3].maxsolutions = _nj3;
6515 vinfos[4].jointtype = 1;
6516 vinfos[4].foffset = j4;
6517 vinfos[4].indices[0] = _ij4[0];
6518 vinfos[4].indices[1] = _ij4[1];
6519 vinfos[4].maxsolutions = _nj4;
6520 vinfos[5].jointtype = 1;
6521 vinfos[5].foffset = j5;
6522 vinfos[5].indices[0] = _ij5[0];
6523 vinfos[5].indices[1] = _ij5[1];
6524 vinfos[5].maxsolutions = _nj5;
6525 std::vector<int> vfree(0);
6526 solutions.AddSolution(vinfos,vfree);
6527 }
6528 }
6529 }
6530 
6531 }
6532 
6533 }
6534 
6535 }
6536 } while(0);
6537 if( bgotonextstatement )
6538 {
6539 bool bgotonextstatement = true;
6540 do
6541 {
6542 if( 1 )
6543 {
6544 bgotonextstatement=false;
6545 continue; // branch miss [j3]
6546 
6547 }
6548 } while(0);
6549 if( bgotonextstatement )
6550 {
6551 }
6552 }
6553 }
6554 }
6555 }
6556 }
6557 
6558 } else
6559 {
6560 {
6561 IkReal j3array[1], cj3array[1], sj3array[1];
6562 bool j3valid[1]={false};
6563 _nj3 = 1;
6564 IkReal x500=((1.0)*new_r01);
6565 CheckValue<IkReal> x501=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*x500))+((gconst7*new_r11)))),-1);
6566 if(!x501.valid){
6567 continue;
6568 }
6569 CheckValue<IkReal> x502 = IKatan2WithCheck(IkReal(((((-1.0)*(gconst7*gconst7)))+(new_r01*new_r01))),IkReal(((((-1.0)*new_r11*x500))+((gconst7*gconst8)))),IKFAST_ATAN2_MAGTHRESH);
6570 if(!x502.valid){
6571 continue;
6572 }
6573 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x501.value)))+(x502.value));
6574 sj3array[0]=IKsin(j3array[0]);
6575 cj3array[0]=IKcos(j3array[0]);
6576 if( j3array[0] > IKPI )
6577 {
6578  j3array[0]-=IK2PI;
6579 }
6580 else if( j3array[0] < -IKPI )
6581 { j3array[0]+=IK2PI;
6582 }
6583 j3valid[0] = true;
6584 for(int ij3 = 0; ij3 < 1; ++ij3)
6585 {
6586 if( !j3valid[ij3] )
6587 {
6588  continue;
6589 }
6590 _ij3[0] = ij3; _ij3[1] = -1;
6591 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6592 {
6593 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6594 {
6595  j3valid[iij3]=false; _ij3[1] = iij3; break;
6596 }
6597 }
6598 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6599 {
6600 IkReal evalcond[8];
6601 IkReal x503=IKsin(j3);
6602 IkReal x504=IKcos(j3);
6603 IkReal x505=(gconst7*x503);
6604 IkReal x506=((1.0)*x504);
6605 IkReal x507=(gconst8*x503);
6606 IkReal x508=(gconst8*x506);
6607 evalcond[0]=(gconst7+((new_r11*x503))+((new_r01*x504)));
6608 evalcond[1]=(((gconst7*x504))+x507+new_r01);
6609 evalcond[2]=(gconst7+((new_r00*x503))+(((-1.0)*new_r10*x506)));
6610 evalcond[3]=(gconst8+(((-1.0)*new_r11*x506))+((new_r01*x503)));
6611 evalcond[4]=((((-1.0)*x508))+x505+new_r00);
6612 evalcond[5]=((((-1.0)*x508))+x505+new_r11);
6613 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x503))+((new_r00*x504)));
6614 evalcond[7]=((((-1.0)*gconst7*x506))+new_r10+(((-1.0)*x507)));
6615 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6616 {
6617 continue;
6618 }
6619 }
6620 
6621 {
6622 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6623 vinfos[0].jointtype = 1;
6624 vinfos[0].foffset = j0;
6625 vinfos[0].indices[0] = _ij0[0];
6626 vinfos[0].indices[1] = _ij0[1];
6627 vinfos[0].maxsolutions = _nj0;
6628 vinfos[1].jointtype = 1;
6629 vinfos[1].foffset = j1;
6630 vinfos[1].indices[0] = _ij1[0];
6631 vinfos[1].indices[1] = _ij1[1];
6632 vinfos[1].maxsolutions = _nj1;
6633 vinfos[2].jointtype = 1;
6634 vinfos[2].foffset = j2;
6635 vinfos[2].indices[0] = _ij2[0];
6636 vinfos[2].indices[1] = _ij2[1];
6637 vinfos[2].maxsolutions = _nj2;
6638 vinfos[3].jointtype = 1;
6639 vinfos[3].foffset = j3;
6640 vinfos[3].indices[0] = _ij3[0];
6641 vinfos[3].indices[1] = _ij3[1];
6642 vinfos[3].maxsolutions = _nj3;
6643 vinfos[4].jointtype = 1;
6644 vinfos[4].foffset = j4;
6645 vinfos[4].indices[0] = _ij4[0];
6646 vinfos[4].indices[1] = _ij4[1];
6647 vinfos[4].maxsolutions = _nj4;
6648 vinfos[5].jointtype = 1;
6649 vinfos[5].foffset = j5;
6650 vinfos[5].indices[0] = _ij5[0];
6651 vinfos[5].indices[1] = _ij5[1];
6652 vinfos[5].maxsolutions = _nj5;
6653 std::vector<int> vfree(0);
6654 solutions.AddSolution(vinfos,vfree);
6655 }
6656 }
6657 }
6658 
6659 }
6660 
6661 }
6662 
6663 } else
6664 {
6665 {
6666 IkReal j3array[1], cj3array[1], sj3array[1];
6667 bool j3valid[1]={false};
6668 _nj3 = 1;
6669 IkReal x509=((1.0)*gconst7);
6670 CheckValue<IkReal> x510 = IKatan2WithCheck(IkReal(((gconst7*gconst7)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst8*x509)))),IKFAST_ATAN2_MAGTHRESH);
6671 if(!x510.valid){
6672 continue;
6673 }
6674 CheckValue<IkReal> x511=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*x509))+(((-1.0)*gconst8*new_r10)))),-1);
6675 if(!x511.valid){
6676 continue;
6677 }
6678 j3array[0]=((-1.5707963267949)+(x510.value)+(((1.5707963267949)*(x511.value))));
6679 sj3array[0]=IKsin(j3array[0]);
6680 cj3array[0]=IKcos(j3array[0]);
6681 if( j3array[0] > IKPI )
6682 {
6683  j3array[0]-=IK2PI;
6684 }
6685 else if( j3array[0] < -IKPI )
6686 { j3array[0]+=IK2PI;
6687 }
6688 j3valid[0] = true;
6689 for(int ij3 = 0; ij3 < 1; ++ij3)
6690 {
6691 if( !j3valid[ij3] )
6692 {
6693  continue;
6694 }
6695 _ij3[0] = ij3; _ij3[1] = -1;
6696 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6697 {
6698 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6699 {
6700  j3valid[iij3]=false; _ij3[1] = iij3; break;
6701 }
6702 }
6703 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6704 {
6705 IkReal evalcond[8];
6706 IkReal x512=IKsin(j3);
6707 IkReal x513=IKcos(j3);
6708 IkReal x514=(gconst7*x512);
6709 IkReal x515=((1.0)*x513);
6710 IkReal x516=(gconst8*x512);
6711 IkReal x517=(gconst8*x515);
6712 evalcond[0]=(((new_r01*x513))+((new_r11*x512))+gconst7);
6713 evalcond[1]=(((gconst7*x513))+x516+new_r01);
6714 evalcond[2]=(((new_r00*x512))+(((-1.0)*new_r10*x515))+gconst7);
6715 evalcond[3]=(((new_r01*x512))+(((-1.0)*new_r11*x515))+gconst8);
6716 evalcond[4]=(x514+new_r00+(((-1.0)*x517)));
6717 evalcond[5]=(x514+new_r11+(((-1.0)*x517)));
6718 evalcond[6]=(((new_r00*x513))+((new_r10*x512))+(((-1.0)*gconst8)));
6719 evalcond[7]=((((-1.0)*x516))+new_r10+(((-1.0)*gconst7*x515)));
6720 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6721 {
6722 continue;
6723 }
6724 }
6725 
6726 {
6727 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6728 vinfos[0].jointtype = 1;
6729 vinfos[0].foffset = j0;
6730 vinfos[0].indices[0] = _ij0[0];
6731 vinfos[0].indices[1] = _ij0[1];
6732 vinfos[0].maxsolutions = _nj0;
6733 vinfos[1].jointtype = 1;
6734 vinfos[1].foffset = j1;
6735 vinfos[1].indices[0] = _ij1[0];
6736 vinfos[1].indices[1] = _ij1[1];
6737 vinfos[1].maxsolutions = _nj1;
6738 vinfos[2].jointtype = 1;
6739 vinfos[2].foffset = j2;
6740 vinfos[2].indices[0] = _ij2[0];
6741 vinfos[2].indices[1] = _ij2[1];
6742 vinfos[2].maxsolutions = _nj2;
6743 vinfos[3].jointtype = 1;
6744 vinfos[3].foffset = j3;
6745 vinfos[3].indices[0] = _ij3[0];
6746 vinfos[3].indices[1] = _ij3[1];
6747 vinfos[3].maxsolutions = _nj3;
6748 vinfos[4].jointtype = 1;
6749 vinfos[4].foffset = j4;
6750 vinfos[4].indices[0] = _ij4[0];
6751 vinfos[4].indices[1] = _ij4[1];
6752 vinfos[4].maxsolutions = _nj4;
6753 vinfos[5].jointtype = 1;
6754 vinfos[5].foffset = j5;
6755 vinfos[5].indices[0] = _ij5[0];
6756 vinfos[5].indices[1] = _ij5[1];
6757 vinfos[5].maxsolutions = _nj5;
6758 std::vector<int> vfree(0);
6759 solutions.AddSolution(vinfos,vfree);
6760 }
6761 }
6762 }
6763 
6764 }
6765 
6766 }
6767 
6768 } else
6769 {
6770 {
6771 IkReal j3array[1], cj3array[1], sj3array[1];
6772 bool j3valid[1]={false};
6773 _nj3 = 1;
6774 IkReal x518=((1.0)*new_r11);
6775 CheckValue<IkReal> x519=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x518))+(((-1.0)*new_r00*new_r01)))),-1);
6776 if(!x519.valid){
6777 continue;
6778 }
6779 CheckValue<IkReal> x520 = IKatan2WithCheck(IkReal((((gconst7*new_r01))+((gconst7*new_r10)))),IkReal(((((-1.0)*gconst7*x518))+((gconst7*new_r00)))),IKFAST_ATAN2_MAGTHRESH);
6780 if(!x520.valid){
6781 continue;
6782 }
6783 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x519.value)))+(x520.value));
6784 sj3array[0]=IKsin(j3array[0]);
6785 cj3array[0]=IKcos(j3array[0]);
6786 if( j3array[0] > IKPI )
6787 {
6788  j3array[0]-=IK2PI;
6789 }
6790 else if( j3array[0] < -IKPI )
6791 { j3array[0]+=IK2PI;
6792 }
6793 j3valid[0] = true;
6794 for(int ij3 = 0; ij3 < 1; ++ij3)
6795 {
6796 if( !j3valid[ij3] )
6797 {
6798  continue;
6799 }
6800 _ij3[0] = ij3; _ij3[1] = -1;
6801 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6802 {
6803 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6804 {
6805  j3valid[iij3]=false; _ij3[1] = iij3; break;
6806 }
6807 }
6808 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6809 {
6810 IkReal evalcond[8];
6811 IkReal x521=IKsin(j3);
6812 IkReal x522=IKcos(j3);
6813 IkReal x523=(gconst7*x521);
6814 IkReal x524=((1.0)*x522);
6815 IkReal x525=(gconst8*x521);
6816 IkReal x526=(gconst8*x524);
6817 evalcond[0]=(((new_r01*x522))+gconst7+((new_r11*x521)));
6818 evalcond[1]=(((gconst7*x522))+x525+new_r01);
6819 evalcond[2]=(gconst7+(((-1.0)*new_r10*x524))+((new_r00*x521)));
6820 evalcond[3]=(((new_r01*x521))+gconst8+(((-1.0)*new_r11*x524)));
6821 evalcond[4]=((((-1.0)*x526))+x523+new_r00);
6822 evalcond[5]=((((-1.0)*x526))+x523+new_r11);
6823 evalcond[6]=((((-1.0)*gconst8))+((new_r10*x521))+((new_r00*x522)));
6824 evalcond[7]=((((-1.0)*x525))+(((-1.0)*gconst7*x524))+new_r10);
6825 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6826 {
6827 continue;
6828 }
6829 }
6830 
6831 {
6832 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6833 vinfos[0].jointtype = 1;
6834 vinfos[0].foffset = j0;
6835 vinfos[0].indices[0] = _ij0[0];
6836 vinfos[0].indices[1] = _ij0[1];
6837 vinfos[0].maxsolutions = _nj0;
6838 vinfos[1].jointtype = 1;
6839 vinfos[1].foffset = j1;
6840 vinfos[1].indices[0] = _ij1[0];
6841 vinfos[1].indices[1] = _ij1[1];
6842 vinfos[1].maxsolutions = _nj1;
6843 vinfos[2].jointtype = 1;
6844 vinfos[2].foffset = j2;
6845 vinfos[2].indices[0] = _ij2[0];
6846 vinfos[2].indices[1] = _ij2[1];
6847 vinfos[2].maxsolutions = _nj2;
6848 vinfos[3].jointtype = 1;
6849 vinfos[3].foffset = j3;
6850 vinfos[3].indices[0] = _ij3[0];
6851 vinfos[3].indices[1] = _ij3[1];
6852 vinfos[3].maxsolutions = _nj3;
6853 vinfos[4].jointtype = 1;
6854 vinfos[4].foffset = j4;
6855 vinfos[4].indices[0] = _ij4[0];
6856 vinfos[4].indices[1] = _ij4[1];
6857 vinfos[4].maxsolutions = _nj4;
6858 vinfos[5].jointtype = 1;
6859 vinfos[5].foffset = j5;
6860 vinfos[5].indices[0] = _ij5[0];
6861 vinfos[5].indices[1] = _ij5[1];
6862 vinfos[5].maxsolutions = _nj5;
6863 std::vector<int> vfree(0);
6864 solutions.AddSolution(vinfos,vfree);
6865 }
6866 }
6867 }
6868 
6869 }
6870 
6871 }
6872 
6873 }
6874 } while(0);
6875 if( bgotonextstatement )
6876 {
6877 bool bgotonextstatement = true;
6878 do
6879 {
6880 IkReal x527=((-1.0)*new_r11);
6881 IkReal x529 = ((new_r01*new_r01)+(new_r11*new_r11));
6882 if(IKabs(x529)==0){
6883 continue;
6884 }
6885 IkReal x528=pow(x529,-0.5);
6886 CheckValue<IkReal> x530 = IKatan2WithCheck(IkReal(x527),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6887 if(!x530.valid){
6888 continue;
6889 }
6890 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x530.value))));
6891 IkReal gconst10=(x527*x528);
6892 IkReal gconst11=((1.0)*new_r01*x528);
6893 CheckValue<IkReal> x531 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6894 if(!x531.valid){
6895 continue;
6896 }
6897 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x531.value)+j5)))), 6.28318530717959)));
6898 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6899 {
6900 bgotonextstatement=false;
6901 {
6902 IkReal j3eval[3];
6903 IkReal x532=((-1.0)*new_r11);
6904 CheckValue<IkReal> x535 = IKatan2WithCheck(IkReal(x532),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6905 if(!x535.valid){
6906 continue;
6907 }
6908 IkReal x533=((1.0)*(x535.value));
6909 IkReal x534=x528;
6910 sj4=0;
6911 cj4=1.0;
6912 j4=0;
6913 sj5=gconst10;
6914 cj5=gconst11;
6915 j5=((3.14159265)+(((-1.0)*x533)));
6916 IkReal gconst9=((3.14159265358979)+(((-1.0)*x533)));
6917 IkReal gconst10=(x532*x534);
6918 IkReal gconst11=((1.0)*new_r01*x534);
6919 IkReal x536=new_r11*new_r11;
6920 IkReal x537=((1.0)*new_r01);
6921 IkReal x538=((1.0)*new_r10);
6922 IkReal x539=((((-1.0)*new_r00*x537))+(((-1.0)*new_r11*x538)));
6923 IkReal x540=x528;
6924 IkReal x541=(new_r11*x540);
6925 j3eval[0]=x539;
6926 j3eval[1]=((IKabs(((((-1.0)*x537*x541))+(((-1.0)*x538*x541)))))+(IKabs((((x536*x540))+(((-1.0)*new_r00*x541))))));
6927 j3eval[2]=IKsign(x539);
6928 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
6929 {
6930 {
6931 IkReal j3eval[1];
6932 IkReal x542=((-1.0)*new_r11);
6933 CheckValue<IkReal> x545 = IKatan2WithCheck(IkReal(x542),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6934 if(!x545.valid){
6935 continue;
6936 }
6937 IkReal x543=((1.0)*(x545.value));
6938 IkReal x544=x528;
6939 sj4=0;
6940 cj4=1.0;
6941 j4=0;
6942 sj5=gconst10;
6943 cj5=gconst11;
6944 j5=((3.14159265)+(((-1.0)*x543)));
6945 IkReal gconst9=((3.14159265358979)+(((-1.0)*x543)));
6946 IkReal gconst10=(x542*x544);
6947 IkReal gconst11=((1.0)*new_r01*x544);
6948 IkReal x546=new_r11*new_r11;
6949 IkReal x547=new_r01*new_r01*new_r01;
6950 CheckValue<IkReal> x551=IKPowWithIntegerCheck(((new_r01*new_r01)+x546),-1);
6951 if(!x551.valid){
6952 continue;
6953 }
6954 IkReal x548=x551.value;
6955 IkReal x549=(x546*x548);
6956 IkReal x550=(x547*x548);
6957 j3eval[0]=((IKabs((((new_r01*new_r11*x548))+((new_r00*x550))+((new_r00*new_r01*x549)))))+(IKabs((((new_r10*x550))+((new_r01*new_r10*x549))+x549))));
6958 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6959 {
6960 {
6961 IkReal j3eval[1];
6962 IkReal x552=((-1.0)*new_r11);
6963 CheckValue<IkReal> x555 = IKatan2WithCheck(IkReal(x552),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6964 if(!x555.valid){
6965 continue;
6966 }
6967 IkReal x553=((1.0)*(x555.value));
6968 IkReal x554=x528;
6969 sj4=0;
6970 cj4=1.0;
6971 j4=0;
6972 sj5=gconst10;
6973 cj5=gconst11;
6974 j5=((3.14159265)+(((-1.0)*x553)));
6975 IkReal gconst9=((3.14159265358979)+(((-1.0)*x553)));
6976 IkReal gconst10=(x552*x554);
6977 IkReal gconst11=((1.0)*new_r01*x554);
6978 IkReal x556=new_r01*new_r01;
6979 IkReal x557=new_r11*new_r11;
6980 CheckValue<IkReal> x564=IKPowWithIntegerCheck((x557+x556),-1);
6981 if(!x564.valid){
6982 continue;
6983 }
6984 IkReal x558=x564.value;
6985 IkReal x559=(x557*x558);
6986 CheckValue<IkReal> x565=IKPowWithIntegerCheck(((((-1.0)*x556))+(((-1.0)*x557))),-1);
6987 if(!x565.valid){
6988 continue;
6989 }
6990 IkReal x560=x565.value;
6991 IkReal x561=((1.0)*x560);
6992 IkReal x562=(new_r11*x561);
6993 IkReal x563=(new_r01*x561);
6994 j3eval[0]=((IKabs(((((-1.0)*new_r01*x562*(new_r11*new_r11)))+(((-1.0)*x562*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x562)))))+(IKabs(((((-1.0)*x559))+((x558*(x556*x556)))+((x556*x559))))));
6995 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6996 {
6997 {
6998 IkReal evalcond[2];
6999 bool bgotonextstatement = true;
7000 do
7001 {
7002 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7003 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7004 {
7005 bgotonextstatement=false;
7006 {
7007 IkReal j3array[2], cj3array[2], sj3array[2];
7008 bool j3valid[2]={false};
7009 _nj3 = 2;
7010 CheckValue<IkReal> x566=IKPowWithIntegerCheck(gconst11,-1);
7011 if(!x566.valid){
7012 continue;
7013 }
7014 sj3array[0]=(new_r10*(x566.value));
7015 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7016 {
7017  j3valid[0] = j3valid[1] = true;
7018  j3array[0] = IKasin(sj3array[0]);
7019  cj3array[0] = IKcos(j3array[0]);
7020  sj3array[1] = sj3array[0];
7021  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7022  cj3array[1] = -cj3array[0];
7023 }
7024 else if( isnan(sj3array[0]) )
7025 {
7026  // probably any value will work
7027  j3valid[0] = true;
7028  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7029 }
7030 for(int ij3 = 0; ij3 < 2; ++ij3)
7031 {
7032 if( !j3valid[ij3] )
7033 {
7034  continue;
7035 }
7036 _ij3[0] = ij3; _ij3[1] = -1;
7037 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7038 {
7039 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7040 {
7041  j3valid[iij3]=false; _ij3[1] = iij3; break;
7042 }
7043 }
7044 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7045 {
7046 IkReal evalcond[6];
7047 IkReal x567=IKcos(j3);
7048 IkReal x568=IKsin(j3);
7049 IkReal x569=((-1.0)*x567);
7050 evalcond[0]=(new_r01*x567);
7051 evalcond[1]=(new_r10*x569);
7052 evalcond[2]=(gconst11*x569);
7053 evalcond[3]=(gconst11+((new_r01*x568)));
7054 evalcond[4]=(((gconst11*x568))+new_r01);
7055 evalcond[5]=((((-1.0)*gconst11))+((new_r10*x568)));
7056 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7057 {
7058 continue;
7059 }
7060 }
7061 
7062 {
7063 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7064 vinfos[0].jointtype = 1;
7065 vinfos[0].foffset = j0;
7066 vinfos[0].indices[0] = _ij0[0];
7067 vinfos[0].indices[1] = _ij0[1];
7068 vinfos[0].maxsolutions = _nj0;
7069 vinfos[1].jointtype = 1;
7070 vinfos[1].foffset = j1;
7071 vinfos[1].indices[0] = _ij1[0];
7072 vinfos[1].indices[1] = _ij1[1];
7073 vinfos[1].maxsolutions = _nj1;
7074 vinfos[2].jointtype = 1;
7075 vinfos[2].foffset = j2;
7076 vinfos[2].indices[0] = _ij2[0];
7077 vinfos[2].indices[1] = _ij2[1];
7078 vinfos[2].maxsolutions = _nj2;
7079 vinfos[3].jointtype = 1;
7080 vinfos[3].foffset = j3;
7081 vinfos[3].indices[0] = _ij3[0];
7082 vinfos[3].indices[1] = _ij3[1];
7083 vinfos[3].maxsolutions = _nj3;
7084 vinfos[4].jointtype = 1;
7085 vinfos[4].foffset = j4;
7086 vinfos[4].indices[0] = _ij4[0];
7087 vinfos[4].indices[1] = _ij4[1];
7088 vinfos[4].maxsolutions = _nj4;
7089 vinfos[5].jointtype = 1;
7090 vinfos[5].foffset = j5;
7091 vinfos[5].indices[0] = _ij5[0];
7092 vinfos[5].indices[1] = _ij5[1];
7093 vinfos[5].maxsolutions = _nj5;
7094 std::vector<int> vfree(0);
7095 solutions.AddSolution(vinfos,vfree);
7096 }
7097 }
7098 }
7099 
7100 }
7101 } while(0);
7102 if( bgotonextstatement )
7103 {
7104 bool bgotonextstatement = true;
7105 do
7106 {
7107 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7108 evalcond[1]=gconst10;
7109 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
7110 {
7111 bgotonextstatement=false;
7112 {
7113 IkReal j3eval[3];
7114 IkReal x570=((-1.0)*new_r11);
7115 CheckValue<IkReal> x572 = IKatan2WithCheck(IkReal(x570),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7116 if(!x572.valid){
7117 continue;
7118 }
7119 IkReal x571=((1.0)*(x572.value));
7120 sj4=0;
7121 cj4=1.0;
7122 j4=0;
7123 sj5=gconst10;
7124 cj5=gconst11;
7125 j5=((3.14159265)+(((-1.0)*x571)));
7126 new_r00=0;
7127 new_r10=0;
7128 new_r21=0;
7129 new_r22=0;
7130 IkReal gconst9=((3.14159265358979)+(((-1.0)*x571)));
7131 IkReal gconst10=x570;
7132 IkReal gconst11=((1.0)*new_r01);
7133 j3eval[0]=1.0;
7134 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(IKabs(((1.0)*new_r01*new_r11))));
7135 j3eval[2]=1.0;
7136 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7137 {
7138 {
7139 IkReal j3eval[4];
7140 IkReal x573=((-1.0)*new_r11);
7141 CheckValue<IkReal> x575 = IKatan2WithCheck(IkReal(x573),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7142 if(!x575.valid){
7143 continue;
7144 }
7145 IkReal x574=((1.0)*(x575.value));
7146 sj4=0;
7147 cj4=1.0;
7148 j4=0;
7149 sj5=gconst10;
7150 cj5=gconst11;
7151 j5=((3.14159265)+(((-1.0)*x574)));
7152 new_r00=0;
7153 new_r10=0;
7154 new_r21=0;
7155 new_r22=0;
7156 IkReal gconst9=((3.14159265358979)+(((-1.0)*x574)));
7157 IkReal gconst10=x573;
7158 IkReal gconst11=((1.0)*new_r01);
7159 j3eval[0]=-1.0;
7160 j3eval[1]=-1.0;
7161 j3eval[2]=new_r01;
7162 j3eval[3]=1.0;
7163 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
7164 {
7165 {
7166 IkReal j3eval[3];
7167 IkReal x576=((-1.0)*new_r11);
7168 CheckValue<IkReal> x578 = IKatan2WithCheck(IkReal(x576),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7169 if(!x578.valid){
7170 continue;
7171 }
7172 IkReal x577=((1.0)*(x578.value));
7173 sj4=0;
7174 cj4=1.0;
7175 j4=0;
7176 sj5=gconst10;
7177 cj5=gconst11;
7178 j5=((3.14159265)+(((-1.0)*x577)));
7179 new_r00=0;
7180 new_r10=0;
7181 new_r21=0;
7182 new_r22=0;
7183 IkReal gconst9=((3.14159265358979)+(((-1.0)*x577)));
7184 IkReal gconst10=x576;
7185 IkReal gconst11=((1.0)*new_r01);
7186 j3eval[0]=-1.0;
7187 j3eval[1]=-1.0;
7188 j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7189 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7190 {
7191 continue; // 3 cases reached
7192 
7193 } else
7194 {
7195 {
7196 IkReal j3array[1], cj3array[1], sj3array[1];
7197 bool j3valid[1]={false};
7198 _nj3 = 1;
7199 IkReal x579=((1.0)*new_r01);
7200 CheckValue<IkReal> x580=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst11*x579))+((gconst10*new_r11)))),-1);
7201 if(!x580.valid){
7202 continue;
7203 }
7204 CheckValue<IkReal> x581 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst10*gconst10))))),IkReal((((gconst10*gconst11))+(((-1.0)*new_r11*x579)))),IKFAST_ATAN2_MAGTHRESH);
7205 if(!x581.valid){
7206 continue;
7207 }
7208 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x580.value)))+(x581.value));
7209 sj3array[0]=IKsin(j3array[0]);
7210 cj3array[0]=IKcos(j3array[0]);
7211 if( j3array[0] > IKPI )
7212 {
7213  j3array[0]-=IK2PI;
7214 }
7215 else if( j3array[0] < -IKPI )
7216 { j3array[0]+=IK2PI;
7217 }
7218 j3valid[0] = true;
7219 for(int ij3 = 0; ij3 < 1; ++ij3)
7220 {
7221 if( !j3valid[ij3] )
7222 {
7223  continue;
7224 }
7225 _ij3[0] = ij3; _ij3[1] = -1;
7226 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7227 {
7228 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7229 {
7230  j3valid[iij3]=false; _ij3[1] = iij3; break;
7231 }
7232 }
7233 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7234 {
7235 IkReal evalcond[6];
7236 IkReal x582=IKcos(j3);
7237 IkReal x583=IKsin(j3);
7238 IkReal x584=(gconst10*x583);
7239 IkReal x585=((1.0)*x582);
7240 IkReal x586=(gconst11*x583);
7241 IkReal x587=(gconst11*x585);
7242 evalcond[0]=(gconst10+((new_r01*x582))+((new_r11*x583)));
7243 evalcond[1]=(x586+new_r01+((gconst10*x582)));
7244 evalcond[2]=(x584+(((-1.0)*x587)));
7245 evalcond[3]=(gconst11+(((-1.0)*new_r11*x585))+((new_r01*x583)));
7246 evalcond[4]=(x584+new_r11+(((-1.0)*x587)));
7247 evalcond[5]=((((-1.0)*gconst10*x585))+(((-1.0)*x586)));
7248 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7249 {
7250 continue;
7251 }
7252 }
7253 
7254 {
7255 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7256 vinfos[0].jointtype = 1;
7257 vinfos[0].foffset = j0;
7258 vinfos[0].indices[0] = _ij0[0];
7259 vinfos[0].indices[1] = _ij0[1];
7260 vinfos[0].maxsolutions = _nj0;
7261 vinfos[1].jointtype = 1;
7262 vinfos[1].foffset = j1;
7263 vinfos[1].indices[0] = _ij1[0];
7264 vinfos[1].indices[1] = _ij1[1];
7265 vinfos[1].maxsolutions = _nj1;
7266 vinfos[2].jointtype = 1;
7267 vinfos[2].foffset = j2;
7268 vinfos[2].indices[0] = _ij2[0];
7269 vinfos[2].indices[1] = _ij2[1];
7270 vinfos[2].maxsolutions = _nj2;
7271 vinfos[3].jointtype = 1;
7272 vinfos[3].foffset = j3;
7273 vinfos[3].indices[0] = _ij3[0];
7274 vinfos[3].indices[1] = _ij3[1];
7275 vinfos[3].maxsolutions = _nj3;
7276 vinfos[4].jointtype = 1;
7277 vinfos[4].foffset = j4;
7278 vinfos[4].indices[0] = _ij4[0];
7279 vinfos[4].indices[1] = _ij4[1];
7280 vinfos[4].maxsolutions = _nj4;
7281 vinfos[5].jointtype = 1;
7282 vinfos[5].foffset = j5;
7283 vinfos[5].indices[0] = _ij5[0];
7284 vinfos[5].indices[1] = _ij5[1];
7285 vinfos[5].maxsolutions = _nj5;
7286 std::vector<int> vfree(0);
7287 solutions.AddSolution(vinfos,vfree);
7288 }
7289 }
7290 }
7291 
7292 }
7293 
7294 }
7295 
7296 } else
7297 {
7298 {
7299 IkReal j3array[1], cj3array[1], sj3array[1];
7300 bool j3valid[1]={false};
7301 _nj3 = 1;
7302 CheckValue<IkReal> x588=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
7303 if(!x588.valid){
7304 continue;
7305 }
7306 CheckValue<IkReal> x589 = IKatan2WithCheck(IkReal((gconst11*new_r01)),IkReal((gconst10*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7307 if(!x589.valid){
7308 continue;
7309 }
7310 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x588.value)))+(x589.value));
7311 sj3array[0]=IKsin(j3array[0]);
7312 cj3array[0]=IKcos(j3array[0]);
7313 if( j3array[0] > IKPI )
7314 {
7315  j3array[0]-=IK2PI;
7316 }
7317 else if( j3array[0] < -IKPI )
7318 { j3array[0]+=IK2PI;
7319 }
7320 j3valid[0] = true;
7321 for(int ij3 = 0; ij3 < 1; ++ij3)
7322 {
7323 if( !j3valid[ij3] )
7324 {
7325  continue;
7326 }
7327 _ij3[0] = ij3; _ij3[1] = -1;
7328 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7329 {
7330 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7331 {
7332  j3valid[iij3]=false; _ij3[1] = iij3; break;
7333 }
7334 }
7335 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7336 {
7337 IkReal evalcond[6];
7338 IkReal x590=IKcos(j3);
7339 IkReal x591=IKsin(j3);
7340 IkReal x592=(gconst10*x591);
7341 IkReal x593=((1.0)*x590);
7342 IkReal x594=(gconst11*x591);
7343 IkReal x595=(gconst11*x593);
7344 evalcond[0]=(((new_r11*x591))+gconst10+((new_r01*x590)));
7345 evalcond[1]=(((gconst10*x590))+x594+new_r01);
7346 evalcond[2]=((((-1.0)*x595))+x592);
7347 evalcond[3]=((((-1.0)*new_r11*x593))+gconst11+((new_r01*x591)));
7348 evalcond[4]=((((-1.0)*x595))+x592+new_r11);
7349 evalcond[5]=((((-1.0)*x594))+(((-1.0)*gconst10*x593)));
7350 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7351 {
7352 continue;
7353 }
7354 }
7355 
7356 {
7357 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7358 vinfos[0].jointtype = 1;
7359 vinfos[0].foffset = j0;
7360 vinfos[0].indices[0] = _ij0[0];
7361 vinfos[0].indices[1] = _ij0[1];
7362 vinfos[0].maxsolutions = _nj0;
7363 vinfos[1].jointtype = 1;
7364 vinfos[1].foffset = j1;
7365 vinfos[1].indices[0] = _ij1[0];
7366 vinfos[1].indices[1] = _ij1[1];
7367 vinfos[1].maxsolutions = _nj1;
7368 vinfos[2].jointtype = 1;
7369 vinfos[2].foffset = j2;
7370 vinfos[2].indices[0] = _ij2[0];
7371 vinfos[2].indices[1] = _ij2[1];
7372 vinfos[2].maxsolutions = _nj2;
7373 vinfos[3].jointtype = 1;
7374 vinfos[3].foffset = j3;
7375 vinfos[3].indices[0] = _ij3[0];
7376 vinfos[3].indices[1] = _ij3[1];
7377 vinfos[3].maxsolutions = _nj3;
7378 vinfos[4].jointtype = 1;
7379 vinfos[4].foffset = j4;
7380 vinfos[4].indices[0] = _ij4[0];
7381 vinfos[4].indices[1] = _ij4[1];
7382 vinfos[4].maxsolutions = _nj4;
7383 vinfos[5].jointtype = 1;
7384 vinfos[5].foffset = j5;
7385 vinfos[5].indices[0] = _ij5[0];
7386 vinfos[5].indices[1] = _ij5[1];
7387 vinfos[5].maxsolutions = _nj5;
7388 std::vector<int> vfree(0);
7389 solutions.AddSolution(vinfos,vfree);
7390 }
7391 }
7392 }
7393 
7394 }
7395 
7396 }
7397 
7398 } else
7399 {
7400 {
7401 IkReal j3array[1], cj3array[1], sj3array[1];
7402 bool j3valid[1]={false};
7403 _nj3 = 1;
7404 CheckValue<IkReal> x596=IKPowWithIntegerCheck(IKsign((((gconst11*new_r01))+(((-1.0)*gconst10*new_r11)))),-1);
7405 if(!x596.valid){
7406 continue;
7407 }
7408 CheckValue<IkReal> x597 = IKatan2WithCheck(IkReal(gconst10*gconst10),IkReal(((-1.0)*gconst10*gconst11)),IKFAST_ATAN2_MAGTHRESH);
7409 if(!x597.valid){
7410 continue;
7411 }
7412 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x596.value)))+(x597.value));
7413 sj3array[0]=IKsin(j3array[0]);
7414 cj3array[0]=IKcos(j3array[0]);
7415 if( j3array[0] > IKPI )
7416 {
7417  j3array[0]-=IK2PI;
7418 }
7419 else if( j3array[0] < -IKPI )
7420 { j3array[0]+=IK2PI;
7421 }
7422 j3valid[0] = true;
7423 for(int ij3 = 0; ij3 < 1; ++ij3)
7424 {
7425 if( !j3valid[ij3] )
7426 {
7427  continue;
7428 }
7429 _ij3[0] = ij3; _ij3[1] = -1;
7430 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7431 {
7432 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7433 {
7434  j3valid[iij3]=false; _ij3[1] = iij3; break;
7435 }
7436 }
7437 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7438 {
7439 IkReal evalcond[6];
7440 IkReal x598=IKcos(j3);
7441 IkReal x599=IKsin(j3);
7442 IkReal x600=(gconst10*x599);
7443 IkReal x601=((1.0)*x598);
7444 IkReal x602=(gconst11*x599);
7445 IkReal x603=(gconst11*x601);
7446 evalcond[0]=(((new_r11*x599))+gconst10+((new_r01*x598)));
7447 evalcond[1]=(((gconst10*x598))+x602+new_r01);
7448 evalcond[2]=(x600+(((-1.0)*x603)));
7449 evalcond[3]=((((-1.0)*new_r11*x601))+gconst11+((new_r01*x599)));
7450 evalcond[4]=(x600+(((-1.0)*x603))+new_r11);
7451 evalcond[5]=((((-1.0)*gconst10*x601))+(((-1.0)*x602)));
7452 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7453 {
7454 continue;
7455 }
7456 }
7457 
7458 {
7459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7460 vinfos[0].jointtype = 1;
7461 vinfos[0].foffset = j0;
7462 vinfos[0].indices[0] = _ij0[0];
7463 vinfos[0].indices[1] = _ij0[1];
7464 vinfos[0].maxsolutions = _nj0;
7465 vinfos[1].jointtype = 1;
7466 vinfos[1].foffset = j1;
7467 vinfos[1].indices[0] = _ij1[0];
7468 vinfos[1].indices[1] = _ij1[1];
7469 vinfos[1].maxsolutions = _nj1;
7470 vinfos[2].jointtype = 1;
7471 vinfos[2].foffset = j2;
7472 vinfos[2].indices[0] = _ij2[0];
7473 vinfos[2].indices[1] = _ij2[1];
7474 vinfos[2].maxsolutions = _nj2;
7475 vinfos[3].jointtype = 1;
7476 vinfos[3].foffset = j3;
7477 vinfos[3].indices[0] = _ij3[0];
7478 vinfos[3].indices[1] = _ij3[1];
7479 vinfos[3].maxsolutions = _nj3;
7480 vinfos[4].jointtype = 1;
7481 vinfos[4].foffset = j4;
7482 vinfos[4].indices[0] = _ij4[0];
7483 vinfos[4].indices[1] = _ij4[1];
7484 vinfos[4].maxsolutions = _nj4;
7485 vinfos[5].jointtype = 1;
7486 vinfos[5].foffset = j5;
7487 vinfos[5].indices[0] = _ij5[0];
7488 vinfos[5].indices[1] = _ij5[1];
7489 vinfos[5].maxsolutions = _nj5;
7490 std::vector<int> vfree(0);
7491 solutions.AddSolution(vinfos,vfree);
7492 }
7493 }
7494 }
7495 
7496 }
7497 
7498 }
7499 
7500 }
7501 } while(0);
7502 if( bgotonextstatement )
7503 {
7504 bool bgotonextstatement = true;
7505 do
7506 {
7507 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
7508 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7509 {
7510 bgotonextstatement=false;
7511 {
7512 IkReal j3eval[1];
7513 IkReal x604=((-1.0)*new_r11);
7514 CheckValue<IkReal> x606 = IKatan2WithCheck(IkReal(x604),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7515 if(!x606.valid){
7516 continue;
7517 }
7518 IkReal x605=((1.0)*(x606.value));
7519 sj4=0;
7520 cj4=1.0;
7521 j4=0;
7522 sj5=gconst10;
7523 cj5=gconst11;
7524 j5=((3.14159265)+(((-1.0)*x605)));
7525 new_r01=0;
7526 new_r10=0;
7527 IkReal gconst9=((3.14159265358979)+(((-1.0)*x605)));
7528 IkReal x607 = new_r11*new_r11;
7529 if(IKabs(x607)==0){
7530 continue;
7531 }
7532 IkReal gconst10=(x604*(pow(x607,-0.5)));
7533 IkReal gconst11=0;
7534 j3eval[0]=new_r00;
7535 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7536 {
7537 {
7538 IkReal j3eval[1];
7539 IkReal x608=((-1.0)*new_r11);
7540 CheckValue<IkReal> x610 = IKatan2WithCheck(IkReal(x608),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7541 if(!x610.valid){
7542 continue;
7543 }
7544 IkReal x609=((1.0)*(x610.value));
7545 sj4=0;
7546 cj4=1.0;
7547 j4=0;
7548 sj5=gconst10;
7549 cj5=gconst11;
7550 j5=((3.14159265)+(((-1.0)*x609)));
7551 new_r01=0;
7552 new_r10=0;
7553 IkReal gconst9=((3.14159265358979)+(((-1.0)*x609)));
7554 IkReal x611 = new_r11*new_r11;
7555 if(IKabs(x611)==0){
7556 continue;
7557 }
7558 IkReal gconst10=(x608*(pow(x611,-0.5)));
7559 IkReal gconst11=0;
7560 j3eval[0]=new_r11;
7561 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7562 {
7563 {
7564 IkReal j3array[2], cj3array[2], sj3array[2];
7565 bool j3valid[2]={false};
7566 _nj3 = 2;
7567 CheckValue<IkReal> x612=IKPowWithIntegerCheck(gconst10,-1);
7568 if(!x612.valid){
7569 continue;
7570 }
7571 sj3array[0]=((-1.0)*new_r00*(x612.value));
7572 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7573 {
7574  j3valid[0] = j3valid[1] = true;
7575  j3array[0] = IKasin(sj3array[0]);
7576  cj3array[0] = IKcos(j3array[0]);
7577  sj3array[1] = sj3array[0];
7578  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7579  cj3array[1] = -cj3array[0];
7580 }
7581 else if( isnan(sj3array[0]) )
7582 {
7583  // probably any value will work
7584  j3valid[0] = true;
7585  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7586 }
7587 for(int ij3 = 0; ij3 < 2; ++ij3)
7588 {
7589 if( !j3valid[ij3] )
7590 {
7591  continue;
7592 }
7593 _ij3[0] = ij3; _ij3[1] = -1;
7594 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7595 {
7596 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7597 {
7598  j3valid[iij3]=false; _ij3[1] = iij3; break;
7599 }
7600 }
7601 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7602 {
7603 IkReal evalcond[6];
7604 IkReal x613=IKcos(j3);
7605 IkReal x614=IKsin(j3);
7606 evalcond[0]=(gconst10*x613);
7607 evalcond[1]=(new_r00*x613);
7608 evalcond[2]=((-1.0)*new_r11*x613);
7609 evalcond[3]=(gconst10+((new_r00*x614)));
7610 evalcond[4]=(gconst10+((new_r11*x614)));
7611 evalcond[5]=(new_r11+((gconst10*x614)));
7612 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7613 {
7614 continue;
7615 }
7616 }
7617 
7618 {
7619 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7620 vinfos[0].jointtype = 1;
7621 vinfos[0].foffset = j0;
7622 vinfos[0].indices[0] = _ij0[0];
7623 vinfos[0].indices[1] = _ij0[1];
7624 vinfos[0].maxsolutions = _nj0;
7625 vinfos[1].jointtype = 1;
7626 vinfos[1].foffset = j1;
7627 vinfos[1].indices[0] = _ij1[0];
7628 vinfos[1].indices[1] = _ij1[1];
7629 vinfos[1].maxsolutions = _nj1;
7630 vinfos[2].jointtype = 1;
7631 vinfos[2].foffset = j2;
7632 vinfos[2].indices[0] = _ij2[0];
7633 vinfos[2].indices[1] = _ij2[1];
7634 vinfos[2].maxsolutions = _nj2;
7635 vinfos[3].jointtype = 1;
7636 vinfos[3].foffset = j3;
7637 vinfos[3].indices[0] = _ij3[0];
7638 vinfos[3].indices[1] = _ij3[1];
7639 vinfos[3].maxsolutions = _nj3;
7640 vinfos[4].jointtype = 1;
7641 vinfos[4].foffset = j4;
7642 vinfos[4].indices[0] = _ij4[0];
7643 vinfos[4].indices[1] = _ij4[1];
7644 vinfos[4].maxsolutions = _nj4;
7645 vinfos[5].jointtype = 1;
7646 vinfos[5].foffset = j5;
7647 vinfos[5].indices[0] = _ij5[0];
7648 vinfos[5].indices[1] = _ij5[1];
7649 vinfos[5].maxsolutions = _nj5;
7650 std::vector<int> vfree(0);
7651 solutions.AddSolution(vinfos,vfree);
7652 }
7653 }
7654 }
7655 
7656 } else
7657 {
7658 {
7659 IkReal j3array[2], cj3array[2], sj3array[2];
7660 bool j3valid[2]={false};
7661 _nj3 = 2;
7662 CheckValue<IkReal> x615=IKPowWithIntegerCheck(new_r11,-1);
7663 if(!x615.valid){
7664 continue;
7665 }
7666 sj3array[0]=((-1.0)*gconst10*(x615.value));
7667 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7668 {
7669  j3valid[0] = j3valid[1] = true;
7670  j3array[0] = IKasin(sj3array[0]);
7671  cj3array[0] = IKcos(j3array[0]);
7672  sj3array[1] = sj3array[0];
7673  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7674  cj3array[1] = -cj3array[0];
7675 }
7676 else if( isnan(sj3array[0]) )
7677 {
7678  // probably any value will work
7679  j3valid[0] = true;
7680  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7681 }
7682 for(int ij3 = 0; ij3 < 2; ++ij3)
7683 {
7684 if( !j3valid[ij3] )
7685 {
7686  continue;
7687 }
7688 _ij3[0] = ij3; _ij3[1] = -1;
7689 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7690 {
7691 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7692 {
7693  j3valid[iij3]=false; _ij3[1] = iij3; break;
7694 }
7695 }
7696 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7697 {
7698 IkReal evalcond[6];
7699 IkReal x616=IKcos(j3);
7700 IkReal x617=IKsin(j3);
7701 IkReal x618=(gconst10*x617);
7702 evalcond[0]=(gconst10*x616);
7703 evalcond[1]=(new_r00*x616);
7704 evalcond[2]=((-1.0)*new_r11*x616);
7705 evalcond[3]=(gconst10+((new_r00*x617)));
7706 evalcond[4]=(x618+new_r00);
7707 evalcond[5]=(x618+new_r11);
7708 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7709 {
7710 continue;
7711 }
7712 }
7713 
7714 {
7715 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7716 vinfos[0].jointtype = 1;
7717 vinfos[0].foffset = j0;
7718 vinfos[0].indices[0] = _ij0[0];
7719 vinfos[0].indices[1] = _ij0[1];
7720 vinfos[0].maxsolutions = _nj0;
7721 vinfos[1].jointtype = 1;
7722 vinfos[1].foffset = j1;
7723 vinfos[1].indices[0] = _ij1[0];
7724 vinfos[1].indices[1] = _ij1[1];
7725 vinfos[1].maxsolutions = _nj1;
7726 vinfos[2].jointtype = 1;
7727 vinfos[2].foffset = j2;
7728 vinfos[2].indices[0] = _ij2[0];
7729 vinfos[2].indices[1] = _ij2[1];
7730 vinfos[2].maxsolutions = _nj2;
7731 vinfos[3].jointtype = 1;
7732 vinfos[3].foffset = j3;
7733 vinfos[3].indices[0] = _ij3[0];
7734 vinfos[3].indices[1] = _ij3[1];
7735 vinfos[3].maxsolutions = _nj3;
7736 vinfos[4].jointtype = 1;
7737 vinfos[4].foffset = j4;
7738 vinfos[4].indices[0] = _ij4[0];
7739 vinfos[4].indices[1] = _ij4[1];
7740 vinfos[4].maxsolutions = _nj4;
7741 vinfos[5].jointtype = 1;
7742 vinfos[5].foffset = j5;
7743 vinfos[5].indices[0] = _ij5[0];
7744 vinfos[5].indices[1] = _ij5[1];
7745 vinfos[5].maxsolutions = _nj5;
7746 std::vector<int> vfree(0);
7747 solutions.AddSolution(vinfos,vfree);
7748 }
7749 }
7750 }
7751 
7752 }
7753 
7754 }
7755 
7756 } else
7757 {
7758 {
7759 IkReal j3array[2], cj3array[2], sj3array[2];
7760 bool j3valid[2]={false};
7761 _nj3 = 2;
7762 CheckValue<IkReal> x619=IKPowWithIntegerCheck(new_r00,-1);
7763 if(!x619.valid){
7764 continue;
7765 }
7766 sj3array[0]=((-1.0)*gconst10*(x619.value));
7767 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7768 {
7769  j3valid[0] = j3valid[1] = true;
7770  j3array[0] = IKasin(sj3array[0]);
7771  cj3array[0] = IKcos(j3array[0]);
7772  sj3array[1] = sj3array[0];
7773  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7774  cj3array[1] = -cj3array[0];
7775 }
7776 else if( isnan(sj3array[0]) )
7777 {
7778  // probably any value will work
7779  j3valid[0] = true;
7780  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7781 }
7782 for(int ij3 = 0; ij3 < 2; ++ij3)
7783 {
7784 if( !j3valid[ij3] )
7785 {
7786  continue;
7787 }
7788 _ij3[0] = ij3; _ij3[1] = -1;
7789 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7790 {
7791 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7792 {
7793  j3valid[iij3]=false; _ij3[1] = iij3; break;
7794 }
7795 }
7796 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7797 {
7798 IkReal evalcond[6];
7799 IkReal x620=IKcos(j3);
7800 IkReal x621=IKsin(j3);
7801 IkReal x622=(gconst10*x621);
7802 evalcond[0]=(gconst10*x620);
7803 evalcond[1]=(new_r00*x620);
7804 evalcond[2]=((-1.0)*new_r11*x620);
7805 evalcond[3]=(gconst10+((new_r11*x621)));
7806 evalcond[4]=(x622+new_r00);
7807 evalcond[5]=(x622+new_r11);
7808 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7809 {
7810 continue;
7811 }
7812 }
7813 
7814 {
7815 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7816 vinfos[0].jointtype = 1;
7817 vinfos[0].foffset = j0;
7818 vinfos[0].indices[0] = _ij0[0];
7819 vinfos[0].indices[1] = _ij0[1];
7820 vinfos[0].maxsolutions = _nj0;
7821 vinfos[1].jointtype = 1;
7822 vinfos[1].foffset = j1;
7823 vinfos[1].indices[0] = _ij1[0];
7824 vinfos[1].indices[1] = _ij1[1];
7825 vinfos[1].maxsolutions = _nj1;
7826 vinfos[2].jointtype = 1;
7827 vinfos[2].foffset = j2;
7828 vinfos[2].indices[0] = _ij2[0];
7829 vinfos[2].indices[1] = _ij2[1];
7830 vinfos[2].maxsolutions = _nj2;
7831 vinfos[3].jointtype = 1;
7832 vinfos[3].foffset = j3;
7833 vinfos[3].indices[0] = _ij3[0];
7834 vinfos[3].indices[1] = _ij3[1];
7835 vinfos[3].maxsolutions = _nj3;
7836 vinfos[4].jointtype = 1;
7837 vinfos[4].foffset = j4;
7838 vinfos[4].indices[0] = _ij4[0];
7839 vinfos[4].indices[1] = _ij4[1];
7840 vinfos[4].maxsolutions = _nj4;
7841 vinfos[5].jointtype = 1;
7842 vinfos[5].foffset = j5;
7843 vinfos[5].indices[0] = _ij5[0];
7844 vinfos[5].indices[1] = _ij5[1];
7845 vinfos[5].maxsolutions = _nj5;
7846 std::vector<int> vfree(0);
7847 solutions.AddSolution(vinfos,vfree);
7848 }
7849 }
7850 }
7851 
7852 }
7853 
7854 }
7855 
7856 }
7857 } while(0);
7858 if( bgotonextstatement )
7859 {
7860 bool bgotonextstatement = true;
7861 do
7862 {
7863 evalcond[0]=IKabs(new_r11);
7864 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7865 {
7866 bgotonextstatement=false;
7867 {
7868 IkReal j3eval[1];
7869 CheckValue<IkReal> x624 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7870 if(!x624.valid){
7871 continue;
7872 }
7873 IkReal x623=((1.0)*(x624.value));
7874 sj4=0;
7875 cj4=1.0;
7876 j4=0;
7877 sj5=gconst10;
7878 cj5=gconst11;
7879 j5=((3.14159265)+(((-1.0)*x623)));
7880 new_r11=0;
7881 IkReal gconst9=((3.14159265358979)+(((-1.0)*x623)));
7882 IkReal gconst10=0;
7883 IkReal x625 = new_r01*new_r01;
7884 if(IKabs(x625)==0){
7885 continue;
7886 }
7887 IkReal gconst11=((1.0)*new_r01*(pow(x625,-0.5)));
7888 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7889 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7890 {
7891 {
7892 IkReal j3eval[1];
7893 CheckValue<IkReal> x627 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7894 if(!x627.valid){
7895 continue;
7896 }
7897 IkReal x626=((1.0)*(x627.value));
7898 sj4=0;
7899 cj4=1.0;
7900 j4=0;
7901 sj5=gconst10;
7902 cj5=gconst11;
7903 j5=((3.14159265)+(((-1.0)*x626)));
7904 new_r11=0;
7905 IkReal gconst9=((3.14159265358979)+(((-1.0)*x626)));
7906 IkReal gconst10=0;
7907 IkReal x628 = new_r01*new_r01;
7908 if(IKabs(x628)==0){
7909 continue;
7910 }
7911 IkReal gconst11=((1.0)*new_r01*(pow(x628,-0.5)));
7912 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
7913 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7914 {
7915 {
7916 IkReal j3eval[1];
7917 CheckValue<IkReal> x630 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7918 if(!x630.valid){
7919 continue;
7920 }
7921 IkReal x629=((1.0)*(x630.value));
7922 sj4=0;
7923 cj4=1.0;
7924 j4=0;
7925 sj5=gconst10;
7926 cj5=gconst11;
7927 j5=((3.14159265)+(((-1.0)*x629)));
7928 new_r11=0;
7929 IkReal gconst9=((3.14159265358979)+(((-1.0)*x629)));
7930 IkReal gconst10=0;
7931 IkReal x631 = new_r01*new_r01;
7932 if(IKabs(x631)==0){
7933 continue;
7934 }
7935 IkReal gconst11=((1.0)*new_r01*(pow(x631,-0.5)));
7936 j3eval[0]=new_r01;
7937 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7938 {
7939 continue; // 3 cases reached
7940 
7941 } else
7942 {
7943 {
7944 IkReal j3array[1], cj3array[1], sj3array[1];
7945 bool j3valid[1]={false};
7946 _nj3 = 1;
7947 CheckValue<IkReal> x632=IKPowWithIntegerCheck(new_r01,-1);
7948 if(!x632.valid){
7949 continue;
7950 }
7951 CheckValue<IkReal> x633=IKPowWithIntegerCheck(gconst11,-1);
7952 if(!x633.valid){
7953 continue;
7954 }
7955 if( IKabs(((-1.0)*gconst11*(x632.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x633.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst11*(x632.value)))+IKsqr((new_r00*(x633.value)))-1) <= IKFAST_SINCOS_THRESH )
7956  continue;
7957 j3array[0]=IKatan2(((-1.0)*gconst11*(x632.value)), (new_r00*(x633.value)));
7958 sj3array[0]=IKsin(j3array[0]);
7959 cj3array[0]=IKcos(j3array[0]);
7960 if( j3array[0] > IKPI )
7961 {
7962  j3array[0]-=IK2PI;
7963 }
7964 else if( j3array[0] < -IKPI )
7965 { j3array[0]+=IK2PI;
7966 }
7967 j3valid[0] = true;
7968 for(int ij3 = 0; ij3 < 1; ++ij3)
7969 {
7970 if( !j3valid[ij3] )
7971 {
7972  continue;
7973 }
7974 _ij3[0] = ij3; _ij3[1] = -1;
7975 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7976 {
7977 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7978 {
7979  j3valid[iij3]=false; _ij3[1] = iij3; break;
7980 }
7981 }
7982 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7983 {
7984 IkReal evalcond[8];
7985 IkReal x634=IKcos(j3);
7986 IkReal x635=IKsin(j3);
7987 IkReal x636=((1.0)*gconst11);
7988 IkReal x637=(gconst11*x634);
7989 evalcond[0]=(new_r01*x634);
7990 evalcond[1]=((-1.0)*x637);
7991 evalcond[2]=(gconst11+((new_r01*x635)));
7992 evalcond[3]=(((gconst11*x635))+new_r01);
7993 evalcond[4]=((((-1.0)*x634*x636))+new_r00);
7994 evalcond[5]=((((-1.0)*x635*x636))+new_r10);
7995 evalcond[6]=((((-1.0)*new_r10*x634))+((new_r00*x635)));
7996 evalcond[7]=((((-1.0)*x636))+((new_r00*x634))+((new_r10*x635)));
7997 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
7998 {
7999 continue;
8000 }
8001 }
8002 
8003 {
8004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8005 vinfos[0].jointtype = 1;
8006 vinfos[0].foffset = j0;
8007 vinfos[0].indices[0] = _ij0[0];
8008 vinfos[0].indices[1] = _ij0[1];
8009 vinfos[0].maxsolutions = _nj0;
8010 vinfos[1].jointtype = 1;
8011 vinfos[1].foffset = j1;
8012 vinfos[1].indices[0] = _ij1[0];
8013 vinfos[1].indices[1] = _ij1[1];
8014 vinfos[1].maxsolutions = _nj1;
8015 vinfos[2].jointtype = 1;
8016 vinfos[2].foffset = j2;
8017 vinfos[2].indices[0] = _ij2[0];
8018 vinfos[2].indices[1] = _ij2[1];
8019 vinfos[2].maxsolutions = _nj2;
8020 vinfos[3].jointtype = 1;
8021 vinfos[3].foffset = j3;
8022 vinfos[3].indices[0] = _ij3[0];
8023 vinfos[3].indices[1] = _ij3[1];
8024 vinfos[3].maxsolutions = _nj3;
8025 vinfos[4].jointtype = 1;
8026 vinfos[4].foffset = j4;
8027 vinfos[4].indices[0] = _ij4[0];
8028 vinfos[4].indices[1] = _ij4[1];
8029 vinfos[4].maxsolutions = _nj4;
8030 vinfos[5].jointtype = 1;
8031 vinfos[5].foffset = j5;
8032 vinfos[5].indices[0] = _ij5[0];
8033 vinfos[5].indices[1] = _ij5[1];
8034 vinfos[5].maxsolutions = _nj5;
8035 std::vector<int> vfree(0);
8036 solutions.AddSolution(vinfos,vfree);
8037 }
8038 }
8039 }
8040 
8041 }
8042 
8043 }
8044 
8045 } else
8046 {
8047 {
8048 IkReal j3array[1], cj3array[1], sj3array[1];
8049 bool j3valid[1]={false};
8050 _nj3 = 1;
8051 CheckValue<IkReal> x638=IKPowWithIntegerCheck(IKsign(gconst11),-1);
8052 if(!x638.valid){
8053 continue;
8054 }
8055 CheckValue<IkReal> x639 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8056 if(!x639.valid){
8057 continue;
8058 }
8059 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x638.value)))+(x639.value));
8060 sj3array[0]=IKsin(j3array[0]);
8061 cj3array[0]=IKcos(j3array[0]);
8062 if( j3array[0] > IKPI )
8063 {
8064  j3array[0]-=IK2PI;
8065 }
8066 else if( j3array[0] < -IKPI )
8067 { j3array[0]+=IK2PI;
8068 }
8069 j3valid[0] = true;
8070 for(int ij3 = 0; ij3 < 1; ++ij3)
8071 {
8072 if( !j3valid[ij3] )
8073 {
8074  continue;
8075 }
8076 _ij3[0] = ij3; _ij3[1] = -1;
8077 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8078 {
8079 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8080 {
8081  j3valid[iij3]=false; _ij3[1] = iij3; break;
8082 }
8083 }
8084 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8085 {
8086 IkReal evalcond[8];
8087 IkReal x640=IKcos(j3);
8088 IkReal x641=IKsin(j3);
8089 IkReal x642=((1.0)*gconst11);
8090 IkReal x643=(gconst11*x640);
8091 evalcond[0]=(new_r01*x640);
8092 evalcond[1]=((-1.0)*x643);
8093 evalcond[2]=(gconst11+((new_r01*x641)));
8094 evalcond[3]=(((gconst11*x641))+new_r01);
8095 evalcond[4]=((((-1.0)*x640*x642))+new_r00);
8096 evalcond[5]=((((-1.0)*x641*x642))+new_r10);
8097 evalcond[6]=((((-1.0)*new_r10*x640))+((new_r00*x641)));
8098 evalcond[7]=((((-1.0)*x642))+((new_r10*x641))+((new_r00*x640)));
8099 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8100 {
8101 continue;
8102 }
8103 }
8104 
8105 {
8106 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8107 vinfos[0].jointtype = 1;
8108 vinfos[0].foffset = j0;
8109 vinfos[0].indices[0] = _ij0[0];
8110 vinfos[0].indices[1] = _ij0[1];
8111 vinfos[0].maxsolutions = _nj0;
8112 vinfos[1].jointtype = 1;
8113 vinfos[1].foffset = j1;
8114 vinfos[1].indices[0] = _ij1[0];
8115 vinfos[1].indices[1] = _ij1[1];
8116 vinfos[1].maxsolutions = _nj1;
8117 vinfos[2].jointtype = 1;
8118 vinfos[2].foffset = j2;
8119 vinfos[2].indices[0] = _ij2[0];
8120 vinfos[2].indices[1] = _ij2[1];
8121 vinfos[2].maxsolutions = _nj2;
8122 vinfos[3].jointtype = 1;
8123 vinfos[3].foffset = j3;
8124 vinfos[3].indices[0] = _ij3[0];
8125 vinfos[3].indices[1] = _ij3[1];
8126 vinfos[3].maxsolutions = _nj3;
8127 vinfos[4].jointtype = 1;
8128 vinfos[4].foffset = j4;
8129 vinfos[4].indices[0] = _ij4[0];
8130 vinfos[4].indices[1] = _ij4[1];
8131 vinfos[4].maxsolutions = _nj4;
8132 vinfos[5].jointtype = 1;
8133 vinfos[5].foffset = j5;
8134 vinfos[5].indices[0] = _ij5[0];
8135 vinfos[5].indices[1] = _ij5[1];
8136 vinfos[5].maxsolutions = _nj5;
8137 std::vector<int> vfree(0);
8138 solutions.AddSolution(vinfos,vfree);
8139 }
8140 }
8141 }
8142 
8143 }
8144 
8145 }
8146 
8147 } else
8148 {
8149 {
8150 IkReal j3array[1], cj3array[1], sj3array[1];
8151 bool j3valid[1]={false};
8152 _nj3 = 1;
8153 CheckValue<IkReal> x644=IKPowWithIntegerCheck(IKsign(gconst11),-1);
8154 if(!x644.valid){
8155 continue;
8156 }
8157 CheckValue<IkReal> x645 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8158 if(!x645.valid){
8159 continue;
8160 }
8161 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x644.value)))+(x645.value));
8162 sj3array[0]=IKsin(j3array[0]);
8163 cj3array[0]=IKcos(j3array[0]);
8164 if( j3array[0] > IKPI )
8165 {
8166  j3array[0]-=IK2PI;
8167 }
8168 else if( j3array[0] < -IKPI )
8169 { j3array[0]+=IK2PI;
8170 }
8171 j3valid[0] = true;
8172 for(int ij3 = 0; ij3 < 1; ++ij3)
8173 {
8174 if( !j3valid[ij3] )
8175 {
8176  continue;
8177 }
8178 _ij3[0] = ij3; _ij3[1] = -1;
8179 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8180 {
8181 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8182 {
8183  j3valid[iij3]=false; _ij3[1] = iij3; break;
8184 }
8185 }
8186 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8187 {
8188 IkReal evalcond[8];
8189 IkReal x646=IKcos(j3);
8190 IkReal x647=IKsin(j3);
8191 IkReal x648=((1.0)*gconst11);
8192 IkReal x649=(gconst11*x646);
8193 evalcond[0]=(new_r01*x646);
8194 evalcond[1]=((-1.0)*x649);
8195 evalcond[2]=(gconst11+((new_r01*x647)));
8196 evalcond[3]=(((gconst11*x647))+new_r01);
8197 evalcond[4]=((((-1.0)*x646*x648))+new_r00);
8198 evalcond[5]=((((-1.0)*x647*x648))+new_r10);
8199 evalcond[6]=((((-1.0)*new_r10*x646))+((new_r00*x647)));
8200 evalcond[7]=((((-1.0)*x648))+((new_r10*x647))+((new_r00*x646)));
8201 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8202 {
8203 continue;
8204 }
8205 }
8206 
8207 {
8208 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8209 vinfos[0].jointtype = 1;
8210 vinfos[0].foffset = j0;
8211 vinfos[0].indices[0] = _ij0[0];
8212 vinfos[0].indices[1] = _ij0[1];
8213 vinfos[0].maxsolutions = _nj0;
8214 vinfos[1].jointtype = 1;
8215 vinfos[1].foffset = j1;
8216 vinfos[1].indices[0] = _ij1[0];
8217 vinfos[1].indices[1] = _ij1[1];
8218 vinfos[1].maxsolutions = _nj1;
8219 vinfos[2].jointtype = 1;
8220 vinfos[2].foffset = j2;
8221 vinfos[2].indices[0] = _ij2[0];
8222 vinfos[2].indices[1] = _ij2[1];
8223 vinfos[2].maxsolutions = _nj2;
8224 vinfos[3].jointtype = 1;
8225 vinfos[3].foffset = j3;
8226 vinfos[3].indices[0] = _ij3[0];
8227 vinfos[3].indices[1] = _ij3[1];
8228 vinfos[3].maxsolutions = _nj3;
8229 vinfos[4].jointtype = 1;
8230 vinfos[4].foffset = j4;
8231 vinfos[4].indices[0] = _ij4[0];
8232 vinfos[4].indices[1] = _ij4[1];
8233 vinfos[4].maxsolutions = _nj4;
8234 vinfos[5].jointtype = 1;
8235 vinfos[5].foffset = j5;
8236 vinfos[5].indices[0] = _ij5[0];
8237 vinfos[5].indices[1] = _ij5[1];
8238 vinfos[5].maxsolutions = _nj5;
8239 std::vector<int> vfree(0);
8240 solutions.AddSolution(vinfos,vfree);
8241 }
8242 }
8243 }
8244 
8245 }
8246 
8247 }
8248 
8249 }
8250 } while(0);
8251 if( bgotonextstatement )
8252 {
8253 bool bgotonextstatement = true;
8254 do
8255 {
8256 if( 1 )
8257 {
8258 bgotonextstatement=false;
8259 continue; // branch miss [j3]
8260 
8261 }
8262 } while(0);
8263 if( bgotonextstatement )
8264 {
8265 }
8266 }
8267 }
8268 }
8269 }
8270 }
8271 
8272 } else
8273 {
8274 {
8275 IkReal j3array[1], cj3array[1], sj3array[1];
8276 bool j3valid[1]={false};
8277 _nj3 = 1;
8278 IkReal x650=((1.0)*new_r01);
8279 CheckValue<IkReal> x651=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst11*x650))+((gconst10*new_r11)))),-1);
8280 if(!x651.valid){
8281 continue;
8282 }
8283 CheckValue<IkReal> x652 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst10*gconst10))))),IkReal((((gconst10*gconst11))+(((-1.0)*new_r11*x650)))),IKFAST_ATAN2_MAGTHRESH);
8284 if(!x652.valid){
8285 continue;
8286 }
8287 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x651.value)))+(x652.value));
8288 sj3array[0]=IKsin(j3array[0]);
8289 cj3array[0]=IKcos(j3array[0]);
8290 if( j3array[0] > IKPI )
8291 {
8292  j3array[0]-=IK2PI;
8293 }
8294 else if( j3array[0] < -IKPI )
8295 { j3array[0]+=IK2PI;
8296 }
8297 j3valid[0] = true;
8298 for(int ij3 = 0; ij3 < 1; ++ij3)
8299 {
8300 if( !j3valid[ij3] )
8301 {
8302  continue;
8303 }
8304 _ij3[0] = ij3; _ij3[1] = -1;
8305 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8306 {
8307 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8308 {
8309  j3valid[iij3]=false; _ij3[1] = iij3; break;
8310 }
8311 }
8312 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8313 {
8314 IkReal evalcond[8];
8315 IkReal x653=IKcos(j3);
8316 IkReal x654=IKsin(j3);
8317 IkReal x655=((1.0)*gconst11);
8318 IkReal x656=(gconst10*x654);
8319 IkReal x657=((1.0)*x653);
8320 IkReal x658=(x653*x655);
8321 evalcond[0]=(gconst10+((new_r01*x653))+((new_r11*x654)));
8322 evalcond[1]=(((gconst10*x653))+((gconst11*x654))+new_r01);
8323 evalcond[2]=(gconst10+(((-1.0)*new_r10*x657))+((new_r00*x654)));
8324 evalcond[3]=(gconst11+(((-1.0)*new_r11*x657))+((new_r01*x654)));
8325 evalcond[4]=((((-1.0)*x658))+x656+new_r00);
8326 evalcond[5]=((((-1.0)*x658))+x656+new_r11);
8327 evalcond[6]=((((-1.0)*x655))+((new_r10*x654))+((new_r00*x653)));
8328 evalcond[7]=((((-1.0)*gconst10*x657))+new_r10+(((-1.0)*x654*x655)));
8329 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8330 {
8331 continue;
8332 }
8333 }
8334 
8335 {
8336 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8337 vinfos[0].jointtype = 1;
8338 vinfos[0].foffset = j0;
8339 vinfos[0].indices[0] = _ij0[0];
8340 vinfos[0].indices[1] = _ij0[1];
8341 vinfos[0].maxsolutions = _nj0;
8342 vinfos[1].jointtype = 1;
8343 vinfos[1].foffset = j1;
8344 vinfos[1].indices[0] = _ij1[0];
8345 vinfos[1].indices[1] = _ij1[1];
8346 vinfos[1].maxsolutions = _nj1;
8347 vinfos[2].jointtype = 1;
8348 vinfos[2].foffset = j2;
8349 vinfos[2].indices[0] = _ij2[0];
8350 vinfos[2].indices[1] = _ij2[1];
8351 vinfos[2].maxsolutions = _nj2;
8352 vinfos[3].jointtype = 1;
8353 vinfos[3].foffset = j3;
8354 vinfos[3].indices[0] = _ij3[0];
8355 vinfos[3].indices[1] = _ij3[1];
8356 vinfos[3].maxsolutions = _nj3;
8357 vinfos[4].jointtype = 1;
8358 vinfos[4].foffset = j4;
8359 vinfos[4].indices[0] = _ij4[0];
8360 vinfos[4].indices[1] = _ij4[1];
8361 vinfos[4].maxsolutions = _nj4;
8362 vinfos[5].jointtype = 1;
8363 vinfos[5].foffset = j5;
8364 vinfos[5].indices[0] = _ij5[0];
8365 vinfos[5].indices[1] = _ij5[1];
8366 vinfos[5].maxsolutions = _nj5;
8367 std::vector<int> vfree(0);
8368 solutions.AddSolution(vinfos,vfree);
8369 }
8370 }
8371 }
8372 
8373 }
8374 
8375 }
8376 
8377 } else
8378 {
8379 {
8380 IkReal j3array[1], cj3array[1], sj3array[1];
8381 bool j3valid[1]={false};
8382 _nj3 = 1;
8383 IkReal x659=((1.0)*gconst10);
8384 CheckValue<IkReal> x660=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*x659))+(((-1.0)*gconst11*new_r10)))),-1);
8385 if(!x660.valid){
8386 continue;
8387 }
8388 CheckValue<IkReal> x661 = IKatan2WithCheck(IkReal(((gconst10*gconst10)+((new_r01*new_r10)))),IkReal(((((-1.0)*gconst11*x659))+((new_r00*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
8389 if(!x661.valid){
8390 continue;
8391 }
8392 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x660.value)))+(x661.value));
8393 sj3array[0]=IKsin(j3array[0]);
8394 cj3array[0]=IKcos(j3array[0]);
8395 if( j3array[0] > IKPI )
8396 {
8397  j3array[0]-=IK2PI;
8398 }
8399 else if( j3array[0] < -IKPI )
8400 { j3array[0]+=IK2PI;
8401 }
8402 j3valid[0] = true;
8403 for(int ij3 = 0; ij3 < 1; ++ij3)
8404 {
8405 if( !j3valid[ij3] )
8406 {
8407  continue;
8408 }
8409 _ij3[0] = ij3; _ij3[1] = -1;
8410 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8411 {
8412 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8413 {
8414  j3valid[iij3]=false; _ij3[1] = iij3; break;
8415 }
8416 }
8417 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8418 {
8419 IkReal evalcond[8];
8420 IkReal x662=IKcos(j3);
8421 IkReal x663=IKsin(j3);
8422 IkReal x664=((1.0)*gconst11);
8423 IkReal x665=(gconst10*x663);
8424 IkReal x666=((1.0)*x662);
8425 IkReal x667=(x662*x664);
8426 evalcond[0]=(gconst10+((new_r11*x663))+((new_r01*x662)));
8427 evalcond[1]=(new_r01+((gconst10*x662))+((gconst11*x663)));
8428 evalcond[2]=(gconst10+(((-1.0)*new_r10*x666))+((new_r00*x663)));
8429 evalcond[3]=((((-1.0)*new_r11*x666))+gconst11+((new_r01*x663)));
8430 evalcond[4]=((((-1.0)*x667))+x665+new_r00);
8431 evalcond[5]=((((-1.0)*x667))+x665+new_r11);
8432 evalcond[6]=(((new_r10*x663))+(((-1.0)*x664))+((new_r00*x662)));
8433 evalcond[7]=((((-1.0)*gconst10*x666))+(((-1.0)*x663*x664))+new_r10);
8434 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8435 {
8436 continue;
8437 }
8438 }
8439 
8440 {
8441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8442 vinfos[0].jointtype = 1;
8443 vinfos[0].foffset = j0;
8444 vinfos[0].indices[0] = _ij0[0];
8445 vinfos[0].indices[1] = _ij0[1];
8446 vinfos[0].maxsolutions = _nj0;
8447 vinfos[1].jointtype = 1;
8448 vinfos[1].foffset = j1;
8449 vinfos[1].indices[0] = _ij1[0];
8450 vinfos[1].indices[1] = _ij1[1];
8451 vinfos[1].maxsolutions = _nj1;
8452 vinfos[2].jointtype = 1;
8453 vinfos[2].foffset = j2;
8454 vinfos[2].indices[0] = _ij2[0];
8455 vinfos[2].indices[1] = _ij2[1];
8456 vinfos[2].maxsolutions = _nj2;
8457 vinfos[3].jointtype = 1;
8458 vinfos[3].foffset = j3;
8459 vinfos[3].indices[0] = _ij3[0];
8460 vinfos[3].indices[1] = _ij3[1];
8461 vinfos[3].maxsolutions = _nj3;
8462 vinfos[4].jointtype = 1;
8463 vinfos[4].foffset = j4;
8464 vinfos[4].indices[0] = _ij4[0];
8465 vinfos[4].indices[1] = _ij4[1];
8466 vinfos[4].maxsolutions = _nj4;
8467 vinfos[5].jointtype = 1;
8468 vinfos[5].foffset = j5;
8469 vinfos[5].indices[0] = _ij5[0];
8470 vinfos[5].indices[1] = _ij5[1];
8471 vinfos[5].maxsolutions = _nj5;
8472 std::vector<int> vfree(0);
8473 solutions.AddSolution(vinfos,vfree);
8474 }
8475 }
8476 }
8477 
8478 }
8479 
8480 }
8481 
8482 } else
8483 {
8484 {
8485 IkReal j3array[1], cj3array[1], sj3array[1];
8486 bool j3valid[1]={false};
8487 _nj3 = 1;
8488 IkReal x668=((1.0)*new_r11);
8489 CheckValue<IkReal> x669 = IKatan2WithCheck(IkReal((((gconst10*new_r01))+((gconst10*new_r10)))),IkReal(((((-1.0)*gconst10*x668))+((gconst10*new_r00)))),IKFAST_ATAN2_MAGTHRESH);
8490 if(!x669.valid){
8491 continue;
8492 }
8493 CheckValue<IkReal> x670=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x668))+(((-1.0)*new_r00*new_r01)))),-1);
8494 if(!x670.valid){
8495 continue;
8496 }
8497 j3array[0]=((-1.5707963267949)+(x669.value)+(((1.5707963267949)*(x670.value))));
8498 sj3array[0]=IKsin(j3array[0]);
8499 cj3array[0]=IKcos(j3array[0]);
8500 if( j3array[0] > IKPI )
8501 {
8502  j3array[0]-=IK2PI;
8503 }
8504 else if( j3array[0] < -IKPI )
8505 { j3array[0]+=IK2PI;
8506 }
8507 j3valid[0] = true;
8508 for(int ij3 = 0; ij3 < 1; ++ij3)
8509 {
8510 if( !j3valid[ij3] )
8511 {
8512  continue;
8513 }
8514 _ij3[0] = ij3; _ij3[1] = -1;
8515 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8516 {
8517 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8518 {
8519  j3valid[iij3]=false; _ij3[1] = iij3; break;
8520 }
8521 }
8522 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8523 {
8524 IkReal evalcond[8];
8525 IkReal x671=IKcos(j3);
8526 IkReal x672=IKsin(j3);
8527 IkReal x673=((1.0)*gconst11);
8528 IkReal x674=(gconst10*x672);
8529 IkReal x675=((1.0)*x671);
8530 IkReal x676=(x671*x673);
8531 evalcond[0]=(((new_r11*x672))+((new_r01*x671))+gconst10);
8532 evalcond[1]=(((gconst11*x672))+((gconst10*x671))+new_r01);
8533 evalcond[2]=(((new_r00*x672))+gconst10+(((-1.0)*new_r10*x675)));
8534 evalcond[3]=(((new_r01*x672))+gconst11+(((-1.0)*new_r11*x675)));
8535 evalcond[4]=(x674+new_r00+(((-1.0)*x676)));
8536 evalcond[5]=(x674+new_r11+(((-1.0)*x676)));
8537 evalcond[6]=(((new_r00*x671))+((new_r10*x672))+(((-1.0)*x673)));
8538 evalcond[7]=((((-1.0)*x672*x673))+new_r10+(((-1.0)*gconst10*x675)));
8539 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8540 {
8541 continue;
8542 }
8543 }
8544 
8545 {
8546 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8547 vinfos[0].jointtype = 1;
8548 vinfos[0].foffset = j0;
8549 vinfos[0].indices[0] = _ij0[0];
8550 vinfos[0].indices[1] = _ij0[1];
8551 vinfos[0].maxsolutions = _nj0;
8552 vinfos[1].jointtype = 1;
8553 vinfos[1].foffset = j1;
8554 vinfos[1].indices[0] = _ij1[0];
8555 vinfos[1].indices[1] = _ij1[1];
8556 vinfos[1].maxsolutions = _nj1;
8557 vinfos[2].jointtype = 1;
8558 vinfos[2].foffset = j2;
8559 vinfos[2].indices[0] = _ij2[0];
8560 vinfos[2].indices[1] = _ij2[1];
8561 vinfos[2].maxsolutions = _nj2;
8562 vinfos[3].jointtype = 1;
8563 vinfos[3].foffset = j3;
8564 vinfos[3].indices[0] = _ij3[0];
8565 vinfos[3].indices[1] = _ij3[1];
8566 vinfos[3].maxsolutions = _nj3;
8567 vinfos[4].jointtype = 1;
8568 vinfos[4].foffset = j4;
8569 vinfos[4].indices[0] = _ij4[0];
8570 vinfos[4].indices[1] = _ij4[1];
8571 vinfos[4].maxsolutions = _nj4;
8572 vinfos[5].jointtype = 1;
8573 vinfos[5].foffset = j5;
8574 vinfos[5].indices[0] = _ij5[0];
8575 vinfos[5].indices[1] = _ij5[1];
8576 vinfos[5].maxsolutions = _nj5;
8577 std::vector<int> vfree(0);
8578 solutions.AddSolution(vinfos,vfree);
8579 }
8580 }
8581 }
8582 
8583 }
8584 
8585 }
8586 
8587 }
8588 } while(0);
8589 if( bgotonextstatement )
8590 {
8591 bool bgotonextstatement = true;
8592 do
8593 {
8594 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8595 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8596 {
8597 bgotonextstatement=false;
8598 {
8599 IkReal j3array[1], cj3array[1], sj3array[1];
8600 bool j3valid[1]={false};
8601 _nj3 = 1;
8602 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 )
8603  continue;
8604 j3array[0]=IKatan2(((-1.0)*new_r01), new_r00);
8605 sj3array[0]=IKsin(j3array[0]);
8606 cj3array[0]=IKcos(j3array[0]);
8607 if( j3array[0] > IKPI )
8608 {
8609  j3array[0]-=IK2PI;
8610 }
8611 else if( j3array[0] < -IKPI )
8612 { j3array[0]+=IK2PI;
8613 }
8614 j3valid[0] = true;
8615 for(int ij3 = 0; ij3 < 1; ++ij3)
8616 {
8617 if( !j3valid[ij3] )
8618 {
8619  continue;
8620 }
8621 _ij3[0] = ij3; _ij3[1] = -1;
8622 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8623 {
8624 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8625 {
8626  j3valid[iij3]=false; _ij3[1] = iij3; break;
8627 }
8628 }
8629 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8630 {
8631 IkReal evalcond[8];
8632 IkReal x677=IKsin(j3);
8633 IkReal x678=IKcos(j3);
8634 IkReal x679=((1.0)*x678);
8635 evalcond[0]=(x677+new_r01);
8636 evalcond[1]=(new_r00+(((-1.0)*x679)));
8637 evalcond[2]=(new_r11+(((-1.0)*x679)));
8638 evalcond[3]=((((-1.0)*x677))+new_r10);
8639 evalcond[4]=(((new_r11*x677))+((new_r01*x678)));
8640 evalcond[5]=(((new_r00*x677))+(((-1.0)*new_r10*x679)));
8641 evalcond[6]=((-1.0)+((new_r00*x678))+((new_r10*x677)));
8642 evalcond[7]=((1.0)+((new_r01*x677))+(((-1.0)*new_r11*x679)));
8643 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8644 {
8645 continue;
8646 }
8647 }
8648 
8649 {
8650 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8651 vinfos[0].jointtype = 1;
8652 vinfos[0].foffset = j0;
8653 vinfos[0].indices[0] = _ij0[0];
8654 vinfos[0].indices[1] = _ij0[1];
8655 vinfos[0].maxsolutions = _nj0;
8656 vinfos[1].jointtype = 1;
8657 vinfos[1].foffset = j1;
8658 vinfos[1].indices[0] = _ij1[0];
8659 vinfos[1].indices[1] = _ij1[1];
8660 vinfos[1].maxsolutions = _nj1;
8661 vinfos[2].jointtype = 1;
8662 vinfos[2].foffset = j2;
8663 vinfos[2].indices[0] = _ij2[0];
8664 vinfos[2].indices[1] = _ij2[1];
8665 vinfos[2].maxsolutions = _nj2;
8666 vinfos[3].jointtype = 1;
8667 vinfos[3].foffset = j3;
8668 vinfos[3].indices[0] = _ij3[0];
8669 vinfos[3].indices[1] = _ij3[1];
8670 vinfos[3].maxsolutions = _nj3;
8671 vinfos[4].jointtype = 1;
8672 vinfos[4].foffset = j4;
8673 vinfos[4].indices[0] = _ij4[0];
8674 vinfos[4].indices[1] = _ij4[1];
8675 vinfos[4].maxsolutions = _nj4;
8676 vinfos[5].jointtype = 1;
8677 vinfos[5].foffset = j5;
8678 vinfos[5].indices[0] = _ij5[0];
8679 vinfos[5].indices[1] = _ij5[1];
8680 vinfos[5].maxsolutions = _nj5;
8681 std::vector<int> vfree(0);
8682 solutions.AddSolution(vinfos,vfree);
8683 }
8684 }
8685 }
8686 
8687 }
8688 } while(0);
8689 if( bgotonextstatement )
8690 {
8691 bool bgotonextstatement = true;
8692 do
8693 {
8694 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8695 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8696 {
8697 bgotonextstatement=false;
8698 {
8699 IkReal j3array[1], cj3array[1], sj3array[1];
8700 bool j3valid[1]={false};
8701 _nj3 = 1;
8702 if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
8703  continue;
8704 j3array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8705 sj3array[0]=IKsin(j3array[0]);
8706 cj3array[0]=IKcos(j3array[0]);
8707 if( j3array[0] > IKPI )
8708 {
8709  j3array[0]-=IK2PI;
8710 }
8711 else if( j3array[0] < -IKPI )
8712 { j3array[0]+=IK2PI;
8713 }
8714 j3valid[0] = true;
8715 for(int ij3 = 0; ij3 < 1; ++ij3)
8716 {
8717 if( !j3valid[ij3] )
8718 {
8719  continue;
8720 }
8721 _ij3[0] = ij3; _ij3[1] = -1;
8722 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8723 {
8724 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8725 {
8726  j3valid[iij3]=false; _ij3[1] = iij3; break;
8727 }
8728 }
8729 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8730 {
8731 IkReal evalcond[8];
8732 IkReal x680=IKcos(j3);
8733 IkReal x681=IKsin(j3);
8734 IkReal x682=((1.0)*x680);
8735 evalcond[0]=(x680+new_r00);
8736 evalcond[1]=(x680+new_r11);
8737 evalcond[2]=(x681+new_r10);
8738 evalcond[3]=(new_r01+(((-1.0)*x681)));
8739 evalcond[4]=(((new_r01*x680))+((new_r11*x681)));
8740 evalcond[5]=((((-1.0)*new_r10*x682))+((new_r00*x681)));
8741 evalcond[6]=((1.0)+((new_r00*x680))+((new_r10*x681)));
8742 evalcond[7]=((-1.0)+((new_r01*x681))+(((-1.0)*new_r11*x682)));
8743 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8744 {
8745 continue;
8746 }
8747 }
8748 
8749 {
8750 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8751 vinfos[0].jointtype = 1;
8752 vinfos[0].foffset = j0;
8753 vinfos[0].indices[0] = _ij0[0];
8754 vinfos[0].indices[1] = _ij0[1];
8755 vinfos[0].maxsolutions = _nj0;
8756 vinfos[1].jointtype = 1;
8757 vinfos[1].foffset = j1;
8758 vinfos[1].indices[0] = _ij1[0];
8759 vinfos[1].indices[1] = _ij1[1];
8760 vinfos[1].maxsolutions = _nj1;
8761 vinfos[2].jointtype = 1;
8762 vinfos[2].foffset = j2;
8763 vinfos[2].indices[0] = _ij2[0];
8764 vinfos[2].indices[1] = _ij2[1];
8765 vinfos[2].maxsolutions = _nj2;
8766 vinfos[3].jointtype = 1;
8767 vinfos[3].foffset = j3;
8768 vinfos[3].indices[0] = _ij3[0];
8769 vinfos[3].indices[1] = _ij3[1];
8770 vinfos[3].maxsolutions = _nj3;
8771 vinfos[4].jointtype = 1;
8772 vinfos[4].foffset = j4;
8773 vinfos[4].indices[0] = _ij4[0];
8774 vinfos[4].indices[1] = _ij4[1];
8775 vinfos[4].maxsolutions = _nj4;
8776 vinfos[5].jointtype = 1;
8777 vinfos[5].foffset = j5;
8778 vinfos[5].indices[0] = _ij5[0];
8779 vinfos[5].indices[1] = _ij5[1];
8780 vinfos[5].maxsolutions = _nj5;
8781 std::vector<int> vfree(0);
8782 solutions.AddSolution(vinfos,vfree);
8783 }
8784 }
8785 }
8786 
8787 }
8788 } while(0);
8789 if( bgotonextstatement )
8790 {
8791 bool bgotonextstatement = true;
8792 do
8793 {
8794 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
8795 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8796 {
8797 bgotonextstatement=false;
8798 {
8799 IkReal j3eval[3];
8800 sj4=0;
8801 cj4=1.0;
8802 j4=0;
8803 new_r11=0;
8804 new_r00=0;
8805 j3eval[0]=new_r01;
8806 j3eval[1]=IKsign(new_r01);
8807 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
8808 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
8809 {
8810 {
8811 IkReal j3eval[2];
8812 sj4=0;
8813 cj4=1.0;
8814 j4=0;
8815 new_r11=0;
8816 new_r00=0;
8817 j3eval[0]=new_r01;
8818 j3eval[1]=new_r10;
8819 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8820 {
8821 {
8822 IkReal j3eval[2];
8823 sj4=0;
8824 cj4=1.0;
8825 j4=0;
8826 new_r11=0;
8827 new_r00=0;
8828 j3eval[0]=new_r01;
8829 j3eval[1]=sj5;
8830 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8831 {
8832 {
8833 IkReal evalcond[1];
8834 bool bgotonextstatement = true;
8835 do
8836 {
8837 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8838 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8839 {
8840 bgotonextstatement=false;
8841 {
8842 IkReal j3array[2], cj3array[2], sj3array[2];
8843 bool j3valid[2]={false};
8844 _nj3 = 2;
8845 sj3array[0]=new_r10;
8846 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8847 {
8848  j3valid[0] = j3valid[1] = true;
8849  j3array[0] = IKasin(sj3array[0]);
8850  cj3array[0] = IKcos(j3array[0]);
8851  sj3array[1] = sj3array[0];
8852  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8853  cj3array[1] = -cj3array[0];
8854 }
8855 else if( isnan(sj3array[0]) )
8856 {
8857  // probably any value will work
8858  j3valid[0] = true;
8859  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8860 }
8861 for(int ij3 = 0; ij3 < 2; ++ij3)
8862 {
8863 if( !j3valid[ij3] )
8864 {
8865  continue;
8866 }
8867 _ij3[0] = ij3; _ij3[1] = -1;
8868 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8869 {
8870 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8871 {
8872  j3valid[iij3]=false; _ij3[1] = iij3; break;
8873 }
8874 }
8875 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8876 {
8877 IkReal evalcond[6];
8878 IkReal x683=IKcos(j3);
8879 IkReal x684=IKsin(j3);
8880 IkReal x685=((-1.0)*x683);
8881 evalcond[0]=(new_r01*x683);
8882 evalcond[1]=(x684+new_r01);
8883 evalcond[2]=x685;
8884 evalcond[3]=(new_r10*x685);
8885 evalcond[4]=((1.0)+((new_r01*x684)));
8886 evalcond[5]=((-1.0)+((new_r10*x684)));
8887 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8888 {
8889 continue;
8890 }
8891 }
8892 
8893 {
8894 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8895 vinfos[0].jointtype = 1;
8896 vinfos[0].foffset = j0;
8897 vinfos[0].indices[0] = _ij0[0];
8898 vinfos[0].indices[1] = _ij0[1];
8899 vinfos[0].maxsolutions = _nj0;
8900 vinfos[1].jointtype = 1;
8901 vinfos[1].foffset = j1;
8902 vinfos[1].indices[0] = _ij1[0];
8903 vinfos[1].indices[1] = _ij1[1];
8904 vinfos[1].maxsolutions = _nj1;
8905 vinfos[2].jointtype = 1;
8906 vinfos[2].foffset = j2;
8907 vinfos[2].indices[0] = _ij2[0];
8908 vinfos[2].indices[1] = _ij2[1];
8909 vinfos[2].maxsolutions = _nj2;
8910 vinfos[3].jointtype = 1;
8911 vinfos[3].foffset = j3;
8912 vinfos[3].indices[0] = _ij3[0];
8913 vinfos[3].indices[1] = _ij3[1];
8914 vinfos[3].maxsolutions = _nj3;
8915 vinfos[4].jointtype = 1;
8916 vinfos[4].foffset = j4;
8917 vinfos[4].indices[0] = _ij4[0];
8918 vinfos[4].indices[1] = _ij4[1];
8919 vinfos[4].maxsolutions = _nj4;
8920 vinfos[5].jointtype = 1;
8921 vinfos[5].foffset = j5;
8922 vinfos[5].indices[0] = _ij5[0];
8923 vinfos[5].indices[1] = _ij5[1];
8924 vinfos[5].maxsolutions = _nj5;
8925 std::vector<int> vfree(0);
8926 solutions.AddSolution(vinfos,vfree);
8927 }
8928 }
8929 }
8930 
8931 }
8932 } while(0);
8933 if( bgotonextstatement )
8934 {
8935 bool bgotonextstatement = true;
8936 do
8937 {
8938 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8939 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8940 {
8941 bgotonextstatement=false;
8942 {
8943 IkReal j3array[2], cj3array[2], sj3array[2];
8944 bool j3valid[2]={false};
8945 _nj3 = 2;
8946 sj3array[0]=new_r01;
8947 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8948 {
8949  j3valid[0] = j3valid[1] = true;
8950  j3array[0] = IKasin(sj3array[0]);
8951  cj3array[0] = IKcos(j3array[0]);
8952  sj3array[1] = sj3array[0];
8953  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8954  cj3array[1] = -cj3array[0];
8955 }
8956 else if( isnan(sj3array[0]) )
8957 {
8958  // probably any value will work
8959  j3valid[0] = true;
8960  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8961 }
8962 for(int ij3 = 0; ij3 < 2; ++ij3)
8963 {
8964 if( !j3valid[ij3] )
8965 {
8966  continue;
8967 }
8968 _ij3[0] = ij3; _ij3[1] = -1;
8969 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8970 {
8971 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8972 {
8973  j3valid[iij3]=false; _ij3[1] = iij3; break;
8974 }
8975 }
8976 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8977 {
8978 IkReal evalcond[6];
8979 IkReal x686=IKcos(j3);
8980 IkReal x687=IKsin(j3);
8981 evalcond[0]=x686;
8982 evalcond[1]=(new_r01*x686);
8983 evalcond[2]=(x687+new_r10);
8984 evalcond[3]=((-1.0)*new_r10*x686);
8985 evalcond[4]=((-1.0)+((new_r01*x687)));
8986 evalcond[5]=((1.0)+((new_r10*x687)));
8987 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8988 {
8989 continue;
8990 }
8991 }
8992 
8993 {
8994 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8995 vinfos[0].jointtype = 1;
8996 vinfos[0].foffset = j0;
8997 vinfos[0].indices[0] = _ij0[0];
8998 vinfos[0].indices[1] = _ij0[1];
8999 vinfos[0].maxsolutions = _nj0;
9000 vinfos[1].jointtype = 1;
9001 vinfos[1].foffset = j1;
9002 vinfos[1].indices[0] = _ij1[0];
9003 vinfos[1].indices[1] = _ij1[1];
9004 vinfos[1].maxsolutions = _nj1;
9005 vinfos[2].jointtype = 1;
9006 vinfos[2].foffset = j2;
9007 vinfos[2].indices[0] = _ij2[0];
9008 vinfos[2].indices[1] = _ij2[1];
9009 vinfos[2].maxsolutions = _nj2;
9010 vinfos[3].jointtype = 1;
9011 vinfos[3].foffset = j3;
9012 vinfos[3].indices[0] = _ij3[0];
9013 vinfos[3].indices[1] = _ij3[1];
9014 vinfos[3].maxsolutions = _nj3;
9015 vinfos[4].jointtype = 1;
9016 vinfos[4].foffset = j4;
9017 vinfos[4].indices[0] = _ij4[0];
9018 vinfos[4].indices[1] = _ij4[1];
9019 vinfos[4].maxsolutions = _nj4;
9020 vinfos[5].jointtype = 1;
9021 vinfos[5].foffset = j5;
9022 vinfos[5].indices[0] = _ij5[0];
9023 vinfos[5].indices[1] = _ij5[1];
9024 vinfos[5].maxsolutions = _nj5;
9025 std::vector<int> vfree(0);
9026 solutions.AddSolution(vinfos,vfree);
9027 }
9028 }
9029 }
9030 
9031 }
9032 } while(0);
9033 if( bgotonextstatement )
9034 {
9035 bool bgotonextstatement = true;
9036 do
9037 {
9038 if( 1 )
9039 {
9040 bgotonextstatement=false;
9041 continue; // branch miss [j3]
9042 
9043 }
9044 } while(0);
9045 if( bgotonextstatement )
9046 {
9047 }
9048 }
9049 }
9050 }
9051 
9052 } else
9053 {
9054 {
9055 IkReal j3array[1], cj3array[1], sj3array[1];
9056 bool j3valid[1]={false};
9057 _nj3 = 1;
9058 CheckValue<IkReal> x689=IKPowWithIntegerCheck(new_r01,-1);
9059 if(!x689.valid){
9060 continue;
9061 }
9062 IkReal x688=x689.value;
9063 CheckValue<IkReal> x690=IKPowWithIntegerCheck(sj5,-1);
9064 if(!x690.valid){
9065 continue;
9066 }
9067 CheckValue<IkReal> x691=IKPowWithIntegerCheck(x688,-2);
9068 if(!x691.valid){
9069 continue;
9070 }
9071 if( IKabs(((-1.0)*cj5*x688)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x688*(x690.value)*(((cj5*cj5)+(((-1.0)*(x691.value))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*x688))+IKsqr((x688*(x690.value)*(((cj5*cj5)+(((-1.0)*(x691.value)))))))-1) <= IKFAST_SINCOS_THRESH )
9072  continue;
9073 j3array[0]=IKatan2(((-1.0)*cj5*x688), (x688*(x690.value)*(((cj5*cj5)+(((-1.0)*(x691.value)))))));
9074 sj3array[0]=IKsin(j3array[0]);
9075 cj3array[0]=IKcos(j3array[0]);
9076 if( j3array[0] > IKPI )
9077 {
9078  j3array[0]-=IK2PI;
9079 }
9080 else if( j3array[0] < -IKPI )
9081 { j3array[0]+=IK2PI;
9082 }
9083 j3valid[0] = true;
9084 for(int ij3 = 0; ij3 < 1; ++ij3)
9085 {
9086 if( !j3valid[ij3] )
9087 {
9088  continue;
9089 }
9090 _ij3[0] = ij3; _ij3[1] = -1;
9091 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9092 {
9093 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9094 {
9095  j3valid[iij3]=false; _ij3[1] = iij3; break;
9096 }
9097 }
9098 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9099 {
9100 IkReal evalcond[7];
9101 IkReal x692=IKcos(j3);
9102 IkReal x693=IKsin(j3);
9103 IkReal x694=((1.0)*cj5);
9104 IkReal x695=(sj5*x692);
9105 evalcond[0]=(cj5+((new_r01*x693)));
9106 evalcond[1]=(sj5+((new_r01*x692)));
9107 evalcond[2]=(sj5+(((-1.0)*new_r10*x692)));
9108 evalcond[3]=(((new_r10*x693))+(((-1.0)*x694)));
9109 evalcond[4]=(x695+new_r01+((cj5*x693)));
9110 evalcond[5]=((((-1.0)*x692*x694))+((sj5*x693)));
9111 evalcond[6]=((((-1.0)*x693*x694))+new_r10+(((-1.0)*x695)));
9112 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9113 {
9114 continue;
9115 }
9116 }
9117 
9118 {
9119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9120 vinfos[0].jointtype = 1;
9121 vinfos[0].foffset = j0;
9122 vinfos[0].indices[0] = _ij0[0];
9123 vinfos[0].indices[1] = _ij0[1];
9124 vinfos[0].maxsolutions = _nj0;
9125 vinfos[1].jointtype = 1;
9126 vinfos[1].foffset = j1;
9127 vinfos[1].indices[0] = _ij1[0];
9128 vinfos[1].indices[1] = _ij1[1];
9129 vinfos[1].maxsolutions = _nj1;
9130 vinfos[2].jointtype = 1;
9131 vinfos[2].foffset = j2;
9132 vinfos[2].indices[0] = _ij2[0];
9133 vinfos[2].indices[1] = _ij2[1];
9134 vinfos[2].maxsolutions = _nj2;
9135 vinfos[3].jointtype = 1;
9136 vinfos[3].foffset = j3;
9137 vinfos[3].indices[0] = _ij3[0];
9138 vinfos[3].indices[1] = _ij3[1];
9139 vinfos[3].maxsolutions = _nj3;
9140 vinfos[4].jointtype = 1;
9141 vinfos[4].foffset = j4;
9142 vinfos[4].indices[0] = _ij4[0];
9143 vinfos[4].indices[1] = _ij4[1];
9144 vinfos[4].maxsolutions = _nj4;
9145 vinfos[5].jointtype = 1;
9146 vinfos[5].foffset = j5;
9147 vinfos[5].indices[0] = _ij5[0];
9148 vinfos[5].indices[1] = _ij5[1];
9149 vinfos[5].maxsolutions = _nj5;
9150 std::vector<int> vfree(0);
9151 solutions.AddSolution(vinfos,vfree);
9152 }
9153 }
9154 }
9155 
9156 }
9157 
9158 }
9159 
9160 } else
9161 {
9162 {
9163 IkReal j3array[1], cj3array[1], sj3array[1];
9164 bool j3valid[1]={false};
9165 _nj3 = 1;
9166 CheckValue<IkReal> x696=IKPowWithIntegerCheck(new_r01,-1);
9167 if(!x696.valid){
9168 continue;
9169 }
9170 CheckValue<IkReal> x697=IKPowWithIntegerCheck(new_r10,-1);
9171 if(!x697.valid){
9172 continue;
9173 }
9174 if( IKabs(((-1.0)*cj5*(x696.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x697.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x696.value)))+IKsqr((sj5*(x697.value)))-1) <= IKFAST_SINCOS_THRESH )
9175  continue;
9176 j3array[0]=IKatan2(((-1.0)*cj5*(x696.value)), (sj5*(x697.value)));
9177 sj3array[0]=IKsin(j3array[0]);
9178 cj3array[0]=IKcos(j3array[0]);
9179 if( j3array[0] > IKPI )
9180 {
9181  j3array[0]-=IK2PI;
9182 }
9183 else if( j3array[0] < -IKPI )
9184 { j3array[0]+=IK2PI;
9185 }
9186 j3valid[0] = true;
9187 for(int ij3 = 0; ij3 < 1; ++ij3)
9188 {
9189 if( !j3valid[ij3] )
9190 {
9191  continue;
9192 }
9193 _ij3[0] = ij3; _ij3[1] = -1;
9194 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9195 {
9196 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9197 {
9198  j3valid[iij3]=false; _ij3[1] = iij3; break;
9199 }
9200 }
9201 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9202 {
9203 IkReal evalcond[7];
9204 IkReal x698=IKcos(j3);
9205 IkReal x699=IKsin(j3);
9206 IkReal x700=((1.0)*cj5);
9207 IkReal x701=(sj5*x698);
9208 evalcond[0]=(cj5+((new_r01*x699)));
9209 evalcond[1]=(sj5+((new_r01*x698)));
9210 evalcond[2]=(sj5+(((-1.0)*new_r10*x698)));
9211 evalcond[3]=(((new_r10*x699))+(((-1.0)*x700)));
9212 evalcond[4]=(x701+new_r01+((cj5*x699)));
9213 evalcond[5]=(((sj5*x699))+(((-1.0)*x698*x700)));
9214 evalcond[6]=((((-1.0)*x701))+(((-1.0)*x699*x700))+new_r10);
9215 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9216 {
9217 continue;
9218 }
9219 }
9220 
9221 {
9222 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9223 vinfos[0].jointtype = 1;
9224 vinfos[0].foffset = j0;
9225 vinfos[0].indices[0] = _ij0[0];
9226 vinfos[0].indices[1] = _ij0[1];
9227 vinfos[0].maxsolutions = _nj0;
9228 vinfos[1].jointtype = 1;
9229 vinfos[1].foffset = j1;
9230 vinfos[1].indices[0] = _ij1[0];
9231 vinfos[1].indices[1] = _ij1[1];
9232 vinfos[1].maxsolutions = _nj1;
9233 vinfos[2].jointtype = 1;
9234 vinfos[2].foffset = j2;
9235 vinfos[2].indices[0] = _ij2[0];
9236 vinfos[2].indices[1] = _ij2[1];
9237 vinfos[2].maxsolutions = _nj2;
9238 vinfos[3].jointtype = 1;
9239 vinfos[3].foffset = j3;
9240 vinfos[3].indices[0] = _ij3[0];
9241 vinfos[3].indices[1] = _ij3[1];
9242 vinfos[3].maxsolutions = _nj3;
9243 vinfos[4].jointtype = 1;
9244 vinfos[4].foffset = j4;
9245 vinfos[4].indices[0] = _ij4[0];
9246 vinfos[4].indices[1] = _ij4[1];
9247 vinfos[4].maxsolutions = _nj4;
9248 vinfos[5].jointtype = 1;
9249 vinfos[5].foffset = j5;
9250 vinfos[5].indices[0] = _ij5[0];
9251 vinfos[5].indices[1] = _ij5[1];
9252 vinfos[5].maxsolutions = _nj5;
9253 std::vector<int> vfree(0);
9254 solutions.AddSolution(vinfos,vfree);
9255 }
9256 }
9257 }
9258 
9259 }
9260 
9261 }
9262 
9263 } else
9264 {
9265 {
9266 IkReal j3array[1], cj3array[1], sj3array[1];
9267 bool j3valid[1]={false};
9268 _nj3 = 1;
9269 CheckValue<IkReal> x702=IKPowWithIntegerCheck(IKsign(new_r01),-1);
9270 if(!x702.valid){
9271 continue;
9272 }
9273 CheckValue<IkReal> x703 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(((-1.0)*sj5)),IKFAST_ATAN2_MAGTHRESH);
9274 if(!x703.valid){
9275 continue;
9276 }
9277 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x702.value)))+(x703.value));
9278 sj3array[0]=IKsin(j3array[0]);
9279 cj3array[0]=IKcos(j3array[0]);
9280 if( j3array[0] > IKPI )
9281 {
9282  j3array[0]-=IK2PI;
9283 }
9284 else if( j3array[0] < -IKPI )
9285 { j3array[0]+=IK2PI;
9286 }
9287 j3valid[0] = true;
9288 for(int ij3 = 0; ij3 < 1; ++ij3)
9289 {
9290 if( !j3valid[ij3] )
9291 {
9292  continue;
9293 }
9294 _ij3[0] = ij3; _ij3[1] = -1;
9295 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9296 {
9297 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9298 {
9299  j3valid[iij3]=false; _ij3[1] = iij3; break;
9300 }
9301 }
9302 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9303 {
9304 IkReal evalcond[7];
9305 IkReal x704=IKcos(j3);
9306 IkReal x705=IKsin(j3);
9307 IkReal x706=((1.0)*cj5);
9308 IkReal x707=(sj5*x704);
9309 evalcond[0]=(cj5+((new_r01*x705)));
9310 evalcond[1]=(sj5+((new_r01*x704)));
9311 evalcond[2]=(sj5+(((-1.0)*new_r10*x704)));
9312 evalcond[3]=((((-1.0)*x706))+((new_r10*x705)));
9313 evalcond[4]=(((cj5*x705))+x707+new_r01);
9314 evalcond[5]=(((sj5*x705))+(((-1.0)*x704*x706)));
9315 evalcond[6]=((((-1.0)*x707))+(((-1.0)*x705*x706))+new_r10);
9316 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9317 {
9318 continue;
9319 }
9320 }
9321 
9322 {
9323 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9324 vinfos[0].jointtype = 1;
9325 vinfos[0].foffset = j0;
9326 vinfos[0].indices[0] = _ij0[0];
9327 vinfos[0].indices[1] = _ij0[1];
9328 vinfos[0].maxsolutions = _nj0;
9329 vinfos[1].jointtype = 1;
9330 vinfos[1].foffset = j1;
9331 vinfos[1].indices[0] = _ij1[0];
9332 vinfos[1].indices[1] = _ij1[1];
9333 vinfos[1].maxsolutions = _nj1;
9334 vinfos[2].jointtype = 1;
9335 vinfos[2].foffset = j2;
9336 vinfos[2].indices[0] = _ij2[0];
9337 vinfos[2].indices[1] = _ij2[1];
9338 vinfos[2].maxsolutions = _nj2;
9339 vinfos[3].jointtype = 1;
9340 vinfos[3].foffset = j3;
9341 vinfos[3].indices[0] = _ij3[0];
9342 vinfos[3].indices[1] = _ij3[1];
9343 vinfos[3].maxsolutions = _nj3;
9344 vinfos[4].jointtype = 1;
9345 vinfos[4].foffset = j4;
9346 vinfos[4].indices[0] = _ij4[0];
9347 vinfos[4].indices[1] = _ij4[1];
9348 vinfos[4].maxsolutions = _nj4;
9349 vinfos[5].jointtype = 1;
9350 vinfos[5].foffset = j5;
9351 vinfos[5].indices[0] = _ij5[0];
9352 vinfos[5].indices[1] = _ij5[1];
9353 vinfos[5].maxsolutions = _nj5;
9354 std::vector<int> vfree(0);
9355 solutions.AddSolution(vinfos,vfree);
9356 }
9357 }
9358 }
9359 
9360 }
9361 
9362 }
9363 
9364 }
9365 } while(0);
9366 if( bgotonextstatement )
9367 {
9368 bool bgotonextstatement = true;
9369 do
9370 {
9371 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9372 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9373 {
9374 bgotonextstatement=false;
9375 {
9376 IkReal j3eval[1];
9377 sj4=0;
9378 cj4=1.0;
9379 j4=0;
9380 new_r11=0;
9381 new_r01=0;
9382 new_r22=0;
9383 new_r20=0;
9384 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9385 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9386 {
9387 continue; // no branches [j3]
9388 
9389 } else
9390 {
9391 {
9392 IkReal j3array[2], cj3array[2], sj3array[2];
9393 bool j3valid[2]={false};
9394 _nj3 = 2;
9395 CheckValue<IkReal> x709 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9396 if(!x709.valid){
9397 continue;
9398 }
9399 IkReal x708=x709.value;
9400 j3array[0]=((-1.0)*x708);
9401 sj3array[0]=IKsin(j3array[0]);
9402 cj3array[0]=IKcos(j3array[0]);
9403 j3array[1]=((3.14159265358979)+(((-1.0)*x708)));
9404 sj3array[1]=IKsin(j3array[1]);
9405 cj3array[1]=IKcos(j3array[1]);
9406 if( j3array[0] > IKPI )
9407 {
9408  j3array[0]-=IK2PI;
9409 }
9410 else if( j3array[0] < -IKPI )
9411 { j3array[0]+=IK2PI;
9412 }
9413 j3valid[0] = true;
9414 if( j3array[1] > IKPI )
9415 {
9416  j3array[1]-=IK2PI;
9417 }
9418 else if( j3array[1] < -IKPI )
9419 { j3array[1]+=IK2PI;
9420 }
9421 j3valid[1] = true;
9422 for(int ij3 = 0; ij3 < 2; ++ij3)
9423 {
9424 if( !j3valid[ij3] )
9425 {
9426  continue;
9427 }
9428 _ij3[0] = ij3; _ij3[1] = -1;
9429 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9430 {
9431 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9432 {
9433  j3valid[iij3]=false; _ij3[1] = iij3; break;
9434 }
9435 }
9436 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9437 {
9438 IkReal evalcond[1];
9439 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
9440 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9441 {
9442 continue;
9443 }
9444 }
9445 
9446 {
9447 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9448 vinfos[0].jointtype = 1;
9449 vinfos[0].foffset = j0;
9450 vinfos[0].indices[0] = _ij0[0];
9451 vinfos[0].indices[1] = _ij0[1];
9452 vinfos[0].maxsolutions = _nj0;
9453 vinfos[1].jointtype = 1;
9454 vinfos[1].foffset = j1;
9455 vinfos[1].indices[0] = _ij1[0];
9456 vinfos[1].indices[1] = _ij1[1];
9457 vinfos[1].maxsolutions = _nj1;
9458 vinfos[2].jointtype = 1;
9459 vinfos[2].foffset = j2;
9460 vinfos[2].indices[0] = _ij2[0];
9461 vinfos[2].indices[1] = _ij2[1];
9462 vinfos[2].maxsolutions = _nj2;
9463 vinfos[3].jointtype = 1;
9464 vinfos[3].foffset = j3;
9465 vinfos[3].indices[0] = _ij3[0];
9466 vinfos[3].indices[1] = _ij3[1];
9467 vinfos[3].maxsolutions = _nj3;
9468 vinfos[4].jointtype = 1;
9469 vinfos[4].foffset = j4;
9470 vinfos[4].indices[0] = _ij4[0];
9471 vinfos[4].indices[1] = _ij4[1];
9472 vinfos[4].maxsolutions = _nj4;
9473 vinfos[5].jointtype = 1;
9474 vinfos[5].foffset = j5;
9475 vinfos[5].indices[0] = _ij5[0];
9476 vinfos[5].indices[1] = _ij5[1];
9477 vinfos[5].maxsolutions = _nj5;
9478 std::vector<int> vfree(0);
9479 solutions.AddSolution(vinfos,vfree);
9480 }
9481 }
9482 }
9483 
9484 }
9485 
9486 }
9487 
9488 }
9489 } while(0);
9490 if( bgotonextstatement )
9491 {
9492 bool bgotonextstatement = true;
9493 do
9494 {
9495 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9496 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9497 {
9498 bgotonextstatement=false;
9499 {
9500 IkReal j3eval[1];
9501 sj4=0;
9502 cj4=1.0;
9503 j4=0;
9504 new_r00=0;
9505 new_r10=0;
9506 new_r21=0;
9507 new_r22=0;
9508 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9509 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9510 {
9511 continue; // no branches [j3]
9512 
9513 } else
9514 {
9515 {
9516 IkReal j3array[2], cj3array[2], sj3array[2];
9517 bool j3valid[2]={false};
9518 _nj3 = 2;
9519 CheckValue<IkReal> x711 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9520 if(!x711.valid){
9521 continue;
9522 }
9523 IkReal x710=x711.value;
9524 j3array[0]=((-1.0)*x710);
9525 sj3array[0]=IKsin(j3array[0]);
9526 cj3array[0]=IKcos(j3array[0]);
9527 j3array[1]=((3.14159265358979)+(((-1.0)*x710)));
9528 sj3array[1]=IKsin(j3array[1]);
9529 cj3array[1]=IKcos(j3array[1]);
9530 if( j3array[0] > IKPI )
9531 {
9532  j3array[0]-=IK2PI;
9533 }
9534 else if( j3array[0] < -IKPI )
9535 { j3array[0]+=IK2PI;
9536 }
9537 j3valid[0] = true;
9538 if( j3array[1] > IKPI )
9539 {
9540  j3array[1]-=IK2PI;
9541 }
9542 else if( j3array[1] < -IKPI )
9543 { j3array[1]+=IK2PI;
9544 }
9545 j3valid[1] = true;
9546 for(int ij3 = 0; ij3 < 2; ++ij3)
9547 {
9548 if( !j3valid[ij3] )
9549 {
9550  continue;
9551 }
9552 _ij3[0] = ij3; _ij3[1] = -1;
9553 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9554 {
9555 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9556 {
9557  j3valid[iij3]=false; _ij3[1] = iij3; break;
9558 }
9559 }
9560 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9561 {
9562 IkReal evalcond[1];
9563 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
9564 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9565 {
9566 continue;
9567 }
9568 }
9569 
9570 {
9571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9572 vinfos[0].jointtype = 1;
9573 vinfos[0].foffset = j0;
9574 vinfos[0].indices[0] = _ij0[0];
9575 vinfos[0].indices[1] = _ij0[1];
9576 vinfos[0].maxsolutions = _nj0;
9577 vinfos[1].jointtype = 1;
9578 vinfos[1].foffset = j1;
9579 vinfos[1].indices[0] = _ij1[0];
9580 vinfos[1].indices[1] = _ij1[1];
9581 vinfos[1].maxsolutions = _nj1;
9582 vinfos[2].jointtype = 1;
9583 vinfos[2].foffset = j2;
9584 vinfos[2].indices[0] = _ij2[0];
9585 vinfos[2].indices[1] = _ij2[1];
9586 vinfos[2].maxsolutions = _nj2;
9587 vinfos[3].jointtype = 1;
9588 vinfos[3].foffset = j3;
9589 vinfos[3].indices[0] = _ij3[0];
9590 vinfos[3].indices[1] = _ij3[1];
9591 vinfos[3].maxsolutions = _nj3;
9592 vinfos[4].jointtype = 1;
9593 vinfos[4].foffset = j4;
9594 vinfos[4].indices[0] = _ij4[0];
9595 vinfos[4].indices[1] = _ij4[1];
9596 vinfos[4].maxsolutions = _nj4;
9597 vinfos[5].jointtype = 1;
9598 vinfos[5].foffset = j5;
9599 vinfos[5].indices[0] = _ij5[0];
9600 vinfos[5].indices[1] = _ij5[1];
9601 vinfos[5].maxsolutions = _nj5;
9602 std::vector<int> vfree(0);
9603 solutions.AddSolution(vinfos,vfree);
9604 }
9605 }
9606 }
9607 
9608 }
9609 
9610 }
9611 
9612 }
9613 } while(0);
9614 if( bgotonextstatement )
9615 {
9616 bool bgotonextstatement = true;
9617 do
9618 {
9619 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
9620 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9621 {
9622 bgotonextstatement=false;
9623 {
9624 IkReal j3eval[3];
9625 sj4=0;
9626 cj4=1.0;
9627 j4=0;
9628 new_r01=0;
9629 new_r10=0;
9630 j3eval[0]=new_r11;
9631 j3eval[1]=IKsign(new_r11);
9632 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
9633 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9634 {
9635 {
9636 IkReal j3eval[3];
9637 sj4=0;
9638 cj4=1.0;
9639 j4=0;
9640 new_r01=0;
9641 new_r10=0;
9642 j3eval[0]=new_r00;
9643 j3eval[1]=((IKabs(cj5))+(IKabs(sj5)));
9644 j3eval[2]=IKsign(new_r00);
9645 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9646 {
9647 {
9648 IkReal j3eval[2];
9649 sj4=0;
9650 cj4=1.0;
9651 j4=0;
9652 new_r01=0;
9653 new_r10=0;
9654 j3eval[0]=new_r00;
9655 j3eval[1]=new_r11;
9656 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9657 {
9658 continue; // no branches [j3]
9659 
9660 } else
9661 {
9662 {
9663 IkReal j3array[1], cj3array[1], sj3array[1];
9664 bool j3valid[1]={false};
9665 _nj3 = 1;
9666 CheckValue<IkReal> x712=IKPowWithIntegerCheck(new_r00,-1);
9667 if(!x712.valid){
9668 continue;
9669 }
9670 CheckValue<IkReal> x713=IKPowWithIntegerCheck(new_r11,-1);
9671 if(!x713.valid){
9672 continue;
9673 }
9674 if( IKabs(((-1.0)*sj5*(x712.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x713.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x712.value)))+IKsqr((cj5*(x713.value)))-1) <= IKFAST_SINCOS_THRESH )
9675  continue;
9676 j3array[0]=IKatan2(((-1.0)*sj5*(x712.value)), (cj5*(x713.value)));
9677 sj3array[0]=IKsin(j3array[0]);
9678 cj3array[0]=IKcos(j3array[0]);
9679 if( j3array[0] > IKPI )
9680 {
9681  j3array[0]-=IK2PI;
9682 }
9683 else if( j3array[0] < -IKPI )
9684 { j3array[0]+=IK2PI;
9685 }
9686 j3valid[0] = true;
9687 for(int ij3 = 0; ij3 < 1; ++ij3)
9688 {
9689 if( !j3valid[ij3] )
9690 {
9691  continue;
9692 }
9693 _ij3[0] = ij3; _ij3[1] = -1;
9694 for(int iij3 = ij3+1; iij3 < 1; ++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[7];
9704 IkReal x714=IKsin(j3);
9705 IkReal x715=IKcos(j3);
9706 IkReal x716=(sj5*x714);
9707 IkReal x717=((1.0)*x715);
9708 IkReal x718=(cj5*x717);
9709 evalcond[0]=(sj5+((new_r00*x714)));
9710 evalcond[1]=(sj5+((new_r11*x714)));
9711 evalcond[2]=(cj5+(((-1.0)*new_r11*x717)));
9712 evalcond[3]=(((new_r00*x715))+(((-1.0)*cj5)));
9713 evalcond[4]=(((cj5*x714))+((sj5*x715)));
9714 evalcond[5]=((((-1.0)*x718))+x716+new_r00);
9715 evalcond[6]=((((-1.0)*x718))+x716+new_r11);
9716 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9717 {
9718 continue;
9719 }
9720 }
9721 
9722 {
9723 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9724 vinfos[0].jointtype = 1;
9725 vinfos[0].foffset = j0;
9726 vinfos[0].indices[0] = _ij0[0];
9727 vinfos[0].indices[1] = _ij0[1];
9728 vinfos[0].maxsolutions = _nj0;
9729 vinfos[1].jointtype = 1;
9730 vinfos[1].foffset = j1;
9731 vinfos[1].indices[0] = _ij1[0];
9732 vinfos[1].indices[1] = _ij1[1];
9733 vinfos[1].maxsolutions = _nj1;
9734 vinfos[2].jointtype = 1;
9735 vinfos[2].foffset = j2;
9736 vinfos[2].indices[0] = _ij2[0];
9737 vinfos[2].indices[1] = _ij2[1];
9738 vinfos[2].maxsolutions = _nj2;
9739 vinfos[3].jointtype = 1;
9740 vinfos[3].foffset = j3;
9741 vinfos[3].indices[0] = _ij3[0];
9742 vinfos[3].indices[1] = _ij3[1];
9743 vinfos[3].maxsolutions = _nj3;
9744 vinfos[4].jointtype = 1;
9745 vinfos[4].foffset = j4;
9746 vinfos[4].indices[0] = _ij4[0];
9747 vinfos[4].indices[1] = _ij4[1];
9748 vinfos[4].maxsolutions = _nj4;
9749 vinfos[5].jointtype = 1;
9750 vinfos[5].foffset = j5;
9751 vinfos[5].indices[0] = _ij5[0];
9752 vinfos[5].indices[1] = _ij5[1];
9753 vinfos[5].maxsolutions = _nj5;
9754 std::vector<int> vfree(0);
9755 solutions.AddSolution(vinfos,vfree);
9756 }
9757 }
9758 }
9759 
9760 }
9761 
9762 }
9763 
9764 } else
9765 {
9766 {
9767 IkReal j3array[1], cj3array[1], sj3array[1];
9768 bool j3valid[1]={false};
9769 _nj3 = 1;
9770 CheckValue<IkReal> x719=IKPowWithIntegerCheck(IKsign(new_r00),-1);
9771 if(!x719.valid){
9772 continue;
9773 }
9774 CheckValue<IkReal> x720 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9775 if(!x720.valid){
9776 continue;
9777 }
9778 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x719.value)))+(x720.value));
9779 sj3array[0]=IKsin(j3array[0]);
9780 cj3array[0]=IKcos(j3array[0]);
9781 if( j3array[0] > IKPI )
9782 {
9783  j3array[0]-=IK2PI;
9784 }
9785 else if( j3array[0] < -IKPI )
9786 { j3array[0]+=IK2PI;
9787 }
9788 j3valid[0] = true;
9789 for(int ij3 = 0; ij3 < 1; ++ij3)
9790 {
9791 if( !j3valid[ij3] )
9792 {
9793  continue;
9794 }
9795 _ij3[0] = ij3; _ij3[1] = -1;
9796 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9797 {
9798 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9799 {
9800  j3valid[iij3]=false; _ij3[1] = iij3; break;
9801 }
9802 }
9803 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9804 {
9805 IkReal evalcond[7];
9806 IkReal x721=IKsin(j3);
9807 IkReal x722=IKcos(j3);
9808 IkReal x723=(sj5*x721);
9809 IkReal x724=((1.0)*x722);
9810 IkReal x725=(cj5*x724);
9811 evalcond[0]=(sj5+((new_r00*x721)));
9812 evalcond[1]=(sj5+((new_r11*x721)));
9813 evalcond[2]=(cj5+(((-1.0)*new_r11*x724)));
9814 evalcond[3]=(((new_r00*x722))+(((-1.0)*cj5)));
9815 evalcond[4]=(((cj5*x721))+((sj5*x722)));
9816 evalcond[5]=((((-1.0)*x725))+x723+new_r00);
9817 evalcond[6]=((((-1.0)*x725))+x723+new_r11);
9818 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9819 {
9820 continue;
9821 }
9822 }
9823 
9824 {
9825 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9826 vinfos[0].jointtype = 1;
9827 vinfos[0].foffset = j0;
9828 vinfos[0].indices[0] = _ij0[0];
9829 vinfos[0].indices[1] = _ij0[1];
9830 vinfos[0].maxsolutions = _nj0;
9831 vinfos[1].jointtype = 1;
9832 vinfos[1].foffset = j1;
9833 vinfos[1].indices[0] = _ij1[0];
9834 vinfos[1].indices[1] = _ij1[1];
9835 vinfos[1].maxsolutions = _nj1;
9836 vinfos[2].jointtype = 1;
9837 vinfos[2].foffset = j2;
9838 vinfos[2].indices[0] = _ij2[0];
9839 vinfos[2].indices[1] = _ij2[1];
9840 vinfos[2].maxsolutions = _nj2;
9841 vinfos[3].jointtype = 1;
9842 vinfos[3].foffset = j3;
9843 vinfos[3].indices[0] = _ij3[0];
9844 vinfos[3].indices[1] = _ij3[1];
9845 vinfos[3].maxsolutions = _nj3;
9846 vinfos[4].jointtype = 1;
9847 vinfos[4].foffset = j4;
9848 vinfos[4].indices[0] = _ij4[0];
9849 vinfos[4].indices[1] = _ij4[1];
9850 vinfos[4].maxsolutions = _nj4;
9851 vinfos[5].jointtype = 1;
9852 vinfos[5].foffset = j5;
9853 vinfos[5].indices[0] = _ij5[0];
9854 vinfos[5].indices[1] = _ij5[1];
9855 vinfos[5].maxsolutions = _nj5;
9856 std::vector<int> vfree(0);
9857 solutions.AddSolution(vinfos,vfree);
9858 }
9859 }
9860 }
9861 
9862 }
9863 
9864 }
9865 
9866 } else
9867 {
9868 {
9869 IkReal j3array[1], cj3array[1], sj3array[1];
9870 bool j3valid[1]={false};
9871 _nj3 = 1;
9872 CheckValue<IkReal> x726=IKPowWithIntegerCheck(IKsign(new_r11),-1);
9873 if(!x726.valid){
9874 continue;
9875 }
9876 CheckValue<IkReal> x727 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9877 if(!x727.valid){
9878 continue;
9879 }
9880 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x726.value)))+(x727.value));
9881 sj3array[0]=IKsin(j3array[0]);
9882 cj3array[0]=IKcos(j3array[0]);
9883 if( j3array[0] > IKPI )
9884 {
9885  j3array[0]-=IK2PI;
9886 }
9887 else if( j3array[0] < -IKPI )
9888 { j3array[0]+=IK2PI;
9889 }
9890 j3valid[0] = true;
9891 for(int ij3 = 0; ij3 < 1; ++ij3)
9892 {
9893 if( !j3valid[ij3] )
9894 {
9895  continue;
9896 }
9897 _ij3[0] = ij3; _ij3[1] = -1;
9898 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9899 {
9900 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9901 {
9902  j3valid[iij3]=false; _ij3[1] = iij3; break;
9903 }
9904 }
9905 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9906 {
9907 IkReal evalcond[7];
9908 IkReal x728=IKsin(j3);
9909 IkReal x729=IKcos(j3);
9910 IkReal x730=(sj5*x728);
9911 IkReal x731=((1.0)*x729);
9912 IkReal x732=(cj5*x731);
9913 evalcond[0]=(sj5+((new_r00*x728)));
9914 evalcond[1]=(sj5+((new_r11*x728)));
9915 evalcond[2]=(cj5+(((-1.0)*new_r11*x731)));
9916 evalcond[3]=(((new_r00*x729))+(((-1.0)*cj5)));
9917 evalcond[4]=(((cj5*x728))+((sj5*x729)));
9918 evalcond[5]=((((-1.0)*x732))+x730+new_r00);
9919 evalcond[6]=((((-1.0)*x732))+x730+new_r11);
9920 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9921 {
9922 continue;
9923 }
9924 }
9925 
9926 {
9927 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9928 vinfos[0].jointtype = 1;
9929 vinfos[0].foffset = j0;
9930 vinfos[0].indices[0] = _ij0[0];
9931 vinfos[0].indices[1] = _ij0[1];
9932 vinfos[0].maxsolutions = _nj0;
9933 vinfos[1].jointtype = 1;
9934 vinfos[1].foffset = j1;
9935 vinfos[1].indices[0] = _ij1[0];
9936 vinfos[1].indices[1] = _ij1[1];
9937 vinfos[1].maxsolutions = _nj1;
9938 vinfos[2].jointtype = 1;
9939 vinfos[2].foffset = j2;
9940 vinfos[2].indices[0] = _ij2[0];
9941 vinfos[2].indices[1] = _ij2[1];
9942 vinfos[2].maxsolutions = _nj2;
9943 vinfos[3].jointtype = 1;
9944 vinfos[3].foffset = j3;
9945 vinfos[3].indices[0] = _ij3[0];
9946 vinfos[3].indices[1] = _ij3[1];
9947 vinfos[3].maxsolutions = _nj3;
9948 vinfos[4].jointtype = 1;
9949 vinfos[4].foffset = j4;
9950 vinfos[4].indices[0] = _ij4[0];
9951 vinfos[4].indices[1] = _ij4[1];
9952 vinfos[4].maxsolutions = _nj4;
9953 vinfos[5].jointtype = 1;
9954 vinfos[5].foffset = j5;
9955 vinfos[5].indices[0] = _ij5[0];
9956 vinfos[5].indices[1] = _ij5[1];
9957 vinfos[5].maxsolutions = _nj5;
9958 std::vector<int> vfree(0);
9959 solutions.AddSolution(vinfos,vfree);
9960 }
9961 }
9962 }
9963 
9964 }
9965 
9966 }
9967 
9968 }
9969 } while(0);
9970 if( bgotonextstatement )
9971 {
9972 bool bgotonextstatement = true;
9973 do
9974 {
9975 if( 1 )
9976 {
9977 bgotonextstatement=false;
9978 continue; // branch miss [j3]
9979 
9980 }
9981 } while(0);
9982 if( bgotonextstatement )
9983 {
9984 }
9985 }
9986 }
9987 }
9988 }
9989 }
9990 }
9991 }
9992 }
9993 }
9994 
9995 } else
9996 {
9997 {
9998 IkReal j3array[1], cj3array[1], sj3array[1];
9999 bool j3valid[1]={false};
10000 _nj3 = 1;
10001 IkReal x733=((1.0)*new_r11);
10002 CheckValue<IkReal> x734 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((1.0)+(((-1.0)*(cj5*cj5)))+(((-1.0)*new_r00*x733)))),IKFAST_ATAN2_MAGTHRESH);
10003 if(!x734.valid){
10004 continue;
10005 }
10006 CheckValue<IkReal> x735=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*x733)))),-1);
10007 if(!x735.valid){
10008 continue;
10009 }
10010 j3array[0]=((-1.5707963267949)+(x734.value)+(((1.5707963267949)*(x735.value))));
10011 sj3array[0]=IKsin(j3array[0]);
10012 cj3array[0]=IKcos(j3array[0]);
10013 if( j3array[0] > IKPI )
10014 {
10015  j3array[0]-=IK2PI;
10016 }
10017 else if( j3array[0] < -IKPI )
10018 { j3array[0]+=IK2PI;
10019 }
10020 j3valid[0] = true;
10021 for(int ij3 = 0; ij3 < 1; ++ij3)
10022 {
10023 if( !j3valid[ij3] )
10024 {
10025  continue;
10026 }
10027 _ij3[0] = ij3; _ij3[1] = -1;
10028 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10029 {
10030 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10031 {
10032  j3valid[iij3]=false; _ij3[1] = iij3; break;
10033 }
10034 }
10035 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10036 {
10037 IkReal evalcond[8];
10038 IkReal x736=IKcos(j3);
10039 IkReal x737=IKsin(j3);
10040 IkReal x738=(sj5*x737);
10041 IkReal x739=(cj5*x737);
10042 IkReal x740=(sj5*x736);
10043 IkReal x741=((1.0)*x736);
10044 IkReal x742=(cj5*x741);
10045 evalcond[0]=(sj5+((new_r01*x736))+((new_r11*x737)));
10046 evalcond[1]=(x739+x740+new_r01);
10047 evalcond[2]=(sj5+((new_r00*x737))+(((-1.0)*new_r10*x741)));
10048 evalcond[3]=(cj5+((new_r01*x737))+(((-1.0)*new_r11*x741)));
10049 evalcond[4]=(x738+new_r00+(((-1.0)*x742)));
10050 evalcond[5]=(x738+new_r11+(((-1.0)*x742)));
10051 evalcond[6]=(((new_r00*x736))+((new_r10*x737))+(((-1.0)*cj5)));
10052 evalcond[7]=((((-1.0)*x739))+(((-1.0)*x740))+new_r10);
10053 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10054 {
10055 continue;
10056 }
10057 }
10058 
10059 {
10060 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10061 vinfos[0].jointtype = 1;
10062 vinfos[0].foffset = j0;
10063 vinfos[0].indices[0] = _ij0[0];
10064 vinfos[0].indices[1] = _ij0[1];
10065 vinfos[0].maxsolutions = _nj0;
10066 vinfos[1].jointtype = 1;
10067 vinfos[1].foffset = j1;
10068 vinfos[1].indices[0] = _ij1[0];
10069 vinfos[1].indices[1] = _ij1[1];
10070 vinfos[1].maxsolutions = _nj1;
10071 vinfos[2].jointtype = 1;
10072 vinfos[2].foffset = j2;
10073 vinfos[2].indices[0] = _ij2[0];
10074 vinfos[2].indices[1] = _ij2[1];
10075 vinfos[2].maxsolutions = _nj2;
10076 vinfos[3].jointtype = 1;
10077 vinfos[3].foffset = j3;
10078 vinfos[3].indices[0] = _ij3[0];
10079 vinfos[3].indices[1] = _ij3[1];
10080 vinfos[3].maxsolutions = _nj3;
10081 vinfos[4].jointtype = 1;
10082 vinfos[4].foffset = j4;
10083 vinfos[4].indices[0] = _ij4[0];
10084 vinfos[4].indices[1] = _ij4[1];
10085 vinfos[4].maxsolutions = _nj4;
10086 vinfos[5].jointtype = 1;
10087 vinfos[5].foffset = j5;
10088 vinfos[5].indices[0] = _ij5[0];
10089 vinfos[5].indices[1] = _ij5[1];
10090 vinfos[5].maxsolutions = _nj5;
10091 std::vector<int> vfree(0);
10092 solutions.AddSolution(vinfos,vfree);
10093 }
10094 }
10095 }
10096 
10097 }
10098 
10099 }
10100 
10101 } else
10102 {
10103 {
10104 IkReal j3array[1], cj3array[1], sj3array[1];
10105 bool j3valid[1]={false};
10106 _nj3 = 1;
10107 CheckValue<IkReal> x743 = IKatan2WithCheck(IkReal((((new_r11*sj5))+((cj5*new_r01)))),IkReal((((new_r01*sj5))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10108 if(!x743.valid){
10109 continue;
10110 }
10111 CheckValue<IkReal> x744=IKPowWithIntegerCheck(IKsign(((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))))),-1);
10112 if(!x744.valid){
10113 continue;
10114 }
10115 j3array[0]=((-1.5707963267949)+(x743.value)+(((1.5707963267949)*(x744.value))));
10116 sj3array[0]=IKsin(j3array[0]);
10117 cj3array[0]=IKcos(j3array[0]);
10118 if( j3array[0] > IKPI )
10119 {
10120  j3array[0]-=IK2PI;
10121 }
10122 else if( j3array[0] < -IKPI )
10123 { j3array[0]+=IK2PI;
10124 }
10125 j3valid[0] = true;
10126 for(int ij3 = 0; ij3 < 1; ++ij3)
10127 {
10128 if( !j3valid[ij3] )
10129 {
10130  continue;
10131 }
10132 _ij3[0] = ij3; _ij3[1] = -1;
10133 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10134 {
10135 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10136 {
10137  j3valid[iij3]=false; _ij3[1] = iij3; break;
10138 }
10139 }
10140 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10141 {
10142 IkReal evalcond[8];
10143 IkReal x745=IKcos(j3);
10144 IkReal x746=IKsin(j3);
10145 IkReal x747=(sj5*x746);
10146 IkReal x748=(cj5*x746);
10147 IkReal x749=(sj5*x745);
10148 IkReal x750=((1.0)*x745);
10149 IkReal x751=(cj5*x750);
10150 evalcond[0]=(sj5+((new_r01*x745))+((new_r11*x746)));
10151 evalcond[1]=(x748+x749+new_r01);
10152 evalcond[2]=(sj5+(((-1.0)*new_r10*x750))+((new_r00*x746)));
10153 evalcond[3]=(cj5+(((-1.0)*new_r11*x750))+((new_r01*x746)));
10154 evalcond[4]=(x747+new_r00+(((-1.0)*x751)));
10155 evalcond[5]=(x747+new_r11+(((-1.0)*x751)));
10156 evalcond[6]=(((new_r10*x746))+((new_r00*x745))+(((-1.0)*cj5)));
10157 evalcond[7]=((((-1.0)*x748))+(((-1.0)*x749))+new_r10);
10158 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10159 {
10160 continue;
10161 }
10162 }
10163 
10164 {
10165 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10166 vinfos[0].jointtype = 1;
10167 vinfos[0].foffset = j0;
10168 vinfos[0].indices[0] = _ij0[0];
10169 vinfos[0].indices[1] = _ij0[1];
10170 vinfos[0].maxsolutions = _nj0;
10171 vinfos[1].jointtype = 1;
10172 vinfos[1].foffset = j1;
10173 vinfos[1].indices[0] = _ij1[0];
10174 vinfos[1].indices[1] = _ij1[1];
10175 vinfos[1].maxsolutions = _nj1;
10176 vinfos[2].jointtype = 1;
10177 vinfos[2].foffset = j2;
10178 vinfos[2].indices[0] = _ij2[0];
10179 vinfos[2].indices[1] = _ij2[1];
10180 vinfos[2].maxsolutions = _nj2;
10181 vinfos[3].jointtype = 1;
10182 vinfos[3].foffset = j3;
10183 vinfos[3].indices[0] = _ij3[0];
10184 vinfos[3].indices[1] = _ij3[1];
10185 vinfos[3].maxsolutions = _nj3;
10186 vinfos[4].jointtype = 1;
10187 vinfos[4].foffset = j4;
10188 vinfos[4].indices[0] = _ij4[0];
10189 vinfos[4].indices[1] = _ij4[1];
10190 vinfos[4].maxsolutions = _nj4;
10191 vinfos[5].jointtype = 1;
10192 vinfos[5].foffset = j5;
10193 vinfos[5].indices[0] = _ij5[0];
10194 vinfos[5].indices[1] = _ij5[1];
10195 vinfos[5].maxsolutions = _nj5;
10196 std::vector<int> vfree(0);
10197 solutions.AddSolution(vinfos,vfree);
10198 }
10199 }
10200 }
10201 
10202 }
10203 
10204 }
10205 
10206 } else
10207 {
10208 {
10209 IkReal j3array[1], cj3array[1], sj3array[1];
10210 bool j3valid[1]={false};
10211 _nj3 = 1;
10212 IkReal x752=((1.0)*new_r11);
10213 CheckValue<IkReal> x753=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x752))+(((-1.0)*new_r00*new_r01)))),-1);
10214 if(!x753.valid){
10215 continue;
10216 }
10217 CheckValue<IkReal> x754 = IKatan2WithCheck(IkReal((((new_r10*sj5))+((new_r01*sj5)))),IkReal(((((-1.0)*sj5*x752))+((new_r00*sj5)))),IKFAST_ATAN2_MAGTHRESH);
10218 if(!x754.valid){
10219 continue;
10220 }
10221 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x753.value)))+(x754.value));
10222 sj3array[0]=IKsin(j3array[0]);
10223 cj3array[0]=IKcos(j3array[0]);
10224 if( j3array[0] > IKPI )
10225 {
10226  j3array[0]-=IK2PI;
10227 }
10228 else if( j3array[0] < -IKPI )
10229 { j3array[0]+=IK2PI;
10230 }
10231 j3valid[0] = true;
10232 for(int ij3 = 0; ij3 < 1; ++ij3)
10233 {
10234 if( !j3valid[ij3] )
10235 {
10236  continue;
10237 }
10238 _ij3[0] = ij3; _ij3[1] = -1;
10239 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10240 {
10241 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10242 {
10243  j3valid[iij3]=false; _ij3[1] = iij3; break;
10244 }
10245 }
10246 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10247 {
10248 IkReal evalcond[8];
10249 IkReal x755=IKcos(j3);
10250 IkReal x756=IKsin(j3);
10251 IkReal x757=(sj5*x756);
10252 IkReal x758=(cj5*x756);
10253 IkReal x759=(sj5*x755);
10254 IkReal x760=((1.0)*x755);
10255 IkReal x761=(cj5*x760);
10256 evalcond[0]=(sj5+((new_r11*x756))+((new_r01*x755)));
10257 evalcond[1]=(x759+x758+new_r01);
10258 evalcond[2]=(sj5+(((-1.0)*new_r10*x760))+((new_r00*x756)));
10259 evalcond[3]=(cj5+(((-1.0)*new_r11*x760))+((new_r01*x756)));
10260 evalcond[4]=((((-1.0)*x761))+x757+new_r00);
10261 evalcond[5]=((((-1.0)*x761))+x757+new_r11);
10262 evalcond[6]=(((new_r00*x755))+((new_r10*x756))+(((-1.0)*cj5)));
10263 evalcond[7]=((((-1.0)*x759))+(((-1.0)*x758))+new_r10);
10264 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10265 {
10266 continue;
10267 }
10268 }
10269 
10270 {
10271 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10272 vinfos[0].jointtype = 1;
10273 vinfos[0].foffset = j0;
10274 vinfos[0].indices[0] = _ij0[0];
10275 vinfos[0].indices[1] = _ij0[1];
10276 vinfos[0].maxsolutions = _nj0;
10277 vinfos[1].jointtype = 1;
10278 vinfos[1].foffset = j1;
10279 vinfos[1].indices[0] = _ij1[0];
10280 vinfos[1].indices[1] = _ij1[1];
10281 vinfos[1].maxsolutions = _nj1;
10282 vinfos[2].jointtype = 1;
10283 vinfos[2].foffset = j2;
10284 vinfos[2].indices[0] = _ij2[0];
10285 vinfos[2].indices[1] = _ij2[1];
10286 vinfos[2].maxsolutions = _nj2;
10287 vinfos[3].jointtype = 1;
10288 vinfos[3].foffset = j3;
10289 vinfos[3].indices[0] = _ij3[0];
10290 vinfos[3].indices[1] = _ij3[1];
10291 vinfos[3].maxsolutions = _nj3;
10292 vinfos[4].jointtype = 1;
10293 vinfos[4].foffset = j4;
10294 vinfos[4].indices[0] = _ij4[0];
10295 vinfos[4].indices[1] = _ij4[1];
10296 vinfos[4].maxsolutions = _nj4;
10297 vinfos[5].jointtype = 1;
10298 vinfos[5].foffset = j5;
10299 vinfos[5].indices[0] = _ij5[0];
10300 vinfos[5].indices[1] = _ij5[1];
10301 vinfos[5].maxsolutions = _nj5;
10302 std::vector<int> vfree(0);
10303 solutions.AddSolution(vinfos,vfree);
10304 }
10305 }
10306 }
10307 
10308 }
10309 
10310 }
10311 
10312 }
10313 } while(0);
10314 if( bgotonextstatement )
10315 {
10316 bool bgotonextstatement = true;
10317 do
10318 {
10319 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10320 evalcond[1]=new_r02;
10321 evalcond[2]=new_r12;
10322 evalcond[3]=new_r21;
10323 evalcond[4]=new_r20;
10324 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 )
10325 {
10326 bgotonextstatement=false;
10327 {
10328 IkReal j3eval[3];
10329 sj4=0;
10330 cj4=-1.0;
10331 j4=3.14159265358979;
10332 IkReal x762=((1.0)*new_r10);
10333 IkReal x763=((((-1.0)*new_r11*x762))+(((-1.0)*new_r00*new_r01)));
10334 j3eval[0]=x763;
10335 j3eval[1]=((IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(IKabs((((cj5*new_r01))+(((-1.0)*cj5*x762))))));
10336 j3eval[2]=IKsign(x763);
10337 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10338 {
10339 {
10340 IkReal j3eval[3];
10341 sj4=0;
10342 cj4=-1.0;
10343 j4=3.14159265358979;
10344 IkReal x764=((1.0)*new_r10);
10345 IkReal x765=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x764)));
10346 j3eval[0]=x765;
10347 j3eval[1]=IKsign(x765);
10348 j3eval[2]=((IKabs(((cj5*cj5)+(((-1.0)*new_r01*x764)))))+(IKabs((((new_r00*new_r01))+((cj5*sj5))))));
10349 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10350 {
10351 {
10352 IkReal j3eval[3];
10353 sj4=0;
10354 cj4=-1.0;
10355 j4=3.14159265358979;
10356 IkReal x766=((1.0)*new_r00);
10357 IkReal x767=(((cj5*new_r10))+(((-1.0)*sj5*x766)));
10358 j3eval[0]=x767;
10359 j3eval[1]=((IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))))+(IKabs(((((-1.0)*new_r10*x766))+((cj5*sj5))))));
10360 j3eval[2]=IKsign(x767);
10361 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10362 {
10363 {
10364 IkReal evalcond[1];
10365 bool bgotonextstatement = true;
10366 do
10367 {
10368 IkReal x768=((-1.0)*new_r00);
10369 IkReal x770 = ((new_r10*new_r10)+(new_r00*new_r00));
10370 if(IKabs(x770)==0){
10371 continue;
10372 }
10373 IkReal x769=pow(x770,-0.5);
10374 CheckValue<IkReal> x771 = IKatan2WithCheck(IkReal(new_r10),IkReal(x768),IKFAST_ATAN2_MAGTHRESH);
10375 if(!x771.valid){
10376 continue;
10377 }
10378 IkReal gconst12=((-1.0)*(x771.value));
10379 IkReal gconst13=((-1.0)*new_r10*x769);
10380 IkReal gconst14=(x768*x769);
10381 CheckValue<IkReal> x772 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10382 if(!x772.valid){
10383 continue;
10384 }
10385 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j5+(x772.value))))), 6.28318530717959)));
10386 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10387 {
10388 bgotonextstatement=false;
10389 {
10390 IkReal j3eval[3];
10391 IkReal x773=((-1.0)*new_r00);
10392 CheckValue<IkReal> x776 = IKatan2WithCheck(IkReal(new_r10),IkReal(x773),IKFAST_ATAN2_MAGTHRESH);
10393 if(!x776.valid){
10394 continue;
10395 }
10396 IkReal x774=((-1.0)*(x776.value));
10397 IkReal x775=x769;
10398 sj4=0;
10399 cj4=-1.0;
10400 j4=3.14159265358979;
10401 sj5=gconst13;
10402 cj5=gconst14;
10403 j5=x774;
10404 IkReal gconst12=x774;
10405 IkReal gconst13=((-1.0)*new_r10*x775);
10406 IkReal gconst14=(x773*x775);
10407 IkReal x777=new_r00*new_r00;
10408 IkReal x778=((1.0)*new_r11);
10409 IkReal x779=((1.0)*new_r00*new_r01);
10410 IkReal x780=((((-1.0)*new_r10*x778))+(((-1.0)*x779)));
10411 IkReal x781=x769;
10412 IkReal x782=(new_r00*x781);
10413 j3eval[0]=x780;
10414 j3eval[1]=((IKabs(((((-1.0)*x777*x781))+(((-1.0)*x778*x782)))))+(IKabs((((new_r10*x782))+(((-1.0)*x779*x781))))));
10415 j3eval[2]=IKsign(x780);
10416 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10417 {
10418 {
10419 IkReal j3eval[1];
10420 IkReal x783=((-1.0)*new_r00);
10421 CheckValue<IkReal> x786 = IKatan2WithCheck(IkReal(new_r10),IkReal(x783),IKFAST_ATAN2_MAGTHRESH);
10422 if(!x786.valid){
10423 continue;
10424 }
10425 IkReal x784=((-1.0)*(x786.value));
10426 IkReal x785=x769;
10427 sj4=0;
10428 cj4=-1.0;
10429 j4=3.14159265358979;
10430 sj5=gconst13;
10431 cj5=gconst14;
10432 j5=x784;
10433 IkReal gconst12=x784;
10434 IkReal gconst13=((-1.0)*new_r10*x785);
10435 IkReal gconst14=(x783*x785);
10436 IkReal x787=new_r10*new_r10;
10437 IkReal x788=new_r00*new_r00;
10438 CheckValue<IkReal> x791=IKPowWithIntegerCheck((x787+x788),-1);
10439 if(!x791.valid){
10440 continue;
10441 }
10442 IkReal x789=x791.value;
10443 IkReal x790=(new_r00*x789);
10444 j3eval[0]=((IKabs((((new_r01*x790*(new_r00*new_r00)))+((new_r01*x787*x790))+((new_r10*x790)))))+(IKabs((((x788*x789))+(((-1.0)*new_r01*new_r10))))));
10445 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10446 {
10447 {
10448 IkReal j3eval[1];
10449 IkReal x792=((-1.0)*new_r00);
10450 CheckValue<IkReal> x795 = IKatan2WithCheck(IkReal(new_r10),IkReal(x792),IKFAST_ATAN2_MAGTHRESH);
10451 if(!x795.valid){
10452 continue;
10453 }
10454 IkReal x793=((-1.0)*(x795.value));
10455 IkReal x794=x769;
10456 sj4=0;
10457 cj4=-1.0;
10458 j4=3.14159265358979;
10459 sj5=gconst13;
10460 cj5=gconst14;
10461 j5=x793;
10462 IkReal gconst12=x793;
10463 IkReal gconst13=((-1.0)*new_r10*x794);
10464 IkReal gconst14=(x792*x794);
10465 IkReal x796=new_r00*new_r00;
10466 IkReal x797=new_r10*new_r10;
10467 CheckValue<IkReal> x801=IKPowWithIntegerCheck((x797+x796),-1);
10468 if(!x801.valid){
10469 continue;
10470 }
10471 IkReal x798=x801.value;
10472 IkReal x799=(new_r10*x798);
10473 IkReal x800=((1.0)*x798);
10474 j3eval[0]=((IKabs((((x799*(new_r00*new_r00*new_r00)))+((new_r00*x799))+((new_r00*x799*(new_r10*new_r10))))))+(IKabs((((x796*x798))+(((-1.0)*x796*x797*x800))+(((-1.0)*x800*(x797*x797)))))));
10475 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10476 {
10477 {
10478 IkReal evalcond[3];
10479 bool bgotonextstatement = true;
10480 do
10481 {
10482 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10483 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10484 {
10485 bgotonextstatement=false;
10486 {
10487 IkReal j3eval[1];
10488 CheckValue<IkReal> x803 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10489 if(!x803.valid){
10490 continue;
10491 }
10492 IkReal x802=((-1.0)*(x803.value));
10493 sj4=0;
10494 cj4=-1.0;
10495 j4=3.14159265358979;
10496 sj5=gconst13;
10497 cj5=gconst14;
10498 j5=x802;
10499 new_r11=0;
10500 new_r00=0;
10501 IkReal gconst12=x802;
10502 IkReal x804 = new_r10*new_r10;
10503 if(IKabs(x804)==0){
10504 continue;
10505 }
10506 IkReal gconst13=((-1.0)*new_r10*(pow(x804,-0.5)));
10507 IkReal gconst14=0;
10508 j3eval[0]=new_r10;
10509 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10510 {
10511 {
10512 IkReal j3array[2], cj3array[2], sj3array[2];
10513 bool j3valid[2]={false};
10514 _nj3 = 2;
10515 CheckValue<IkReal> x805=IKPowWithIntegerCheck(gconst13,-1);
10516 if(!x805.valid){
10517 continue;
10518 }
10519 cj3array[0]=(new_r01*(x805.value));
10520 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10521 {
10522  j3valid[0] = j3valid[1] = true;
10523  j3array[0] = IKacos(cj3array[0]);
10524  sj3array[0] = IKsin(j3array[0]);
10525  cj3array[1] = cj3array[0];
10526  j3array[1] = -j3array[0];
10527  sj3array[1] = -sj3array[0];
10528 }
10529 else if( isnan(cj3array[0]) )
10530 {
10531  // probably any value will work
10532  j3valid[0] = true;
10533  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10534 }
10535 for(int ij3 = 0; ij3 < 2; ++ij3)
10536 {
10537 if( !j3valid[ij3] )
10538 {
10539  continue;
10540 }
10541 _ij3[0] = ij3; _ij3[1] = -1;
10542 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10543 {
10544 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10545 {
10546  j3valid[iij3]=false; _ij3[1] = iij3; break;
10547 }
10548 }
10549 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10550 {
10551 IkReal evalcond[6];
10552 IkReal x806=IKsin(j3);
10553 IkReal x807=IKcos(j3);
10554 IkReal x808=((1.0)*gconst13);
10555 evalcond[0]=(new_r01*x806);
10556 evalcond[1]=(new_r10*x806);
10557 evalcond[2]=(gconst13*x806);
10558 evalcond[3]=(gconst13+(((-1.0)*new_r10*x807)));
10559 evalcond[4]=((((-1.0)*x807*x808))+new_r10);
10560 evalcond[5]=(((new_r01*x807))+(((-1.0)*x808)));
10561 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10562 {
10563 continue;
10564 }
10565 }
10566 
10567 {
10568 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10569 vinfos[0].jointtype = 1;
10570 vinfos[0].foffset = j0;
10571 vinfos[0].indices[0] = _ij0[0];
10572 vinfos[0].indices[1] = _ij0[1];
10573 vinfos[0].maxsolutions = _nj0;
10574 vinfos[1].jointtype = 1;
10575 vinfos[1].foffset = j1;
10576 vinfos[1].indices[0] = _ij1[0];
10577 vinfos[1].indices[1] = _ij1[1];
10578 vinfos[1].maxsolutions = _nj1;
10579 vinfos[2].jointtype = 1;
10580 vinfos[2].foffset = j2;
10581 vinfos[2].indices[0] = _ij2[0];
10582 vinfos[2].indices[1] = _ij2[1];
10583 vinfos[2].maxsolutions = _nj2;
10584 vinfos[3].jointtype = 1;
10585 vinfos[3].foffset = j3;
10586 vinfos[3].indices[0] = _ij3[0];
10587 vinfos[3].indices[1] = _ij3[1];
10588 vinfos[3].maxsolutions = _nj3;
10589 vinfos[4].jointtype = 1;
10590 vinfos[4].foffset = j4;
10591 vinfos[4].indices[0] = _ij4[0];
10592 vinfos[4].indices[1] = _ij4[1];
10593 vinfos[4].maxsolutions = _nj4;
10594 vinfos[5].jointtype = 1;
10595 vinfos[5].foffset = j5;
10596 vinfos[5].indices[0] = _ij5[0];
10597 vinfos[5].indices[1] = _ij5[1];
10598 vinfos[5].maxsolutions = _nj5;
10599 std::vector<int> vfree(0);
10600 solutions.AddSolution(vinfos,vfree);
10601 }
10602 }
10603 }
10604 
10605 } else
10606 {
10607 {
10608 IkReal j3array[2], cj3array[2], sj3array[2];
10609 bool j3valid[2]={false};
10610 _nj3 = 2;
10611 CheckValue<IkReal> x809=IKPowWithIntegerCheck(new_r10,-1);
10612 if(!x809.valid){
10613 continue;
10614 }
10615 cj3array[0]=(gconst13*(x809.value));
10616 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10617 {
10618  j3valid[0] = j3valid[1] = true;
10619  j3array[0] = IKacos(cj3array[0]);
10620  sj3array[0] = IKsin(j3array[0]);
10621  cj3array[1] = cj3array[0];
10622  j3array[1] = -j3array[0];
10623  sj3array[1] = -sj3array[0];
10624 }
10625 else if( isnan(cj3array[0]) )
10626 {
10627  // probably any value will work
10628  j3valid[0] = true;
10629  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10630 }
10631 for(int ij3 = 0; ij3 < 2; ++ij3)
10632 {
10633 if( !j3valid[ij3] )
10634 {
10635  continue;
10636 }
10637 _ij3[0] = ij3; _ij3[1] = -1;
10638 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10639 {
10640 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10641 {
10642  j3valid[iij3]=false; _ij3[1] = iij3; break;
10643 }
10644 }
10645 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10646 {
10647 IkReal evalcond[6];
10648 IkReal x810=IKsin(j3);
10649 IkReal x811=IKcos(j3);
10650 IkReal x812=((1.0)*gconst13);
10651 IkReal x813=(x811*x812);
10652 evalcond[0]=(new_r01*x810);
10653 evalcond[1]=(new_r10*x810);
10654 evalcond[2]=(gconst13*x810);
10655 evalcond[3]=((((-1.0)*x813))+new_r01);
10656 evalcond[4]=((((-1.0)*x813))+new_r10);
10657 evalcond[5]=(((new_r01*x811))+(((-1.0)*x812)));
10658 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10659 {
10660 continue;
10661 }
10662 }
10663 
10664 {
10665 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10666 vinfos[0].jointtype = 1;
10667 vinfos[0].foffset = j0;
10668 vinfos[0].indices[0] = _ij0[0];
10669 vinfos[0].indices[1] = _ij0[1];
10670 vinfos[0].maxsolutions = _nj0;
10671 vinfos[1].jointtype = 1;
10672 vinfos[1].foffset = j1;
10673 vinfos[1].indices[0] = _ij1[0];
10674 vinfos[1].indices[1] = _ij1[1];
10675 vinfos[1].maxsolutions = _nj1;
10676 vinfos[2].jointtype = 1;
10677 vinfos[2].foffset = j2;
10678 vinfos[2].indices[0] = _ij2[0];
10679 vinfos[2].indices[1] = _ij2[1];
10680 vinfos[2].maxsolutions = _nj2;
10681 vinfos[3].jointtype = 1;
10682 vinfos[3].foffset = j3;
10683 vinfos[3].indices[0] = _ij3[0];
10684 vinfos[3].indices[1] = _ij3[1];
10685 vinfos[3].maxsolutions = _nj3;
10686 vinfos[4].jointtype = 1;
10687 vinfos[4].foffset = j4;
10688 vinfos[4].indices[0] = _ij4[0];
10689 vinfos[4].indices[1] = _ij4[1];
10690 vinfos[4].maxsolutions = _nj4;
10691 vinfos[5].jointtype = 1;
10692 vinfos[5].foffset = j5;
10693 vinfos[5].indices[0] = _ij5[0];
10694 vinfos[5].indices[1] = _ij5[1];
10695 vinfos[5].maxsolutions = _nj5;
10696 std::vector<int> vfree(0);
10697 solutions.AddSolution(vinfos,vfree);
10698 }
10699 }
10700 }
10701 
10702 }
10703 
10704 }
10705 
10706 }
10707 } while(0);
10708 if( bgotonextstatement )
10709 {
10710 bool bgotonextstatement = true;
10711 do
10712 {
10713 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
10714 evalcond[1]=gconst14;
10715 evalcond[2]=gconst13;
10716 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
10717 {
10718 bgotonextstatement=false;
10719 {
10720 IkReal j3eval[3];
10721 IkReal x814=((-1.0)*new_r00);
10722 CheckValue<IkReal> x816 = IKatan2WithCheck(IkReal(new_r10),IkReal(x814),IKFAST_ATAN2_MAGTHRESH);
10723 if(!x816.valid){
10724 continue;
10725 }
10726 IkReal x815=((-1.0)*(x816.value));
10727 sj4=0;
10728 cj4=-1.0;
10729 j4=3.14159265358979;
10730 sj5=gconst13;
10731 cj5=gconst14;
10732 j5=x815;
10733 new_r11=0;
10734 new_r01=0;
10735 new_r22=0;
10736 new_r20=0;
10737 IkReal gconst12=x815;
10738 IkReal gconst13=((-1.0)*new_r10);
10739 IkReal gconst14=x814;
10740 j3eval[0]=1.0;
10741 j3eval[1]=1.0;
10742 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
10743 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10744 {
10745 {
10746 IkReal j3eval[3];
10747 IkReal x817=((-1.0)*new_r00);
10748 CheckValue<IkReal> x819 = IKatan2WithCheck(IkReal(new_r10),IkReal(x817),IKFAST_ATAN2_MAGTHRESH);
10749 if(!x819.valid){
10750 continue;
10751 }
10752 IkReal x818=((-1.0)*(x819.value));
10753 sj4=0;
10754 cj4=-1.0;
10755 j4=3.14159265358979;
10756 sj5=gconst13;
10757 cj5=gconst14;
10758 j5=x818;
10759 new_r11=0;
10760 new_r01=0;
10761 new_r22=0;
10762 new_r20=0;
10763 IkReal gconst12=x818;
10764 IkReal gconst13=((-1.0)*new_r10);
10765 IkReal gconst14=x817;
10766 j3eval[0]=-1.0;
10767 j3eval[1]=-1.0;
10768 j3eval[2]=((IKabs(((-1.0)+(new_r10*new_r10))))+(IKabs((new_r00*new_r10))));
10769 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10770 {
10771 {
10772 IkReal j3eval[3];
10773 IkReal x820=((-1.0)*new_r00);
10774 CheckValue<IkReal> x822 = IKatan2WithCheck(IkReal(new_r10),IkReal(x820),IKFAST_ATAN2_MAGTHRESH);
10775 if(!x822.valid){
10776 continue;
10777 }
10778 IkReal x821=((-1.0)*(x822.value));
10779 sj4=0;
10780 cj4=-1.0;
10781 j4=3.14159265358979;
10782 sj5=gconst13;
10783 cj5=gconst14;
10784 j5=x821;
10785 new_r11=0;
10786 new_r01=0;
10787 new_r22=0;
10788 new_r20=0;
10789 IkReal gconst12=x821;
10790 IkReal gconst13=((-1.0)*new_r10);
10791 IkReal gconst14=x820;
10792 j3eval[0]=1.0;
10793 j3eval[1]=((((0.5)*(IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
10794 j3eval[2]=1.0;
10795 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10796 {
10797 continue; // 3 cases reached
10798 
10799 } else
10800 {
10801 {
10802 IkReal j3array[1], cj3array[1], sj3array[1];
10803 bool j3valid[1]={false};
10804 _nj3 = 1;
10805 CheckValue<IkReal> x823 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r10)))),IkReal(((gconst14*gconst14)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
10806 if(!x823.valid){
10807 continue;
10808 }
10809 CheckValue<IkReal> x824=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
10810 if(!x824.valid){
10811 continue;
10812 }
10813 j3array[0]=((-1.5707963267949)+(x823.value)+(((1.5707963267949)*(x824.value))));
10814 sj3array[0]=IKsin(j3array[0]);
10815 cj3array[0]=IKcos(j3array[0]);
10816 if( j3array[0] > IKPI )
10817 {
10818  j3array[0]-=IK2PI;
10819 }
10820 else if( j3array[0] < -IKPI )
10821 { j3array[0]+=IK2PI;
10822 }
10823 j3valid[0] = true;
10824 for(int ij3 = 0; ij3 < 1; ++ij3)
10825 {
10826 if( !j3valid[ij3] )
10827 {
10828  continue;
10829 }
10830 _ij3[0] = ij3; _ij3[1] = -1;
10831 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10832 {
10833 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10834 {
10835  j3valid[iij3]=false; _ij3[1] = iij3; break;
10836 }
10837 }
10838 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10839 {
10840 IkReal evalcond[6];
10841 IkReal x825=IKsin(j3);
10842 IkReal x826=IKcos(j3);
10843 IkReal x827=((1.0)*gconst13);
10844 IkReal x828=(gconst14*x825);
10845 IkReal x829=(gconst14*x826);
10846 IkReal x830=(x826*x827);
10847 evalcond[0]=((((-1.0)*x830))+x828);
10848 evalcond[1]=(((new_r10*x825))+gconst14+((new_r00*x826)));
10849 evalcond[2]=(((gconst13*x825))+new_r00+x829);
10850 evalcond[3]=((((-1.0)*new_r10*x826))+gconst13+((new_r00*x825)));
10851 evalcond[4]=((((-1.0)*x829))+(((-1.0)*x825*x827)));
10852 evalcond[5]=((((-1.0)*x830))+new_r10+x828);
10853 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10854 {
10855 continue;
10856 }
10857 }
10858 
10859 {
10860 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10861 vinfos[0].jointtype = 1;
10862 vinfos[0].foffset = j0;
10863 vinfos[0].indices[0] = _ij0[0];
10864 vinfos[0].indices[1] = _ij0[1];
10865 vinfos[0].maxsolutions = _nj0;
10866 vinfos[1].jointtype = 1;
10867 vinfos[1].foffset = j1;
10868 vinfos[1].indices[0] = _ij1[0];
10869 vinfos[1].indices[1] = _ij1[1];
10870 vinfos[1].maxsolutions = _nj1;
10871 vinfos[2].jointtype = 1;
10872 vinfos[2].foffset = j2;
10873 vinfos[2].indices[0] = _ij2[0];
10874 vinfos[2].indices[1] = _ij2[1];
10875 vinfos[2].maxsolutions = _nj2;
10876 vinfos[3].jointtype = 1;
10877 vinfos[3].foffset = j3;
10878 vinfos[3].indices[0] = _ij3[0];
10879 vinfos[3].indices[1] = _ij3[1];
10880 vinfos[3].maxsolutions = _nj3;
10881 vinfos[4].jointtype = 1;
10882 vinfos[4].foffset = j4;
10883 vinfos[4].indices[0] = _ij4[0];
10884 vinfos[4].indices[1] = _ij4[1];
10885 vinfos[4].maxsolutions = _nj4;
10886 vinfos[5].jointtype = 1;
10887 vinfos[5].foffset = j5;
10888 vinfos[5].indices[0] = _ij5[0];
10889 vinfos[5].indices[1] = _ij5[1];
10890 vinfos[5].maxsolutions = _nj5;
10891 std::vector<int> vfree(0);
10892 solutions.AddSolution(vinfos,vfree);
10893 }
10894 }
10895 }
10896 
10897 }
10898 
10899 }
10900 
10901 } else
10902 {
10903 {
10904 IkReal j3array[1], cj3array[1], sj3array[1];
10905 bool j3valid[1]={false};
10906 _nj3 = 1;
10907 CheckValue<IkReal> x831=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst14*gconst14)))+(((-1.0)*(gconst13*gconst13))))),-1);
10908 if(!x831.valid){
10909 continue;
10910 }
10911 CheckValue<IkReal> x832 = IKatan2WithCheck(IkReal((gconst13*new_r00)),IkReal((gconst14*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10912 if(!x832.valid){
10913 continue;
10914 }
10915 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x831.value)))+(x832.value));
10916 sj3array[0]=IKsin(j3array[0]);
10917 cj3array[0]=IKcos(j3array[0]);
10918 if( j3array[0] > IKPI )
10919 {
10920  j3array[0]-=IK2PI;
10921 }
10922 else if( j3array[0] < -IKPI )
10923 { j3array[0]+=IK2PI;
10924 }
10925 j3valid[0] = true;
10926 for(int ij3 = 0; ij3 < 1; ++ij3)
10927 {
10928 if( !j3valid[ij3] )
10929 {
10930  continue;
10931 }
10932 _ij3[0] = ij3; _ij3[1] = -1;
10933 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10934 {
10935 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10936 {
10937  j3valid[iij3]=false; _ij3[1] = iij3; break;
10938 }
10939 }
10940 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10941 {
10942 IkReal evalcond[6];
10943 IkReal x833=IKsin(j3);
10944 IkReal x834=IKcos(j3);
10945 IkReal x835=((1.0)*gconst13);
10946 IkReal x836=(gconst14*x833);
10947 IkReal x837=(gconst14*x834);
10948 IkReal x838=(x834*x835);
10949 evalcond[0]=((((-1.0)*x838))+x836);
10950 evalcond[1]=(gconst14+((new_r00*x834))+((new_r10*x833)));
10951 evalcond[2]=(((gconst13*x833))+new_r00+x837);
10952 evalcond[3]=(gconst13+((new_r00*x833))+(((-1.0)*new_r10*x834)));
10953 evalcond[4]=((((-1.0)*x833*x835))+(((-1.0)*x837)));
10954 evalcond[5]=((((-1.0)*x838))+new_r10+x836);
10955 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10956 {
10957 continue;
10958 }
10959 }
10960 
10961 {
10962 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10963 vinfos[0].jointtype = 1;
10964 vinfos[0].foffset = j0;
10965 vinfos[0].indices[0] = _ij0[0];
10966 vinfos[0].indices[1] = _ij0[1];
10967 vinfos[0].maxsolutions = _nj0;
10968 vinfos[1].jointtype = 1;
10969 vinfos[1].foffset = j1;
10970 vinfos[1].indices[0] = _ij1[0];
10971 vinfos[1].indices[1] = _ij1[1];
10972 vinfos[1].maxsolutions = _nj1;
10973 vinfos[2].jointtype = 1;
10974 vinfos[2].foffset = j2;
10975 vinfos[2].indices[0] = _ij2[0];
10976 vinfos[2].indices[1] = _ij2[1];
10977 vinfos[2].maxsolutions = _nj2;
10978 vinfos[3].jointtype = 1;
10979 vinfos[3].foffset = j3;
10980 vinfos[3].indices[0] = _ij3[0];
10981 vinfos[3].indices[1] = _ij3[1];
10982 vinfos[3].maxsolutions = _nj3;
10983 vinfos[4].jointtype = 1;
10984 vinfos[4].foffset = j4;
10985 vinfos[4].indices[0] = _ij4[0];
10986 vinfos[4].indices[1] = _ij4[1];
10987 vinfos[4].maxsolutions = _nj4;
10988 vinfos[5].jointtype = 1;
10989 vinfos[5].foffset = j5;
10990 vinfos[5].indices[0] = _ij5[0];
10991 vinfos[5].indices[1] = _ij5[1];
10992 vinfos[5].maxsolutions = _nj5;
10993 std::vector<int> vfree(0);
10994 solutions.AddSolution(vinfos,vfree);
10995 }
10996 }
10997 }
10998 
10999 }
11000 
11001 }
11002 
11003 } else
11004 {
11005 {
11006 IkReal j3array[1], cj3array[1], sj3array[1];
11007 bool j3valid[1]={false};
11008 _nj3 = 1;
11009 CheckValue<IkReal> x839=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
11010 if(!x839.valid){
11011 continue;
11012 }
11013 CheckValue<IkReal> x840 = IKatan2WithCheck(IkReal((gconst13*gconst14)),IkReal(gconst14*gconst14),IKFAST_ATAN2_MAGTHRESH);
11014 if(!x840.valid){
11015 continue;
11016 }
11017 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x839.value)))+(x840.value));
11018 sj3array[0]=IKsin(j3array[0]);
11019 cj3array[0]=IKcos(j3array[0]);
11020 if( j3array[0] > IKPI )
11021 {
11022  j3array[0]-=IK2PI;
11023 }
11024 else if( j3array[0] < -IKPI )
11025 { j3array[0]+=IK2PI;
11026 }
11027 j3valid[0] = true;
11028 for(int ij3 = 0; ij3 < 1; ++ij3)
11029 {
11030 if( !j3valid[ij3] )
11031 {
11032  continue;
11033 }
11034 _ij3[0] = ij3; _ij3[1] = -1;
11035 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11036 {
11037 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11038 {
11039  j3valid[iij3]=false; _ij3[1] = iij3; break;
11040 }
11041 }
11042 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11043 {
11044 IkReal evalcond[6];
11045 IkReal x841=IKsin(j3);
11046 IkReal x842=IKcos(j3);
11047 IkReal x843=((1.0)*gconst13);
11048 IkReal x844=(gconst14*x841);
11049 IkReal x845=(gconst14*x842);
11050 IkReal x846=(x842*x843);
11051 evalcond[0]=((((-1.0)*x846))+x844);
11052 evalcond[1]=(((new_r10*x841))+((new_r00*x842))+gconst14);
11053 evalcond[2]=(((gconst13*x841))+new_r00+x845);
11054 evalcond[3]=(((new_r00*x841))+gconst13+(((-1.0)*new_r10*x842)));
11055 evalcond[4]=((((-1.0)*x841*x843))+(((-1.0)*x845)));
11056 evalcond[5]=((((-1.0)*x846))+new_r10+x844);
11057 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11058 {
11059 continue;
11060 }
11061 }
11062 
11063 {
11064 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11065 vinfos[0].jointtype = 1;
11066 vinfos[0].foffset = j0;
11067 vinfos[0].indices[0] = _ij0[0];
11068 vinfos[0].indices[1] = _ij0[1];
11069 vinfos[0].maxsolutions = _nj0;
11070 vinfos[1].jointtype = 1;
11071 vinfos[1].foffset = j1;
11072 vinfos[1].indices[0] = _ij1[0];
11073 vinfos[1].indices[1] = _ij1[1];
11074 vinfos[1].maxsolutions = _nj1;
11075 vinfos[2].jointtype = 1;
11076 vinfos[2].foffset = j2;
11077 vinfos[2].indices[0] = _ij2[0];
11078 vinfos[2].indices[1] = _ij2[1];
11079 vinfos[2].maxsolutions = _nj2;
11080 vinfos[3].jointtype = 1;
11081 vinfos[3].foffset = j3;
11082 vinfos[3].indices[0] = _ij3[0];
11083 vinfos[3].indices[1] = _ij3[1];
11084 vinfos[3].maxsolutions = _nj3;
11085 vinfos[4].jointtype = 1;
11086 vinfos[4].foffset = j4;
11087 vinfos[4].indices[0] = _ij4[0];
11088 vinfos[4].indices[1] = _ij4[1];
11089 vinfos[4].maxsolutions = _nj4;
11090 vinfos[5].jointtype = 1;
11091 vinfos[5].foffset = j5;
11092 vinfos[5].indices[0] = _ij5[0];
11093 vinfos[5].indices[1] = _ij5[1];
11094 vinfos[5].maxsolutions = _nj5;
11095 std::vector<int> vfree(0);
11096 solutions.AddSolution(vinfos,vfree);
11097 }
11098 }
11099 }
11100 
11101 }
11102 
11103 }
11104 
11105 }
11106 } while(0);
11107 if( bgotonextstatement )
11108 {
11109 bool bgotonextstatement = true;
11110 do
11111 {
11112 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
11113 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11114 {
11115 bgotonextstatement=false;
11116 {
11117 IkReal j3eval[1];
11118 IkReal x847=((-1.0)*new_r00);
11119 CheckValue<IkReal> x849 = IKatan2WithCheck(IkReal(0),IkReal(x847),IKFAST_ATAN2_MAGTHRESH);
11120 if(!x849.valid){
11121 continue;
11122 }
11123 IkReal x848=((-1.0)*(x849.value));
11124 sj4=0;
11125 cj4=-1.0;
11126 j4=3.14159265358979;
11127 sj5=gconst13;
11128 cj5=gconst14;
11129 j5=x848;
11130 new_r01=0;
11131 new_r10=0;
11132 IkReal gconst12=x848;
11133 IkReal gconst13=0;
11134 IkReal x850 = new_r00*new_r00;
11135 if(IKabs(x850)==0){
11136 continue;
11137 }
11138 IkReal gconst14=(x847*(pow(x850,-0.5)));
11139 j3eval[0]=new_r11;
11140 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11141 {
11142 {
11143 IkReal j3array[2], cj3array[2], sj3array[2];
11144 bool j3valid[2]={false};
11145 _nj3 = 2;
11146 CheckValue<IkReal> x851=IKPowWithIntegerCheck(gconst14,-1);
11147 if(!x851.valid){
11148 continue;
11149 }
11150 cj3array[0]=(new_r11*(x851.value));
11151 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11152 {
11153  j3valid[0] = j3valid[1] = true;
11154  j3array[0] = IKacos(cj3array[0]);
11155  sj3array[0] = IKsin(j3array[0]);
11156  cj3array[1] = cj3array[0];
11157  j3array[1] = -j3array[0];
11158  sj3array[1] = -sj3array[0];
11159 }
11160 else if( isnan(cj3array[0]) )
11161 {
11162  // probably any value will work
11163  j3valid[0] = true;
11164  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11165 }
11166 for(int ij3 = 0; ij3 < 2; ++ij3)
11167 {
11168 if( !j3valid[ij3] )
11169 {
11170  continue;
11171 }
11172 _ij3[0] = ij3; _ij3[1] = -1;
11173 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11174 {
11175 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11176 {
11177  j3valid[iij3]=false; _ij3[1] = iij3; break;
11178 }
11179 }
11180 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11181 {
11182 IkReal evalcond[6];
11183 IkReal x852=IKsin(j3);
11184 IkReal x853=IKcos(j3);
11185 evalcond[0]=(new_r00*x852);
11186 evalcond[1]=(new_r11*x852);
11187 evalcond[2]=(gconst14*x852);
11188 evalcond[3]=(gconst14+((new_r00*x853)));
11189 evalcond[4]=(new_r00+((gconst14*x853)));
11190 evalcond[5]=(gconst14+(((-1.0)*new_r11*x853)));
11191 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11192 {
11193 continue;
11194 }
11195 }
11196 
11197 {
11198 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11199 vinfos[0].jointtype = 1;
11200 vinfos[0].foffset = j0;
11201 vinfos[0].indices[0] = _ij0[0];
11202 vinfos[0].indices[1] = _ij0[1];
11203 vinfos[0].maxsolutions = _nj0;
11204 vinfos[1].jointtype = 1;
11205 vinfos[1].foffset = j1;
11206 vinfos[1].indices[0] = _ij1[0];
11207 vinfos[1].indices[1] = _ij1[1];
11208 vinfos[1].maxsolutions = _nj1;
11209 vinfos[2].jointtype = 1;
11210 vinfos[2].foffset = j2;
11211 vinfos[2].indices[0] = _ij2[0];
11212 vinfos[2].indices[1] = _ij2[1];
11213 vinfos[2].maxsolutions = _nj2;
11214 vinfos[3].jointtype = 1;
11215 vinfos[3].foffset = j3;
11216 vinfos[3].indices[0] = _ij3[0];
11217 vinfos[3].indices[1] = _ij3[1];
11218 vinfos[3].maxsolutions = _nj3;
11219 vinfos[4].jointtype = 1;
11220 vinfos[4].foffset = j4;
11221 vinfos[4].indices[0] = _ij4[0];
11222 vinfos[4].indices[1] = _ij4[1];
11223 vinfos[4].maxsolutions = _nj4;
11224 vinfos[5].jointtype = 1;
11225 vinfos[5].foffset = j5;
11226 vinfos[5].indices[0] = _ij5[0];
11227 vinfos[5].indices[1] = _ij5[1];
11228 vinfos[5].maxsolutions = _nj5;
11229 std::vector<int> vfree(0);
11230 solutions.AddSolution(vinfos,vfree);
11231 }
11232 }
11233 }
11234 
11235 } else
11236 {
11237 {
11238 IkReal j3array[2], cj3array[2], sj3array[2];
11239 bool j3valid[2]={false};
11240 _nj3 = 2;
11241 CheckValue<IkReal> x854=IKPowWithIntegerCheck(new_r11,-1);
11242 if(!x854.valid){
11243 continue;
11244 }
11245 cj3array[0]=(gconst14*(x854.value));
11246 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11247 {
11248  j3valid[0] = j3valid[1] = true;
11249  j3array[0] = IKacos(cj3array[0]);
11250  sj3array[0] = IKsin(j3array[0]);
11251  cj3array[1] = cj3array[0];
11252  j3array[1] = -j3array[0];
11253  sj3array[1] = -sj3array[0];
11254 }
11255 else if( isnan(cj3array[0]) )
11256 {
11257  // probably any value will work
11258  j3valid[0] = true;
11259  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11260 }
11261 for(int ij3 = 0; ij3 < 2; ++ij3)
11262 {
11263 if( !j3valid[ij3] )
11264 {
11265  continue;
11266 }
11267 _ij3[0] = ij3; _ij3[1] = -1;
11268 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11269 {
11270 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11271 {
11272  j3valid[iij3]=false; _ij3[1] = iij3; break;
11273 }
11274 }
11275 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11276 {
11277 IkReal evalcond[6];
11278 IkReal x855=IKsin(j3);
11279 IkReal x856=IKcos(j3);
11280 IkReal x857=(gconst14*x856);
11281 evalcond[0]=(new_r00*x855);
11282 evalcond[1]=(new_r11*x855);
11283 evalcond[2]=(gconst14*x855);
11284 evalcond[3]=(gconst14+((new_r00*x856)));
11285 evalcond[4]=(new_r00+x857);
11286 evalcond[5]=((((-1.0)*x857))+new_r11);
11287 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11288 {
11289 continue;
11290 }
11291 }
11292 
11293 {
11294 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11295 vinfos[0].jointtype = 1;
11296 vinfos[0].foffset = j0;
11297 vinfos[0].indices[0] = _ij0[0];
11298 vinfos[0].indices[1] = _ij0[1];
11299 vinfos[0].maxsolutions = _nj0;
11300 vinfos[1].jointtype = 1;
11301 vinfos[1].foffset = j1;
11302 vinfos[1].indices[0] = _ij1[0];
11303 vinfos[1].indices[1] = _ij1[1];
11304 vinfos[1].maxsolutions = _nj1;
11305 vinfos[2].jointtype = 1;
11306 vinfos[2].foffset = j2;
11307 vinfos[2].indices[0] = _ij2[0];
11308 vinfos[2].indices[1] = _ij2[1];
11309 vinfos[2].maxsolutions = _nj2;
11310 vinfos[3].jointtype = 1;
11311 vinfos[3].foffset = j3;
11312 vinfos[3].indices[0] = _ij3[0];
11313 vinfos[3].indices[1] = _ij3[1];
11314 vinfos[3].maxsolutions = _nj3;
11315 vinfos[4].jointtype = 1;
11316 vinfos[4].foffset = j4;
11317 vinfos[4].indices[0] = _ij4[0];
11318 vinfos[4].indices[1] = _ij4[1];
11319 vinfos[4].maxsolutions = _nj4;
11320 vinfos[5].jointtype = 1;
11321 vinfos[5].foffset = j5;
11322 vinfos[5].indices[0] = _ij5[0];
11323 vinfos[5].indices[1] = _ij5[1];
11324 vinfos[5].maxsolutions = _nj5;
11325 std::vector<int> vfree(0);
11326 solutions.AddSolution(vinfos,vfree);
11327 }
11328 }
11329 }
11330 
11331 }
11332 
11333 }
11334 
11335 }
11336 } while(0);
11337 if( bgotonextstatement )
11338 {
11339 bool bgotonextstatement = true;
11340 do
11341 {
11342 evalcond[0]=IKabs(new_r00);
11343 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11344 {
11345 bgotonextstatement=false;
11346 {
11347 IkReal j3eval[1];
11348 CheckValue<IkReal> x859 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11349 if(!x859.valid){
11350 continue;
11351 }
11352 IkReal x858=((-1.0)*(x859.value));
11353 sj4=0;
11354 cj4=-1.0;
11355 j4=3.14159265358979;
11356 sj5=gconst13;
11357 cj5=gconst14;
11358 j5=x858;
11359 new_r00=0;
11360 IkReal gconst12=x858;
11361 IkReal x860 = new_r10*new_r10;
11362 if(IKabs(x860)==0){
11363 continue;
11364 }
11365 IkReal gconst13=((-1.0)*new_r10*(pow(x860,-0.5)));
11366 IkReal gconst14=0;
11367 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11368 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11369 {
11370 {
11371 IkReal j3eval[1];
11372 CheckValue<IkReal> x862 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11373 if(!x862.valid){
11374 continue;
11375 }
11376 IkReal x861=((-1.0)*(x862.value));
11377 sj4=0;
11378 cj4=-1.0;
11379 j4=3.14159265358979;
11380 sj5=gconst13;
11381 cj5=gconst14;
11382 j5=x861;
11383 new_r00=0;
11384 IkReal gconst12=x861;
11385 IkReal x863 = new_r10*new_r10;
11386 if(IKabs(x863)==0){
11387 continue;
11388 }
11389 IkReal gconst13=((-1.0)*new_r10*(pow(x863,-0.5)));
11390 IkReal gconst14=0;
11391 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11392 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11393 {
11394 {
11395 IkReal j3eval[1];
11396 CheckValue<IkReal> x865 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11397 if(!x865.valid){
11398 continue;
11399 }
11400 IkReal x864=((-1.0)*(x865.value));
11401 sj4=0;
11402 cj4=-1.0;
11403 j4=3.14159265358979;
11404 sj5=gconst13;
11405 cj5=gconst14;
11406 j5=x864;
11407 new_r00=0;
11408 IkReal gconst12=x864;
11409 IkReal x866 = new_r10*new_r10;
11410 if(IKabs(x866)==0){
11411 continue;
11412 }
11413 IkReal gconst13=((-1.0)*new_r10*(pow(x866,-0.5)));
11414 IkReal gconst14=0;
11415 j3eval[0]=new_r10;
11416 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11417 {
11418 continue; // 3 cases reached
11419 
11420 } else
11421 {
11422 {
11423 IkReal j3array[1], cj3array[1], sj3array[1];
11424 bool j3valid[1]={false};
11425 _nj3 = 1;
11426 CheckValue<IkReal> x867=IKPowWithIntegerCheck(gconst13,-1);
11427 if(!x867.valid){
11428 continue;
11429 }
11430 CheckValue<IkReal> x868=IKPowWithIntegerCheck(new_r10,-1);
11431 if(!x868.valid){
11432 continue;
11433 }
11434 if( IKabs((new_r11*(x867.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst13*(x868.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x867.value)))+IKsqr((gconst13*(x868.value)))-1) <= IKFAST_SINCOS_THRESH )
11435  continue;
11436 j3array[0]=IKatan2((new_r11*(x867.value)), (gconst13*(x868.value)));
11437 sj3array[0]=IKsin(j3array[0]);
11438 cj3array[0]=IKcos(j3array[0]);
11439 if( j3array[0] > IKPI )
11440 {
11441  j3array[0]-=IK2PI;
11442 }
11443 else if( j3array[0] < -IKPI )
11444 { j3array[0]+=IK2PI;
11445 }
11446 j3valid[0] = true;
11447 for(int ij3 = 0; ij3 < 1; ++ij3)
11448 {
11449 if( !j3valid[ij3] )
11450 {
11451  continue;
11452 }
11453 _ij3[0] = ij3; _ij3[1] = -1;
11454 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11455 {
11456 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11457 {
11458  j3valid[iij3]=false; _ij3[1] = iij3; break;
11459 }
11460 }
11461 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11462 {
11463 IkReal evalcond[8];
11464 IkReal x869=IKsin(j3);
11465 IkReal x870=IKcos(j3);
11466 IkReal x871=((1.0)*x870);
11467 IkReal x872=(gconst13*x869);
11468 IkReal x873=(gconst13*x871);
11469 evalcond[0]=(new_r10*x869);
11470 evalcond[1]=x872;
11471 evalcond[2]=(gconst13+(((-1.0)*new_r10*x871)));
11472 evalcond[3]=(new_r01+(((-1.0)*x873)));
11473 evalcond[4]=((((-1.0)*x872))+new_r11);
11474 evalcond[5]=(new_r10+(((-1.0)*x873)));
11475 evalcond[6]=(((new_r01*x869))+(((-1.0)*new_r11*x871)));
11476 evalcond[7]=(((new_r01*x870))+((new_r11*x869))+(((-1.0)*gconst13)));
11477 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11478 {
11479 continue;
11480 }
11481 }
11482 
11483 {
11484 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11485 vinfos[0].jointtype = 1;
11486 vinfos[0].foffset = j0;
11487 vinfos[0].indices[0] = _ij0[0];
11488 vinfos[0].indices[1] = _ij0[1];
11489 vinfos[0].maxsolutions = _nj0;
11490 vinfos[1].jointtype = 1;
11491 vinfos[1].foffset = j1;
11492 vinfos[1].indices[0] = _ij1[0];
11493 vinfos[1].indices[1] = _ij1[1];
11494 vinfos[1].maxsolutions = _nj1;
11495 vinfos[2].jointtype = 1;
11496 vinfos[2].foffset = j2;
11497 vinfos[2].indices[0] = _ij2[0];
11498 vinfos[2].indices[1] = _ij2[1];
11499 vinfos[2].maxsolutions = _nj2;
11500 vinfos[3].jointtype = 1;
11501 vinfos[3].foffset = j3;
11502 vinfos[3].indices[0] = _ij3[0];
11503 vinfos[3].indices[1] = _ij3[1];
11504 vinfos[3].maxsolutions = _nj3;
11505 vinfos[4].jointtype = 1;
11506 vinfos[4].foffset = j4;
11507 vinfos[4].indices[0] = _ij4[0];
11508 vinfos[4].indices[1] = _ij4[1];
11509 vinfos[4].maxsolutions = _nj4;
11510 vinfos[5].jointtype = 1;
11511 vinfos[5].foffset = j5;
11512 vinfos[5].indices[0] = _ij5[0];
11513 vinfos[5].indices[1] = _ij5[1];
11514 vinfos[5].maxsolutions = _nj5;
11515 std::vector<int> vfree(0);
11516 solutions.AddSolution(vinfos,vfree);
11517 }
11518 }
11519 }
11520 
11521 }
11522 
11523 }
11524 
11525 } else
11526 {
11527 {
11528 IkReal j3array[1], cj3array[1], sj3array[1];
11529 bool j3valid[1]={false};
11530 _nj3 = 1;
11531 CheckValue<IkReal> x874 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11532 if(!x874.valid){
11533 continue;
11534 }
11535 CheckValue<IkReal> x875=IKPowWithIntegerCheck(IKsign(gconst13),-1);
11536 if(!x875.valid){
11537 continue;
11538 }
11539 j3array[0]=((-1.5707963267949)+(x874.value)+(((1.5707963267949)*(x875.value))));
11540 sj3array[0]=IKsin(j3array[0]);
11541 cj3array[0]=IKcos(j3array[0]);
11542 if( j3array[0] > IKPI )
11543 {
11544  j3array[0]-=IK2PI;
11545 }
11546 else if( j3array[0] < -IKPI )
11547 { j3array[0]+=IK2PI;
11548 }
11549 j3valid[0] = true;
11550 for(int ij3 = 0; ij3 < 1; ++ij3)
11551 {
11552 if( !j3valid[ij3] )
11553 {
11554  continue;
11555 }
11556 _ij3[0] = ij3; _ij3[1] = -1;
11557 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11558 {
11559 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11560 {
11561  j3valid[iij3]=false; _ij3[1] = iij3; break;
11562 }
11563 }
11564 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11565 {
11566 IkReal evalcond[8];
11567 IkReal x876=IKsin(j3);
11568 IkReal x877=IKcos(j3);
11569 IkReal x878=((1.0)*x877);
11570 IkReal x879=(gconst13*x876);
11571 IkReal x880=(gconst13*x878);
11572 evalcond[0]=(new_r10*x876);
11573 evalcond[1]=x879;
11574 evalcond[2]=(gconst13+(((-1.0)*new_r10*x878)));
11575 evalcond[3]=((((-1.0)*x880))+new_r01);
11576 evalcond[4]=((((-1.0)*x879))+new_r11);
11577 evalcond[5]=((((-1.0)*x880))+new_r10);
11578 evalcond[6]=(((new_r01*x876))+(((-1.0)*new_r11*x878)));
11579 evalcond[7]=(((new_r01*x877))+((new_r11*x876))+(((-1.0)*gconst13)));
11580 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11581 {
11582 continue;
11583 }
11584 }
11585 
11586 {
11587 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11588 vinfos[0].jointtype = 1;
11589 vinfos[0].foffset = j0;
11590 vinfos[0].indices[0] = _ij0[0];
11591 vinfos[0].indices[1] = _ij0[1];
11592 vinfos[0].maxsolutions = _nj0;
11593 vinfos[1].jointtype = 1;
11594 vinfos[1].foffset = j1;
11595 vinfos[1].indices[0] = _ij1[0];
11596 vinfos[1].indices[1] = _ij1[1];
11597 vinfos[1].maxsolutions = _nj1;
11598 vinfos[2].jointtype = 1;
11599 vinfos[2].foffset = j2;
11600 vinfos[2].indices[0] = _ij2[0];
11601 vinfos[2].indices[1] = _ij2[1];
11602 vinfos[2].maxsolutions = _nj2;
11603 vinfos[3].jointtype = 1;
11604 vinfos[3].foffset = j3;
11605 vinfos[3].indices[0] = _ij3[0];
11606 vinfos[3].indices[1] = _ij3[1];
11607 vinfos[3].maxsolutions = _nj3;
11608 vinfos[4].jointtype = 1;
11609 vinfos[4].foffset = j4;
11610 vinfos[4].indices[0] = _ij4[0];
11611 vinfos[4].indices[1] = _ij4[1];
11612 vinfos[4].maxsolutions = _nj4;
11613 vinfos[5].jointtype = 1;
11614 vinfos[5].foffset = j5;
11615 vinfos[5].indices[0] = _ij5[0];
11616 vinfos[5].indices[1] = _ij5[1];
11617 vinfos[5].maxsolutions = _nj5;
11618 std::vector<int> vfree(0);
11619 solutions.AddSolution(vinfos,vfree);
11620 }
11621 }
11622 }
11623 
11624 }
11625 
11626 }
11627 
11628 } else
11629 {
11630 {
11631 IkReal j3array[1], cj3array[1], sj3array[1];
11632 bool j3valid[1]={false};
11633 _nj3 = 1;
11634 CheckValue<IkReal> x881 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
11635 if(!x881.valid){
11636 continue;
11637 }
11638 CheckValue<IkReal> x882=IKPowWithIntegerCheck(IKsign(gconst13),-1);
11639 if(!x882.valid){
11640 continue;
11641 }
11642 j3array[0]=((-1.5707963267949)+(x881.value)+(((1.5707963267949)*(x882.value))));
11643 sj3array[0]=IKsin(j3array[0]);
11644 cj3array[0]=IKcos(j3array[0]);
11645 if( j3array[0] > IKPI )
11646 {
11647  j3array[0]-=IK2PI;
11648 }
11649 else if( j3array[0] < -IKPI )
11650 { j3array[0]+=IK2PI;
11651 }
11652 j3valid[0] = true;
11653 for(int ij3 = 0; ij3 < 1; ++ij3)
11654 {
11655 if( !j3valid[ij3] )
11656 {
11657  continue;
11658 }
11659 _ij3[0] = ij3; _ij3[1] = -1;
11660 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11661 {
11662 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11663 {
11664  j3valid[iij3]=false; _ij3[1] = iij3; break;
11665 }
11666 }
11667 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11668 {
11669 IkReal evalcond[8];
11670 IkReal x883=IKsin(j3);
11671 IkReal x884=IKcos(j3);
11672 IkReal x885=((1.0)*x884);
11673 IkReal x886=(gconst13*x883);
11674 IkReal x887=(gconst13*x885);
11675 evalcond[0]=(new_r10*x883);
11676 evalcond[1]=x886;
11677 evalcond[2]=((((-1.0)*new_r10*x885))+gconst13);
11678 evalcond[3]=((((-1.0)*x887))+new_r01);
11679 evalcond[4]=((((-1.0)*x886))+new_r11);
11680 evalcond[5]=((((-1.0)*x887))+new_r10);
11681 evalcond[6]=(((new_r01*x883))+(((-1.0)*new_r11*x885)));
11682 evalcond[7]=(((new_r01*x884))+((new_r11*x883))+(((-1.0)*gconst13)));
11683 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11684 {
11685 continue;
11686 }
11687 }
11688 
11689 {
11690 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11691 vinfos[0].jointtype = 1;
11692 vinfos[0].foffset = j0;
11693 vinfos[0].indices[0] = _ij0[0];
11694 vinfos[0].indices[1] = _ij0[1];
11695 vinfos[0].maxsolutions = _nj0;
11696 vinfos[1].jointtype = 1;
11697 vinfos[1].foffset = j1;
11698 vinfos[1].indices[0] = _ij1[0];
11699 vinfos[1].indices[1] = _ij1[1];
11700 vinfos[1].maxsolutions = _nj1;
11701 vinfos[2].jointtype = 1;
11702 vinfos[2].foffset = j2;
11703 vinfos[2].indices[0] = _ij2[0];
11704 vinfos[2].indices[1] = _ij2[1];
11705 vinfos[2].maxsolutions = _nj2;
11706 vinfos[3].jointtype = 1;
11707 vinfos[3].foffset = j3;
11708 vinfos[3].indices[0] = _ij3[0];
11709 vinfos[3].indices[1] = _ij3[1];
11710 vinfos[3].maxsolutions = _nj3;
11711 vinfos[4].jointtype = 1;
11712 vinfos[4].foffset = j4;
11713 vinfos[4].indices[0] = _ij4[0];
11714 vinfos[4].indices[1] = _ij4[1];
11715 vinfos[4].maxsolutions = _nj4;
11716 vinfos[5].jointtype = 1;
11717 vinfos[5].foffset = j5;
11718 vinfos[5].indices[0] = _ij5[0];
11719 vinfos[5].indices[1] = _ij5[1];
11720 vinfos[5].maxsolutions = _nj5;
11721 std::vector<int> vfree(0);
11722 solutions.AddSolution(vinfos,vfree);
11723 }
11724 }
11725 }
11726 
11727 }
11728 
11729 }
11730 
11731 }
11732 } while(0);
11733 if( bgotonextstatement )
11734 {
11735 bool bgotonextstatement = true;
11736 do
11737 {
11738 if( 1 )
11739 {
11740 bgotonextstatement=false;
11741 continue; // branch miss [j3]
11742 
11743 }
11744 } while(0);
11745 if( bgotonextstatement )
11746 {
11747 }
11748 }
11749 }
11750 }
11751 }
11752 }
11753 
11754 } else
11755 {
11756 {
11757 IkReal j3array[1], cj3array[1], sj3array[1];
11758 bool j3valid[1]={false};
11759 _nj3 = 1;
11760 CheckValue<IkReal> x888 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r10)))),IkReal(((gconst14*gconst14)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
11761 if(!x888.valid){
11762 continue;
11763 }
11764 CheckValue<IkReal> x889=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r10))+(((-1.0)*gconst14*new_r00)))),-1);
11765 if(!x889.valid){
11766 continue;
11767 }
11768 j3array[0]=((-1.5707963267949)+(x888.value)+(((1.5707963267949)*(x889.value))));
11769 sj3array[0]=IKsin(j3array[0]);
11770 cj3array[0]=IKcos(j3array[0]);
11771 if( j3array[0] > IKPI )
11772 {
11773  j3array[0]-=IK2PI;
11774 }
11775 else if( j3array[0] < -IKPI )
11776 { j3array[0]+=IK2PI;
11777 }
11778 j3valid[0] = true;
11779 for(int ij3 = 0; ij3 < 1; ++ij3)
11780 {
11781 if( !j3valid[ij3] )
11782 {
11783  continue;
11784 }
11785 _ij3[0] = ij3; _ij3[1] = -1;
11786 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11787 {
11788 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11789 {
11790  j3valid[iij3]=false; _ij3[1] = iij3; break;
11791 }
11792 }
11793 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11794 {
11795 IkReal evalcond[8];
11796 IkReal x890=IKsin(j3);
11797 IkReal x891=IKcos(j3);
11798 IkReal x892=(gconst14*x890);
11799 IkReal x893=((1.0)*x891);
11800 IkReal x894=(gconst13*x890);
11801 IkReal x895=(gconst13*x893);
11802 evalcond[0]=(((new_r00*x891))+((new_r10*x890))+gconst14);
11803 evalcond[1]=(((gconst14*x891))+new_r00+x894);
11804 evalcond[2]=(((new_r00*x890))+(((-1.0)*new_r10*x893))+gconst13);
11805 evalcond[3]=(((new_r01*x890))+(((-1.0)*new_r11*x893))+gconst14);
11806 evalcond[4]=((((-1.0)*x895))+new_r01+x892);
11807 evalcond[5]=((((-1.0)*x895))+new_r10+x892);
11808 evalcond[6]=(((new_r11*x890))+((new_r01*x891))+(((-1.0)*gconst13)));
11809 evalcond[7]=((((-1.0)*x894))+new_r11+(((-1.0)*gconst14*x893)));
11810 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11811 {
11812 continue;
11813 }
11814 }
11815 
11816 {
11817 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11818 vinfos[0].jointtype = 1;
11819 vinfos[0].foffset = j0;
11820 vinfos[0].indices[0] = _ij0[0];
11821 vinfos[0].indices[1] = _ij0[1];
11822 vinfos[0].maxsolutions = _nj0;
11823 vinfos[1].jointtype = 1;
11824 vinfos[1].foffset = j1;
11825 vinfos[1].indices[0] = _ij1[0];
11826 vinfos[1].indices[1] = _ij1[1];
11827 vinfos[1].maxsolutions = _nj1;
11828 vinfos[2].jointtype = 1;
11829 vinfos[2].foffset = j2;
11830 vinfos[2].indices[0] = _ij2[0];
11831 vinfos[2].indices[1] = _ij2[1];
11832 vinfos[2].maxsolutions = _nj2;
11833 vinfos[3].jointtype = 1;
11834 vinfos[3].foffset = j3;
11835 vinfos[3].indices[0] = _ij3[0];
11836 vinfos[3].indices[1] = _ij3[1];
11837 vinfos[3].maxsolutions = _nj3;
11838 vinfos[4].jointtype = 1;
11839 vinfos[4].foffset = j4;
11840 vinfos[4].indices[0] = _ij4[0];
11841 vinfos[4].indices[1] = _ij4[1];
11842 vinfos[4].maxsolutions = _nj4;
11843 vinfos[5].jointtype = 1;
11844 vinfos[5].foffset = j5;
11845 vinfos[5].indices[0] = _ij5[0];
11846 vinfos[5].indices[1] = _ij5[1];
11847 vinfos[5].maxsolutions = _nj5;
11848 std::vector<int> vfree(0);
11849 solutions.AddSolution(vinfos,vfree);
11850 }
11851 }
11852 }
11853 
11854 }
11855 
11856 }
11857 
11858 } else
11859 {
11860 {
11861 IkReal j3array[1], cj3array[1], sj3array[1];
11862 bool j3valid[1]={false};
11863 _nj3 = 1;
11864 IkReal x896=((1.0)*new_r10);
11865 CheckValue<IkReal> x897 = IKatan2WithCheck(IkReal((((gconst13*gconst14))+((new_r00*new_r01)))),IkReal(((gconst14*gconst14)+(((-1.0)*new_r01*x896)))),IKFAST_ATAN2_MAGTHRESH);
11866 if(!x897.valid){
11867 continue;
11868 }
11869 CheckValue<IkReal> x898=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*x896))+(((-1.0)*gconst14*new_r00)))),-1);
11870 if(!x898.valid){
11871 continue;
11872 }
11873 j3array[0]=((-1.5707963267949)+(x897.value)+(((1.5707963267949)*(x898.value))));
11874 sj3array[0]=IKsin(j3array[0]);
11875 cj3array[0]=IKcos(j3array[0]);
11876 if( j3array[0] > IKPI )
11877 {
11878  j3array[0]-=IK2PI;
11879 }
11880 else if( j3array[0] < -IKPI )
11881 { j3array[0]+=IK2PI;
11882 }
11883 j3valid[0] = true;
11884 for(int ij3 = 0; ij3 < 1; ++ij3)
11885 {
11886 if( !j3valid[ij3] )
11887 {
11888  continue;
11889 }
11890 _ij3[0] = ij3; _ij3[1] = -1;
11891 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11892 {
11893 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11894 {
11895  j3valid[iij3]=false; _ij3[1] = iij3; break;
11896 }
11897 }
11898 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11899 {
11900 IkReal evalcond[8];
11901 IkReal x899=IKsin(j3);
11902 IkReal x900=IKcos(j3);
11903 IkReal x901=(gconst14*x899);
11904 IkReal x902=((1.0)*x900);
11905 IkReal x903=(gconst13*x899);
11906 IkReal x904=(gconst13*x902);
11907 evalcond[0]=(((new_r10*x899))+gconst14+((new_r00*x900)));
11908 evalcond[1]=(new_r00+x903+((gconst14*x900)));
11909 evalcond[2]=(((new_r00*x899))+gconst13+(((-1.0)*new_r10*x902)));
11910 evalcond[3]=((((-1.0)*new_r11*x902))+((new_r01*x899))+gconst14);
11911 evalcond[4]=((((-1.0)*x904))+new_r01+x901);
11912 evalcond[5]=((((-1.0)*x904))+new_r10+x901);
11913 evalcond[6]=(((new_r11*x899))+((new_r01*x900))+(((-1.0)*gconst13)));
11914 evalcond[7]=((((-1.0)*gconst14*x902))+(((-1.0)*x903))+new_r11);
11915 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11916 {
11917 continue;
11918 }
11919 }
11920 
11921 {
11922 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11923 vinfos[0].jointtype = 1;
11924 vinfos[0].foffset = j0;
11925 vinfos[0].indices[0] = _ij0[0];
11926 vinfos[0].indices[1] = _ij0[1];
11927 vinfos[0].maxsolutions = _nj0;
11928 vinfos[1].jointtype = 1;
11929 vinfos[1].foffset = j1;
11930 vinfos[1].indices[0] = _ij1[0];
11931 vinfos[1].indices[1] = _ij1[1];
11932 vinfos[1].maxsolutions = _nj1;
11933 vinfos[2].jointtype = 1;
11934 vinfos[2].foffset = j2;
11935 vinfos[2].indices[0] = _ij2[0];
11936 vinfos[2].indices[1] = _ij2[1];
11937 vinfos[2].maxsolutions = _nj2;
11938 vinfos[3].jointtype = 1;
11939 vinfos[3].foffset = j3;
11940 vinfos[3].indices[0] = _ij3[0];
11941 vinfos[3].indices[1] = _ij3[1];
11942 vinfos[3].maxsolutions = _nj3;
11943 vinfos[4].jointtype = 1;
11944 vinfos[4].foffset = j4;
11945 vinfos[4].indices[0] = _ij4[0];
11946 vinfos[4].indices[1] = _ij4[1];
11947 vinfos[4].maxsolutions = _nj4;
11948 vinfos[5].jointtype = 1;
11949 vinfos[5].foffset = j5;
11950 vinfos[5].indices[0] = _ij5[0];
11951 vinfos[5].indices[1] = _ij5[1];
11952 vinfos[5].maxsolutions = _nj5;
11953 std::vector<int> vfree(0);
11954 solutions.AddSolution(vinfos,vfree);
11955 }
11956 }
11957 }
11958 
11959 }
11960 
11961 }
11962 
11963 } else
11964 {
11965 {
11966 IkReal j3array[1], cj3array[1], sj3array[1];
11967 bool j3valid[1]={false};
11968 _nj3 = 1;
11969 IkReal x905=((1.0)*new_r10);
11970 CheckValue<IkReal> x906 = IKatan2WithCheck(IkReal((((gconst14*new_r00))+((gconst14*new_r11)))),IkReal(((((-1.0)*gconst14*x905))+((gconst14*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
11971 if(!x906.valid){
11972 continue;
11973 }
11974 CheckValue<IkReal> x907=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x905))+(((-1.0)*new_r00*new_r01)))),-1);
11975 if(!x907.valid){
11976 continue;
11977 }
11978 j3array[0]=((-1.5707963267949)+(x906.value)+(((1.5707963267949)*(x907.value))));
11979 sj3array[0]=IKsin(j3array[0]);
11980 cj3array[0]=IKcos(j3array[0]);
11981 if( j3array[0] > IKPI )
11982 {
11983  j3array[0]-=IK2PI;
11984 }
11985 else if( j3array[0] < -IKPI )
11986 { j3array[0]+=IK2PI;
11987 }
11988 j3valid[0] = true;
11989 for(int ij3 = 0; ij3 < 1; ++ij3)
11990 {
11991 if( !j3valid[ij3] )
11992 {
11993  continue;
11994 }
11995 _ij3[0] = ij3; _ij3[1] = -1;
11996 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11997 {
11998 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11999 {
12000  j3valid[iij3]=false; _ij3[1] = iij3; break;
12001 }
12002 }
12003 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12004 {
12005 IkReal evalcond[8];
12006 IkReal x908=IKsin(j3);
12007 IkReal x909=IKcos(j3);
12008 IkReal x910=(gconst14*x908);
12009 IkReal x911=((1.0)*x909);
12010 IkReal x912=(gconst13*x908);
12011 IkReal x913=(gconst13*x911);
12012 evalcond[0]=(gconst14+((new_r10*x908))+((new_r00*x909)));
12013 evalcond[1]=(new_r00+x912+((gconst14*x909)));
12014 evalcond[2]=(gconst13+((new_r00*x908))+(((-1.0)*new_r10*x911)));
12015 evalcond[3]=(gconst14+((new_r01*x908))+(((-1.0)*new_r11*x911)));
12016 evalcond[4]=((((-1.0)*x913))+new_r01+x910);
12017 evalcond[5]=((((-1.0)*x913))+new_r10+x910);
12018 evalcond[6]=(((new_r11*x908))+((new_r01*x909))+(((-1.0)*gconst13)));
12019 evalcond[7]=((((-1.0)*gconst14*x911))+(((-1.0)*x912))+new_r11);
12020 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12021 {
12022 continue;
12023 }
12024 }
12025 
12026 {
12027 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12028 vinfos[0].jointtype = 1;
12029 vinfos[0].foffset = j0;
12030 vinfos[0].indices[0] = _ij0[0];
12031 vinfos[0].indices[1] = _ij0[1];
12032 vinfos[0].maxsolutions = _nj0;
12033 vinfos[1].jointtype = 1;
12034 vinfos[1].foffset = j1;
12035 vinfos[1].indices[0] = _ij1[0];
12036 vinfos[1].indices[1] = _ij1[1];
12037 vinfos[1].maxsolutions = _nj1;
12038 vinfos[2].jointtype = 1;
12039 vinfos[2].foffset = j2;
12040 vinfos[2].indices[0] = _ij2[0];
12041 vinfos[2].indices[1] = _ij2[1];
12042 vinfos[2].maxsolutions = _nj2;
12043 vinfos[3].jointtype = 1;
12044 vinfos[3].foffset = j3;
12045 vinfos[3].indices[0] = _ij3[0];
12046 vinfos[3].indices[1] = _ij3[1];
12047 vinfos[3].maxsolutions = _nj3;
12048 vinfos[4].jointtype = 1;
12049 vinfos[4].foffset = j4;
12050 vinfos[4].indices[0] = _ij4[0];
12051 vinfos[4].indices[1] = _ij4[1];
12052 vinfos[4].maxsolutions = _nj4;
12053 vinfos[5].jointtype = 1;
12054 vinfos[5].foffset = j5;
12055 vinfos[5].indices[0] = _ij5[0];
12056 vinfos[5].indices[1] = _ij5[1];
12057 vinfos[5].maxsolutions = _nj5;
12058 std::vector<int> vfree(0);
12059 solutions.AddSolution(vinfos,vfree);
12060 }
12061 }
12062 }
12063 
12064 }
12065 
12066 }
12067 
12068 }
12069 } while(0);
12070 if( bgotonextstatement )
12071 {
12072 bool bgotonextstatement = true;
12073 do
12074 {
12075 IkReal x916 = ((new_r10*new_r10)+(new_r00*new_r00));
12076 if(IKabs(x916)==0){
12077 continue;
12078 }
12079 IkReal x914=pow(x916,-0.5);
12080 IkReal x915=((1.0)*x914);
12081 CheckValue<IkReal> x917 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12082 if(!x917.valid){
12083 continue;
12084 }
12085 IkReal gconst15=((3.14159265358979)+(((-1.0)*(x917.value))));
12086 IkReal gconst16=(new_r10*x915);
12087 IkReal gconst17=(new_r00*x915);
12088 CheckValue<IkReal> x918 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12089 if(!x918.valid){
12090 continue;
12091 }
12092 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5+(x918.value))))), 6.28318530717959)));
12093 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12094 {
12095 bgotonextstatement=false;
12096 {
12097 IkReal j3eval[3];
12098 CheckValue<IkReal> x922 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12099 if(!x922.valid){
12100 continue;
12101 }
12102 IkReal x919=((1.0)*(x922.value));
12103 IkReal x920=x914;
12104 IkReal x921=((1.0)*x920);
12105 sj4=0;
12106 cj4=-1.0;
12107 j4=3.14159265358979;
12108 sj5=gconst16;
12109 cj5=gconst17;
12110 j5=((3.14159265)+(((-1.0)*x919)));
12111 IkReal gconst15=((3.14159265358979)+(((-1.0)*x919)));
12112 IkReal gconst16=(new_r10*x921);
12113 IkReal gconst17=(new_r00*x921);
12114 IkReal x923=new_r00*new_r00;
12115 IkReal x924=((1.0)*new_r00);
12116 IkReal x925=((((-1.0)*new_r01*x924))+(((-1.0)*new_r10*new_r11)));
12117 IkReal x926=x914;
12118 IkReal x927=(new_r00*x926);
12119 j3eval[0]=x925;
12120 j3eval[1]=((IKabs((((x923*x926))+((new_r11*x927)))))+(IKabs((((new_r01*x927))+(((-1.0)*new_r10*x924*x926))))));
12121 j3eval[2]=IKsign(x925);
12122 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12123 {
12124 {
12125 IkReal j3eval[1];
12126 CheckValue<IkReal> x931 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12127 if(!x931.valid){
12128 continue;
12129 }
12130 IkReal x928=((1.0)*(x931.value));
12131 IkReal x929=x914;
12132 IkReal x930=((1.0)*x929);
12133 sj4=0;
12134 cj4=-1.0;
12135 j4=3.14159265358979;
12136 sj5=gconst16;
12137 cj5=gconst17;
12138 j5=((3.14159265)+(((-1.0)*x928)));
12139 IkReal gconst15=((3.14159265358979)+(((-1.0)*x928)));
12140 IkReal gconst16=(new_r10*x930);
12141 IkReal gconst17=(new_r00*x930);
12142 IkReal x932=new_r10*new_r10;
12143 IkReal x933=new_r00*new_r00;
12144 IkReal x934=((1.0)*new_r01);
12145 CheckValue<IkReal> x938=IKPowWithIntegerCheck((x933+x932),-1);
12146 if(!x938.valid){
12147 continue;
12148 }
12149 IkReal x935=x938.value;
12150 IkReal x936=(new_r10*x935);
12151 IkReal x937=(new_r01*x935);
12152 j3eval[0]=((IKabs(((((-1.0)*x934*x936*(new_r10*new_r10)))+(((-1.0)*x933*x934*x936))+((x933*x935)))))+(IKabs((((x937*(new_r00*new_r00*new_r00)))+((new_r00*x936))+((new_r00*x932*x937))))));
12153 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12154 {
12155 {
12156 IkReal j3eval[1];
12157 CheckValue<IkReal> x942 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12158 if(!x942.valid){
12159 continue;
12160 }
12161 IkReal x939=((1.0)*(x942.value));
12162 IkReal x940=x914;
12163 IkReal x941=((1.0)*x940);
12164 sj4=0;
12165 cj4=-1.0;
12166 j4=3.14159265358979;
12167 sj5=gconst16;
12168 cj5=gconst17;
12169 j5=((3.14159265)+(((-1.0)*x939)));
12170 IkReal gconst15=((3.14159265358979)+(((-1.0)*x939)));
12171 IkReal gconst16=(new_r10*x941);
12172 IkReal gconst17=(new_r00*x941);
12173 IkReal x943=new_r00*new_r00;
12174 IkReal x944=new_r10*new_r10;
12175 CheckValue<IkReal> x948=IKPowWithIntegerCheck((x943+x944),-1);
12176 if(!x948.valid){
12177 continue;
12178 }
12179 IkReal x945=x948.value;
12180 IkReal x946=(new_r10*x945);
12181 IkReal x947=((1.0)*x945);
12182 j3eval[0]=((IKabs(((((-1.0)*x947*(x944*x944)))+((x943*x945))+(((-1.0)*x943*x944*x947)))))+(IKabs((((new_r00*x946))+((new_r00*x946*(new_r10*new_r10)))+((x946*(new_r00*new_r00*new_r00)))))));
12183 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12184 {
12185 {
12186 IkReal evalcond[3];
12187 bool bgotonextstatement = true;
12188 do
12189 {
12190 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12191 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12192 {
12193 bgotonextstatement=false;
12194 {
12195 IkReal j3eval[1];
12196 CheckValue<IkReal> x950 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12197 if(!x950.valid){
12198 continue;
12199 }
12200 IkReal x949=((1.0)*(x950.value));
12201 sj4=0;
12202 cj4=-1.0;
12203 j4=3.14159265358979;
12204 sj5=gconst16;
12205 cj5=gconst17;
12206 j5=((3.14159265)+(((-1.0)*x949)));
12207 new_r11=0;
12208 new_r00=0;
12209 IkReal gconst15=((3.14159265358979)+(((-1.0)*x949)));
12210 IkReal x951 = new_r10*new_r10;
12211 if(IKabs(x951)==0){
12212 continue;
12213 }
12214 IkReal gconst16=((1.0)*new_r10*(pow(x951,-0.5)));
12215 IkReal gconst17=0;
12216 j3eval[0]=new_r10;
12217 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12218 {
12219 {
12220 IkReal j3array[2], cj3array[2], sj3array[2];
12221 bool j3valid[2]={false};
12222 _nj3 = 2;
12223 CheckValue<IkReal> x952=IKPowWithIntegerCheck(gconst16,-1);
12224 if(!x952.valid){
12225 continue;
12226 }
12227 cj3array[0]=(new_r01*(x952.value));
12228 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12229 {
12230  j3valid[0] = j3valid[1] = true;
12231  j3array[0] = IKacos(cj3array[0]);
12232  sj3array[0] = IKsin(j3array[0]);
12233  cj3array[1] = cj3array[0];
12234  j3array[1] = -j3array[0];
12235  sj3array[1] = -sj3array[0];
12236 }
12237 else if( isnan(cj3array[0]) )
12238 {
12239  // probably any value will work
12240  j3valid[0] = true;
12241  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12242 }
12243 for(int ij3 = 0; ij3 < 2; ++ij3)
12244 {
12245 if( !j3valid[ij3] )
12246 {
12247  continue;
12248 }
12249 _ij3[0] = ij3; _ij3[1] = -1;
12250 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12251 {
12252 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12253 {
12254  j3valid[iij3]=false; _ij3[1] = iij3; break;
12255 }
12256 }
12257 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12258 {
12259 IkReal evalcond[6];
12260 IkReal x953=IKsin(j3);
12261 IkReal x954=IKcos(j3);
12262 IkReal x955=((1.0)*gconst16);
12263 evalcond[0]=(new_r01*x953);
12264 evalcond[1]=(new_r10*x953);
12265 evalcond[2]=(gconst16*x953);
12266 evalcond[3]=(gconst16+(((-1.0)*new_r10*x954)));
12267 evalcond[4]=(new_r10+(((-1.0)*x954*x955)));
12268 evalcond[5]=(((new_r01*x954))+(((-1.0)*x955)));
12269 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12270 {
12271 continue;
12272 }
12273 }
12274 
12275 {
12276 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12277 vinfos[0].jointtype = 1;
12278 vinfos[0].foffset = j0;
12279 vinfos[0].indices[0] = _ij0[0];
12280 vinfos[0].indices[1] = _ij0[1];
12281 vinfos[0].maxsolutions = _nj0;
12282 vinfos[1].jointtype = 1;
12283 vinfos[1].foffset = j1;
12284 vinfos[1].indices[0] = _ij1[0];
12285 vinfos[1].indices[1] = _ij1[1];
12286 vinfos[1].maxsolutions = _nj1;
12287 vinfos[2].jointtype = 1;
12288 vinfos[2].foffset = j2;
12289 vinfos[2].indices[0] = _ij2[0];
12290 vinfos[2].indices[1] = _ij2[1];
12291 vinfos[2].maxsolutions = _nj2;
12292 vinfos[3].jointtype = 1;
12293 vinfos[3].foffset = j3;
12294 vinfos[3].indices[0] = _ij3[0];
12295 vinfos[3].indices[1] = _ij3[1];
12296 vinfos[3].maxsolutions = _nj3;
12297 vinfos[4].jointtype = 1;
12298 vinfos[4].foffset = j4;
12299 vinfos[4].indices[0] = _ij4[0];
12300 vinfos[4].indices[1] = _ij4[1];
12301 vinfos[4].maxsolutions = _nj4;
12302 vinfos[5].jointtype = 1;
12303 vinfos[5].foffset = j5;
12304 vinfos[5].indices[0] = _ij5[0];
12305 vinfos[5].indices[1] = _ij5[1];
12306 vinfos[5].maxsolutions = _nj5;
12307 std::vector<int> vfree(0);
12308 solutions.AddSolution(vinfos,vfree);
12309 }
12310 }
12311 }
12312 
12313 } else
12314 {
12315 {
12316 IkReal j3array[2], cj3array[2], sj3array[2];
12317 bool j3valid[2]={false};
12318 _nj3 = 2;
12319 CheckValue<IkReal> x956=IKPowWithIntegerCheck(new_r10,-1);
12320 if(!x956.valid){
12321 continue;
12322 }
12323 cj3array[0]=(gconst16*(x956.value));
12324 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12325 {
12326  j3valid[0] = j3valid[1] = true;
12327  j3array[0] = IKacos(cj3array[0]);
12328  sj3array[0] = IKsin(j3array[0]);
12329  cj3array[1] = cj3array[0];
12330  j3array[1] = -j3array[0];
12331  sj3array[1] = -sj3array[0];
12332 }
12333 else if( isnan(cj3array[0]) )
12334 {
12335  // probably any value will work
12336  j3valid[0] = true;
12337  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12338 }
12339 for(int ij3 = 0; ij3 < 2; ++ij3)
12340 {
12341 if( !j3valid[ij3] )
12342 {
12343  continue;
12344 }
12345 _ij3[0] = ij3; _ij3[1] = -1;
12346 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12347 {
12348 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12349 {
12350  j3valid[iij3]=false; _ij3[1] = iij3; break;
12351 }
12352 }
12353 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12354 {
12355 IkReal evalcond[6];
12356 IkReal x957=IKsin(j3);
12357 IkReal x958=IKcos(j3);
12358 IkReal x959=((1.0)*gconst16);
12359 IkReal x960=(x958*x959);
12360 evalcond[0]=(new_r01*x957);
12361 evalcond[1]=(new_r10*x957);
12362 evalcond[2]=(gconst16*x957);
12363 evalcond[3]=(new_r01+(((-1.0)*x960)));
12364 evalcond[4]=(new_r10+(((-1.0)*x960)));
12365 evalcond[5]=(((new_r01*x958))+(((-1.0)*x959)));
12366 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12367 {
12368 continue;
12369 }
12370 }
12371 
12372 {
12373 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12374 vinfos[0].jointtype = 1;
12375 vinfos[0].foffset = j0;
12376 vinfos[0].indices[0] = _ij0[0];
12377 vinfos[0].indices[1] = _ij0[1];
12378 vinfos[0].maxsolutions = _nj0;
12379 vinfos[1].jointtype = 1;
12380 vinfos[1].foffset = j1;
12381 vinfos[1].indices[0] = _ij1[0];
12382 vinfos[1].indices[1] = _ij1[1];
12383 vinfos[1].maxsolutions = _nj1;
12384 vinfos[2].jointtype = 1;
12385 vinfos[2].foffset = j2;
12386 vinfos[2].indices[0] = _ij2[0];
12387 vinfos[2].indices[1] = _ij2[1];
12388 vinfos[2].maxsolutions = _nj2;
12389 vinfos[3].jointtype = 1;
12390 vinfos[3].foffset = j3;
12391 vinfos[3].indices[0] = _ij3[0];
12392 vinfos[3].indices[1] = _ij3[1];
12393 vinfos[3].maxsolutions = _nj3;
12394 vinfos[4].jointtype = 1;
12395 vinfos[4].foffset = j4;
12396 vinfos[4].indices[0] = _ij4[0];
12397 vinfos[4].indices[1] = _ij4[1];
12398 vinfos[4].maxsolutions = _nj4;
12399 vinfos[5].jointtype = 1;
12400 vinfos[5].foffset = j5;
12401 vinfos[5].indices[0] = _ij5[0];
12402 vinfos[5].indices[1] = _ij5[1];
12403 vinfos[5].maxsolutions = _nj5;
12404 std::vector<int> vfree(0);
12405 solutions.AddSolution(vinfos,vfree);
12406 }
12407 }
12408 }
12409 
12410 }
12411 
12412 }
12413 
12414 }
12415 } while(0);
12416 if( bgotonextstatement )
12417 {
12418 bool bgotonextstatement = true;
12419 do
12420 {
12421 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
12422 evalcond[1]=gconst17;
12423 evalcond[2]=gconst16;
12424 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
12425 {
12426 bgotonextstatement=false;
12427 {
12428 IkReal j3eval[3];
12429 CheckValue<IkReal> x962 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12430 if(!x962.valid){
12431 continue;
12432 }
12433 IkReal x961=((1.0)*(x962.value));
12434 sj4=0;
12435 cj4=-1.0;
12436 j4=3.14159265358979;
12437 sj5=gconst16;
12438 cj5=gconst17;
12439 j5=((3.14159265)+(((-1.0)*x961)));
12440 new_r11=0;
12441 new_r01=0;
12442 new_r22=0;
12443 new_r20=0;
12444 IkReal gconst15=((3.14159265358979)+(((-1.0)*x961)));
12445 IkReal gconst16=((1.0)*new_r10);
12446 IkReal gconst17=((1.0)*new_r00);
12447 j3eval[0]=-1.0;
12448 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12449 j3eval[2]=-1.0;
12450 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12451 {
12452 {
12453 IkReal j3eval[3];
12454 CheckValue<IkReal> x964 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12455 if(!x964.valid){
12456 continue;
12457 }
12458 IkReal x963=((1.0)*(x964.value));
12459 sj4=0;
12460 cj4=-1.0;
12461 j4=3.14159265358979;
12462 sj5=gconst16;
12463 cj5=gconst17;
12464 j5=((3.14159265)+(((-1.0)*x963)));
12465 new_r11=0;
12466 new_r01=0;
12467 new_r22=0;
12468 new_r20=0;
12469 IkReal gconst15=((3.14159265358979)+(((-1.0)*x963)));
12470 IkReal gconst16=((1.0)*new_r10);
12471 IkReal gconst17=((1.0)*new_r00);
12472 j3eval[0]=-1.0;
12473 j3eval[1]=-1.0;
12474 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12475 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12476 {
12477 {
12478 IkReal j3eval[3];
12479 CheckValue<IkReal> x966 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12480 if(!x966.valid){
12481 continue;
12482 }
12483 IkReal x965=((1.0)*(x966.value));
12484 sj4=0;
12485 cj4=-1.0;
12486 j4=3.14159265358979;
12487 sj5=gconst16;
12488 cj5=gconst17;
12489 j5=((3.14159265)+(((-1.0)*x965)));
12490 new_r11=0;
12491 new_r01=0;
12492 new_r22=0;
12493 new_r20=0;
12494 IkReal gconst15=((3.14159265358979)+(((-1.0)*x965)));
12495 IkReal gconst16=((1.0)*new_r10);
12496 IkReal gconst17=((1.0)*new_r00);
12497 j3eval[0]=-1.0;
12498 j3eval[1]=((IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
12499 j3eval[2]=-1.0;
12500 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12501 {
12502 continue; // 3 cases reached
12503 
12504 } else
12505 {
12506 {
12507 IkReal j3array[1], cj3array[1], sj3array[1];
12508 bool j3valid[1]={false};
12509 _nj3 = 1;
12510 CheckValue<IkReal> x967 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst16*gconst17)))),IkReal(((gconst17*gconst17)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
12511 if(!x967.valid){
12512 continue;
12513 }
12514 CheckValue<IkReal> x968=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
12515 if(!x968.valid){
12516 continue;
12517 }
12518 j3array[0]=((-1.5707963267949)+(x967.value)+(((1.5707963267949)*(x968.value))));
12519 sj3array[0]=IKsin(j3array[0]);
12520 cj3array[0]=IKcos(j3array[0]);
12521 if( j3array[0] > IKPI )
12522 {
12523  j3array[0]-=IK2PI;
12524 }
12525 else if( j3array[0] < -IKPI )
12526 { j3array[0]+=IK2PI;
12527 }
12528 j3valid[0] = true;
12529 for(int ij3 = 0; ij3 < 1; ++ij3)
12530 {
12531 if( !j3valid[ij3] )
12532 {
12533  continue;
12534 }
12535 _ij3[0] = ij3; _ij3[1] = -1;
12536 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12537 {
12538 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12539 {
12540  j3valid[iij3]=false; _ij3[1] = iij3; break;
12541 }
12542 }
12543 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12544 {
12545 IkReal evalcond[6];
12546 IkReal x969=IKsin(j3);
12547 IkReal x970=IKcos(j3);
12548 IkReal x971=(gconst17*x969);
12549 IkReal x972=(gconst16*x969);
12550 IkReal x973=(gconst17*x970);
12551 IkReal x974=((1.0)*x970);
12552 IkReal x975=(gconst16*x974);
12553 evalcond[0]=(x971+(((-1.0)*x975)));
12554 evalcond[1]=(gconst17+((new_r10*x969))+((new_r00*x970)));
12555 evalcond[2]=(new_r00+x973+x972);
12556 evalcond[3]=(((new_r00*x969))+gconst16+(((-1.0)*new_r10*x974)));
12557 evalcond[4]=((((-1.0)*x972))+(((-1.0)*x973)));
12558 evalcond[5]=(new_r10+x971+(((-1.0)*x975)));
12559 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12560 {
12561 continue;
12562 }
12563 }
12564 
12565 {
12566 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12567 vinfos[0].jointtype = 1;
12568 vinfos[0].foffset = j0;
12569 vinfos[0].indices[0] = _ij0[0];
12570 vinfos[0].indices[1] = _ij0[1];
12571 vinfos[0].maxsolutions = _nj0;
12572 vinfos[1].jointtype = 1;
12573 vinfos[1].foffset = j1;
12574 vinfos[1].indices[0] = _ij1[0];
12575 vinfos[1].indices[1] = _ij1[1];
12576 vinfos[1].maxsolutions = _nj1;
12577 vinfos[2].jointtype = 1;
12578 vinfos[2].foffset = j2;
12579 vinfos[2].indices[0] = _ij2[0];
12580 vinfos[2].indices[1] = _ij2[1];
12581 vinfos[2].maxsolutions = _nj2;
12582 vinfos[3].jointtype = 1;
12583 vinfos[3].foffset = j3;
12584 vinfos[3].indices[0] = _ij3[0];
12585 vinfos[3].indices[1] = _ij3[1];
12586 vinfos[3].maxsolutions = _nj3;
12587 vinfos[4].jointtype = 1;
12588 vinfos[4].foffset = j4;
12589 vinfos[4].indices[0] = _ij4[0];
12590 vinfos[4].indices[1] = _ij4[1];
12591 vinfos[4].maxsolutions = _nj4;
12592 vinfos[5].jointtype = 1;
12593 vinfos[5].foffset = j5;
12594 vinfos[5].indices[0] = _ij5[0];
12595 vinfos[5].indices[1] = _ij5[1];
12596 vinfos[5].maxsolutions = _nj5;
12597 std::vector<int> vfree(0);
12598 solutions.AddSolution(vinfos,vfree);
12599 }
12600 }
12601 }
12602 
12603 }
12604 
12605 }
12606 
12607 } else
12608 {
12609 {
12610 IkReal j3array[1], cj3array[1], sj3array[1];
12611 bool j3valid[1]={false};
12612 _nj3 = 1;
12613 CheckValue<IkReal> x976=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst17*gconst17)))+(((-1.0)*(gconst16*gconst16))))),-1);
12614 if(!x976.valid){
12615 continue;
12616 }
12617 CheckValue<IkReal> x977 = IKatan2WithCheck(IkReal((gconst16*new_r00)),IkReal((gconst17*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12618 if(!x977.valid){
12619 continue;
12620 }
12621 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x976.value)))+(x977.value));
12622 sj3array[0]=IKsin(j3array[0]);
12623 cj3array[0]=IKcos(j3array[0]);
12624 if( j3array[0] > IKPI )
12625 {
12626  j3array[0]-=IK2PI;
12627 }
12628 else if( j3array[0] < -IKPI )
12629 { j3array[0]+=IK2PI;
12630 }
12631 j3valid[0] = true;
12632 for(int ij3 = 0; ij3 < 1; ++ij3)
12633 {
12634 if( !j3valid[ij3] )
12635 {
12636  continue;
12637 }
12638 _ij3[0] = ij3; _ij3[1] = -1;
12639 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12640 {
12641 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12642 {
12643  j3valid[iij3]=false; _ij3[1] = iij3; break;
12644 }
12645 }
12646 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12647 {
12648 IkReal evalcond[6];
12649 IkReal x978=IKsin(j3);
12650 IkReal x979=IKcos(j3);
12651 IkReal x980=(gconst17*x978);
12652 IkReal x981=(gconst16*x978);
12653 IkReal x982=(gconst17*x979);
12654 IkReal x983=((1.0)*x979);
12655 IkReal x984=(gconst16*x983);
12656 evalcond[0]=((((-1.0)*x984))+x980);
12657 evalcond[1]=(gconst17+((new_r10*x978))+((new_r00*x979)));
12658 evalcond[2]=(new_r00+x981+x982);
12659 evalcond[3]=(gconst16+(((-1.0)*new_r10*x983))+((new_r00*x978)));
12660 evalcond[4]=((((-1.0)*x981))+(((-1.0)*x982)));
12661 evalcond[5]=((((-1.0)*x984))+new_r10+x980);
12662 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12663 {
12664 continue;
12665 }
12666 }
12667 
12668 {
12669 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12670 vinfos[0].jointtype = 1;
12671 vinfos[0].foffset = j0;
12672 vinfos[0].indices[0] = _ij0[0];
12673 vinfos[0].indices[1] = _ij0[1];
12674 vinfos[0].maxsolutions = _nj0;
12675 vinfos[1].jointtype = 1;
12676 vinfos[1].foffset = j1;
12677 vinfos[1].indices[0] = _ij1[0];
12678 vinfos[1].indices[1] = _ij1[1];
12679 vinfos[1].maxsolutions = _nj1;
12680 vinfos[2].jointtype = 1;
12681 vinfos[2].foffset = j2;
12682 vinfos[2].indices[0] = _ij2[0];
12683 vinfos[2].indices[1] = _ij2[1];
12684 vinfos[2].maxsolutions = _nj2;
12685 vinfos[3].jointtype = 1;
12686 vinfos[3].foffset = j3;
12687 vinfos[3].indices[0] = _ij3[0];
12688 vinfos[3].indices[1] = _ij3[1];
12689 vinfos[3].maxsolutions = _nj3;
12690 vinfos[4].jointtype = 1;
12691 vinfos[4].foffset = j4;
12692 vinfos[4].indices[0] = _ij4[0];
12693 vinfos[4].indices[1] = _ij4[1];
12694 vinfos[4].maxsolutions = _nj4;
12695 vinfos[5].jointtype = 1;
12696 vinfos[5].foffset = j5;
12697 vinfos[5].indices[0] = _ij5[0];
12698 vinfos[5].indices[1] = _ij5[1];
12699 vinfos[5].maxsolutions = _nj5;
12700 std::vector<int> vfree(0);
12701 solutions.AddSolution(vinfos,vfree);
12702 }
12703 }
12704 }
12705 
12706 }
12707 
12708 }
12709 
12710 } else
12711 {
12712 {
12713 IkReal j3array[1], cj3array[1], sj3array[1];
12714 bool j3valid[1]={false};
12715 _nj3 = 1;
12716 CheckValue<IkReal> x985 = IKatan2WithCheck(IkReal((gconst16*gconst17)),IkReal(gconst17*gconst17),IKFAST_ATAN2_MAGTHRESH);
12717 if(!x985.valid){
12718 continue;
12719 }
12720 CheckValue<IkReal> x986=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
12721 if(!x986.valid){
12722 continue;
12723 }
12724 j3array[0]=((-1.5707963267949)+(x985.value)+(((1.5707963267949)*(x986.value))));
12725 sj3array[0]=IKsin(j3array[0]);
12726 cj3array[0]=IKcos(j3array[0]);
12727 if( j3array[0] > IKPI )
12728 {
12729  j3array[0]-=IK2PI;
12730 }
12731 else if( j3array[0] < -IKPI )
12732 { j3array[0]+=IK2PI;
12733 }
12734 j3valid[0] = true;
12735 for(int ij3 = 0; ij3 < 1; ++ij3)
12736 {
12737 if( !j3valid[ij3] )
12738 {
12739  continue;
12740 }
12741 _ij3[0] = ij3; _ij3[1] = -1;
12742 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12743 {
12744 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12745 {
12746  j3valid[iij3]=false; _ij3[1] = iij3; break;
12747 }
12748 }
12749 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12750 {
12751 IkReal evalcond[6];
12752 IkReal x987=IKsin(j3);
12753 IkReal x988=IKcos(j3);
12754 IkReal x989=(gconst17*x987);
12755 IkReal x990=(gconst16*x987);
12756 IkReal x991=(gconst17*x988);
12757 IkReal x992=((1.0)*x988);
12758 IkReal x993=(gconst16*x992);
12759 evalcond[0]=((((-1.0)*x993))+x989);
12760 evalcond[1]=(((new_r00*x988))+gconst17+((new_r10*x987)));
12761 evalcond[2]=(new_r00+x991+x990);
12762 evalcond[3]=(((new_r00*x987))+(((-1.0)*new_r10*x992))+gconst16);
12763 evalcond[4]=((((-1.0)*x991))+(((-1.0)*x990)));
12764 evalcond[5]=((((-1.0)*x993))+new_r10+x989);
12765 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12766 {
12767 continue;
12768 }
12769 }
12770 
12771 {
12772 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12773 vinfos[0].jointtype = 1;
12774 vinfos[0].foffset = j0;
12775 vinfos[0].indices[0] = _ij0[0];
12776 vinfos[0].indices[1] = _ij0[1];
12777 vinfos[0].maxsolutions = _nj0;
12778 vinfos[1].jointtype = 1;
12779 vinfos[1].foffset = j1;
12780 vinfos[1].indices[0] = _ij1[0];
12781 vinfos[1].indices[1] = _ij1[1];
12782 vinfos[1].maxsolutions = _nj1;
12783 vinfos[2].jointtype = 1;
12784 vinfos[2].foffset = j2;
12785 vinfos[2].indices[0] = _ij2[0];
12786 vinfos[2].indices[1] = _ij2[1];
12787 vinfos[2].maxsolutions = _nj2;
12788 vinfos[3].jointtype = 1;
12789 vinfos[3].foffset = j3;
12790 vinfos[3].indices[0] = _ij3[0];
12791 vinfos[3].indices[1] = _ij3[1];
12792 vinfos[3].maxsolutions = _nj3;
12793 vinfos[4].jointtype = 1;
12794 vinfos[4].foffset = j4;
12795 vinfos[4].indices[0] = _ij4[0];
12796 vinfos[4].indices[1] = _ij4[1];
12797 vinfos[4].maxsolutions = _nj4;
12798 vinfos[5].jointtype = 1;
12799 vinfos[5].foffset = j5;
12800 vinfos[5].indices[0] = _ij5[0];
12801 vinfos[5].indices[1] = _ij5[1];
12802 vinfos[5].maxsolutions = _nj5;
12803 std::vector<int> vfree(0);
12804 solutions.AddSolution(vinfos,vfree);
12805 }
12806 }
12807 }
12808 
12809 }
12810 
12811 }
12812 
12813 }
12814 } while(0);
12815 if( bgotonextstatement )
12816 {
12817 bool bgotonextstatement = true;
12818 do
12819 {
12820 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12821 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12822 {
12823 bgotonextstatement=false;
12824 {
12825 IkReal j3eval[1];
12826 CheckValue<IkReal> x995 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12827 if(!x995.valid){
12828 continue;
12829 }
12830 IkReal x994=((1.0)*(x995.value));
12831 sj4=0;
12832 cj4=-1.0;
12833 j4=3.14159265358979;
12834 sj5=gconst16;
12835 cj5=gconst17;
12836 j5=((3.14159265)+(((-1.0)*x994)));
12837 new_r01=0;
12838 new_r10=0;
12839 IkReal gconst15=((3.14159265358979)+(((-1.0)*x994)));
12840 IkReal gconst16=0;
12841 IkReal x996 = new_r00*new_r00;
12842 if(IKabs(x996)==0){
12843 continue;
12844 }
12845 IkReal gconst17=((1.0)*new_r00*(pow(x996,-0.5)));
12846 j3eval[0]=new_r11;
12847 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12848 {
12849 {
12850 IkReal j3array[2], cj3array[2], sj3array[2];
12851 bool j3valid[2]={false};
12852 _nj3 = 2;
12853 CheckValue<IkReal> x997=IKPowWithIntegerCheck(gconst17,-1);
12854 if(!x997.valid){
12855 continue;
12856 }
12857 cj3array[0]=(new_r11*(x997.value));
12858 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12859 {
12860  j3valid[0] = j3valid[1] = true;
12861  j3array[0] = IKacos(cj3array[0]);
12862  sj3array[0] = IKsin(j3array[0]);
12863  cj3array[1] = cj3array[0];
12864  j3array[1] = -j3array[0];
12865  sj3array[1] = -sj3array[0];
12866 }
12867 else if( isnan(cj3array[0]) )
12868 {
12869  // probably any value will work
12870  j3valid[0] = true;
12871  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12872 }
12873 for(int ij3 = 0; ij3 < 2; ++ij3)
12874 {
12875 if( !j3valid[ij3] )
12876 {
12877  continue;
12878 }
12879 _ij3[0] = ij3; _ij3[1] = -1;
12880 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12881 {
12882 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12883 {
12884  j3valid[iij3]=false; _ij3[1] = iij3; break;
12885 }
12886 }
12887 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12888 {
12889 IkReal evalcond[6];
12890 IkReal x998=IKsin(j3);
12891 IkReal x999=IKcos(j3);
12892 evalcond[0]=(new_r00*x998);
12893 evalcond[1]=(new_r11*x998);
12894 evalcond[2]=(gconst17*x998);
12895 evalcond[3]=(gconst17+((new_r00*x999)));
12896 evalcond[4]=(new_r00+((gconst17*x999)));
12897 evalcond[5]=(gconst17+(((-1.0)*new_r11*x999)));
12898 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12899 {
12900 continue;
12901 }
12902 }
12903 
12904 {
12905 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12906 vinfos[0].jointtype = 1;
12907 vinfos[0].foffset = j0;
12908 vinfos[0].indices[0] = _ij0[0];
12909 vinfos[0].indices[1] = _ij0[1];
12910 vinfos[0].maxsolutions = _nj0;
12911 vinfos[1].jointtype = 1;
12912 vinfos[1].foffset = j1;
12913 vinfos[1].indices[0] = _ij1[0];
12914 vinfos[1].indices[1] = _ij1[1];
12915 vinfos[1].maxsolutions = _nj1;
12916 vinfos[2].jointtype = 1;
12917 vinfos[2].foffset = j2;
12918 vinfos[2].indices[0] = _ij2[0];
12919 vinfos[2].indices[1] = _ij2[1];
12920 vinfos[2].maxsolutions = _nj2;
12921 vinfos[3].jointtype = 1;
12922 vinfos[3].foffset = j3;
12923 vinfos[3].indices[0] = _ij3[0];
12924 vinfos[3].indices[1] = _ij3[1];
12925 vinfos[3].maxsolutions = _nj3;
12926 vinfos[4].jointtype = 1;
12927 vinfos[4].foffset = j4;
12928 vinfos[4].indices[0] = _ij4[0];
12929 vinfos[4].indices[1] = _ij4[1];
12930 vinfos[4].maxsolutions = _nj4;
12931 vinfos[5].jointtype = 1;
12932 vinfos[5].foffset = j5;
12933 vinfos[5].indices[0] = _ij5[0];
12934 vinfos[5].indices[1] = _ij5[1];
12935 vinfos[5].maxsolutions = _nj5;
12936 std::vector<int> vfree(0);
12937 solutions.AddSolution(vinfos,vfree);
12938 }
12939 }
12940 }
12941 
12942 } else
12943 {
12944 {
12945 IkReal j3array[2], cj3array[2], sj3array[2];
12946 bool j3valid[2]={false};
12947 _nj3 = 2;
12948 CheckValue<IkReal> x1000=IKPowWithIntegerCheck(new_r11,-1);
12949 if(!x1000.valid){
12950 continue;
12951 }
12952 cj3array[0]=(gconst17*(x1000.value));
12953 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12954 {
12955  j3valid[0] = j3valid[1] = true;
12956  j3array[0] = IKacos(cj3array[0]);
12957  sj3array[0] = IKsin(j3array[0]);
12958  cj3array[1] = cj3array[0];
12959  j3array[1] = -j3array[0];
12960  sj3array[1] = -sj3array[0];
12961 }
12962 else if( isnan(cj3array[0]) )
12963 {
12964  // probably any value will work
12965  j3valid[0] = true;
12966  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12967 }
12968 for(int ij3 = 0; ij3 < 2; ++ij3)
12969 {
12970 if( !j3valid[ij3] )
12971 {
12972  continue;
12973 }
12974 _ij3[0] = ij3; _ij3[1] = -1;
12975 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12976 {
12977 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12978 {
12979  j3valid[iij3]=false; _ij3[1] = iij3; break;
12980 }
12981 }
12982 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12983 {
12984 IkReal evalcond[6];
12985 IkReal x1001=IKsin(j3);
12986 IkReal x1002=IKcos(j3);
12987 IkReal x1003=(gconst17*x1002);
12988 evalcond[0]=(new_r00*x1001);
12989 evalcond[1]=(new_r11*x1001);
12990 evalcond[2]=(gconst17*x1001);
12991 evalcond[3]=(gconst17+((new_r00*x1002)));
12992 evalcond[4]=(x1003+new_r00);
12993 evalcond[5]=((((-1.0)*x1003))+new_r11);
12994 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12995 {
12996 continue;
12997 }
12998 }
12999 
13000 {
13001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13002 vinfos[0].jointtype = 1;
13003 vinfos[0].foffset = j0;
13004 vinfos[0].indices[0] = _ij0[0];
13005 vinfos[0].indices[1] = _ij0[1];
13006 vinfos[0].maxsolutions = _nj0;
13007 vinfos[1].jointtype = 1;
13008 vinfos[1].foffset = j1;
13009 vinfos[1].indices[0] = _ij1[0];
13010 vinfos[1].indices[1] = _ij1[1];
13011 vinfos[1].maxsolutions = _nj1;
13012 vinfos[2].jointtype = 1;
13013 vinfos[2].foffset = j2;
13014 vinfos[2].indices[0] = _ij2[0];
13015 vinfos[2].indices[1] = _ij2[1];
13016 vinfos[2].maxsolutions = _nj2;
13017 vinfos[3].jointtype = 1;
13018 vinfos[3].foffset = j3;
13019 vinfos[3].indices[0] = _ij3[0];
13020 vinfos[3].indices[1] = _ij3[1];
13021 vinfos[3].maxsolutions = _nj3;
13022 vinfos[4].jointtype = 1;
13023 vinfos[4].foffset = j4;
13024 vinfos[4].indices[0] = _ij4[0];
13025 vinfos[4].indices[1] = _ij4[1];
13026 vinfos[4].maxsolutions = _nj4;
13027 vinfos[5].jointtype = 1;
13028 vinfos[5].foffset = j5;
13029 vinfos[5].indices[0] = _ij5[0];
13030 vinfos[5].indices[1] = _ij5[1];
13031 vinfos[5].maxsolutions = _nj5;
13032 std::vector<int> vfree(0);
13033 solutions.AddSolution(vinfos,vfree);
13034 }
13035 }
13036 }
13037 
13038 }
13039 
13040 }
13041 
13042 }
13043 } while(0);
13044 if( bgotonextstatement )
13045 {
13046 bool bgotonextstatement = true;
13047 do
13048 {
13049 evalcond[0]=IKabs(new_r00);
13050 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13051 {
13052 bgotonextstatement=false;
13053 {
13054 IkReal j3eval[1];
13055 CheckValue<IkReal> x1005 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13056 if(!x1005.valid){
13057 continue;
13058 }
13059 IkReal x1004=((1.0)*(x1005.value));
13060 sj4=0;
13061 cj4=-1.0;
13062 j4=3.14159265358979;
13063 sj5=gconst16;
13064 cj5=gconst17;
13065 j5=((3.14159265)+(((-1.0)*x1004)));
13066 new_r00=0;
13067 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1004)));
13068 IkReal x1006 = new_r10*new_r10;
13069 if(IKabs(x1006)==0){
13070 continue;
13071 }
13072 IkReal gconst16=((1.0)*new_r10*(pow(x1006,-0.5)));
13073 IkReal gconst17=0;
13074 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13075 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13076 {
13077 {
13078 IkReal j3eval[1];
13079 CheckValue<IkReal> x1008 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13080 if(!x1008.valid){
13081 continue;
13082 }
13083 IkReal x1007=((1.0)*(x1008.value));
13084 sj4=0;
13085 cj4=-1.0;
13086 j4=3.14159265358979;
13087 sj5=gconst16;
13088 cj5=gconst17;
13089 j5=((3.14159265)+(((-1.0)*x1007)));
13090 new_r00=0;
13091 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1007)));
13092 IkReal x1009 = new_r10*new_r10;
13093 if(IKabs(x1009)==0){
13094 continue;
13095 }
13096 IkReal gconst16=((1.0)*new_r10*(pow(x1009,-0.5)));
13097 IkReal gconst17=0;
13098 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13099 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13100 {
13101 {
13102 IkReal j3eval[1];
13103 CheckValue<IkReal> x1011 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13104 if(!x1011.valid){
13105 continue;
13106 }
13107 IkReal x1010=((1.0)*(x1011.value));
13108 sj4=0;
13109 cj4=-1.0;
13110 j4=3.14159265358979;
13111 sj5=gconst16;
13112 cj5=gconst17;
13113 j5=((3.14159265)+(((-1.0)*x1010)));
13114 new_r00=0;
13115 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1010)));
13116 IkReal x1012 = new_r10*new_r10;
13117 if(IKabs(x1012)==0){
13118 continue;
13119 }
13120 IkReal gconst16=((1.0)*new_r10*(pow(x1012,-0.5)));
13121 IkReal gconst17=0;
13122 j3eval[0]=new_r10;
13123 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13124 {
13125 continue; // 3 cases reached
13126 
13127 } else
13128 {
13129 {
13130 IkReal j3array[1], cj3array[1], sj3array[1];
13131 bool j3valid[1]={false};
13132 _nj3 = 1;
13133 CheckValue<IkReal> x1013=IKPowWithIntegerCheck(gconst16,-1);
13134 if(!x1013.valid){
13135 continue;
13136 }
13137 CheckValue<IkReal> x1014=IKPowWithIntegerCheck(new_r10,-1);
13138 if(!x1014.valid){
13139 continue;
13140 }
13141 if( IKabs((new_r11*(x1013.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst16*(x1014.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x1013.value)))+IKsqr((gconst16*(x1014.value)))-1) <= IKFAST_SINCOS_THRESH )
13142  continue;
13143 j3array[0]=IKatan2((new_r11*(x1013.value)), (gconst16*(x1014.value)));
13144 sj3array[0]=IKsin(j3array[0]);
13145 cj3array[0]=IKcos(j3array[0]);
13146 if( j3array[0] > IKPI )
13147 {
13148  j3array[0]-=IK2PI;
13149 }
13150 else if( j3array[0] < -IKPI )
13151 { j3array[0]+=IK2PI;
13152 }
13153 j3valid[0] = true;
13154 for(int ij3 = 0; ij3 < 1; ++ij3)
13155 {
13156 if( !j3valid[ij3] )
13157 {
13158  continue;
13159 }
13160 _ij3[0] = ij3; _ij3[1] = -1;
13161 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13162 {
13163 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13164 {
13165  j3valid[iij3]=false; _ij3[1] = iij3; break;
13166 }
13167 }
13168 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13169 {
13170 IkReal evalcond[8];
13171 IkReal x1015=IKsin(j3);
13172 IkReal x1016=IKcos(j3);
13173 IkReal x1017=((1.0)*gconst16);
13174 IkReal x1018=((1.0)*x1016);
13175 IkReal x1019=(x1016*x1017);
13176 evalcond[0]=(new_r10*x1015);
13177 evalcond[1]=(gconst16*x1015);
13178 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1018)));
13179 evalcond[3]=((((-1.0)*x1019))+new_r01);
13180 evalcond[4]=((((-1.0)*x1015*x1017))+new_r11);
13181 evalcond[5]=((((-1.0)*x1019))+new_r10);
13182 evalcond[6]=((((-1.0)*new_r11*x1018))+((new_r01*x1015)));
13183 evalcond[7]=(((new_r11*x1015))+(((-1.0)*x1017))+((new_r01*x1016)));
13184 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13185 {
13186 continue;
13187 }
13188 }
13189 
13190 {
13191 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13192 vinfos[0].jointtype = 1;
13193 vinfos[0].foffset = j0;
13194 vinfos[0].indices[0] = _ij0[0];
13195 vinfos[0].indices[1] = _ij0[1];
13196 vinfos[0].maxsolutions = _nj0;
13197 vinfos[1].jointtype = 1;
13198 vinfos[1].foffset = j1;
13199 vinfos[1].indices[0] = _ij1[0];
13200 vinfos[1].indices[1] = _ij1[1];
13201 vinfos[1].maxsolutions = _nj1;
13202 vinfos[2].jointtype = 1;
13203 vinfos[2].foffset = j2;
13204 vinfos[2].indices[0] = _ij2[0];
13205 vinfos[2].indices[1] = _ij2[1];
13206 vinfos[2].maxsolutions = _nj2;
13207 vinfos[3].jointtype = 1;
13208 vinfos[3].foffset = j3;
13209 vinfos[3].indices[0] = _ij3[0];
13210 vinfos[3].indices[1] = _ij3[1];
13211 vinfos[3].maxsolutions = _nj3;
13212 vinfos[4].jointtype = 1;
13213 vinfos[4].foffset = j4;
13214 vinfos[4].indices[0] = _ij4[0];
13215 vinfos[4].indices[1] = _ij4[1];
13216 vinfos[4].maxsolutions = _nj4;
13217 vinfos[5].jointtype = 1;
13218 vinfos[5].foffset = j5;
13219 vinfos[5].indices[0] = _ij5[0];
13220 vinfos[5].indices[1] = _ij5[1];
13221 vinfos[5].maxsolutions = _nj5;
13222 std::vector<int> vfree(0);
13223 solutions.AddSolution(vinfos,vfree);
13224 }
13225 }
13226 }
13227 
13228 }
13229 
13230 }
13231 
13232 } else
13233 {
13234 {
13235 IkReal j3array[1], cj3array[1], sj3array[1];
13236 bool j3valid[1]={false};
13237 _nj3 = 1;
13238 CheckValue<IkReal> x1020=IKPowWithIntegerCheck(IKsign(gconst16),-1);
13239 if(!x1020.valid){
13240 continue;
13241 }
13242 CheckValue<IkReal> x1021 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13243 if(!x1021.valid){
13244 continue;
13245 }
13246 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1020.value)))+(x1021.value));
13247 sj3array[0]=IKsin(j3array[0]);
13248 cj3array[0]=IKcos(j3array[0]);
13249 if( j3array[0] > IKPI )
13250 {
13251  j3array[0]-=IK2PI;
13252 }
13253 else if( j3array[0] < -IKPI )
13254 { j3array[0]+=IK2PI;
13255 }
13256 j3valid[0] = true;
13257 for(int ij3 = 0; ij3 < 1; ++ij3)
13258 {
13259 if( !j3valid[ij3] )
13260 {
13261  continue;
13262 }
13263 _ij3[0] = ij3; _ij3[1] = -1;
13264 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13265 {
13266 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13267 {
13268  j3valid[iij3]=false; _ij3[1] = iij3; break;
13269 }
13270 }
13271 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13272 {
13273 IkReal evalcond[8];
13274 IkReal x1022=IKsin(j3);
13275 IkReal x1023=IKcos(j3);
13276 IkReal x1024=((1.0)*gconst16);
13277 IkReal x1025=((1.0)*x1023);
13278 IkReal x1026=(x1023*x1024);
13279 evalcond[0]=(new_r10*x1022);
13280 evalcond[1]=(gconst16*x1022);
13281 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1025)));
13282 evalcond[3]=((((-1.0)*x1026))+new_r01);
13283 evalcond[4]=((((-1.0)*x1022*x1024))+new_r11);
13284 evalcond[5]=((((-1.0)*x1026))+new_r10);
13285 evalcond[6]=((((-1.0)*new_r11*x1025))+((new_r01*x1022)));
13286 evalcond[7]=(((new_r11*x1022))+(((-1.0)*x1024))+((new_r01*x1023)));
13287 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13288 {
13289 continue;
13290 }
13291 }
13292 
13293 {
13294 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13295 vinfos[0].jointtype = 1;
13296 vinfos[0].foffset = j0;
13297 vinfos[0].indices[0] = _ij0[0];
13298 vinfos[0].indices[1] = _ij0[1];
13299 vinfos[0].maxsolutions = _nj0;
13300 vinfos[1].jointtype = 1;
13301 vinfos[1].foffset = j1;
13302 vinfos[1].indices[0] = _ij1[0];
13303 vinfos[1].indices[1] = _ij1[1];
13304 vinfos[1].maxsolutions = _nj1;
13305 vinfos[2].jointtype = 1;
13306 vinfos[2].foffset = j2;
13307 vinfos[2].indices[0] = _ij2[0];
13308 vinfos[2].indices[1] = _ij2[1];
13309 vinfos[2].maxsolutions = _nj2;
13310 vinfos[3].jointtype = 1;
13311 vinfos[3].foffset = j3;
13312 vinfos[3].indices[0] = _ij3[0];
13313 vinfos[3].indices[1] = _ij3[1];
13314 vinfos[3].maxsolutions = _nj3;
13315 vinfos[4].jointtype = 1;
13316 vinfos[4].foffset = j4;
13317 vinfos[4].indices[0] = _ij4[0];
13318 vinfos[4].indices[1] = _ij4[1];
13319 vinfos[4].maxsolutions = _nj4;
13320 vinfos[5].jointtype = 1;
13321 vinfos[5].foffset = j5;
13322 vinfos[5].indices[0] = _ij5[0];
13323 vinfos[5].indices[1] = _ij5[1];
13324 vinfos[5].maxsolutions = _nj5;
13325 std::vector<int> vfree(0);
13326 solutions.AddSolution(vinfos,vfree);
13327 }
13328 }
13329 }
13330 
13331 }
13332 
13333 }
13334 
13335 } else
13336 {
13337 {
13338 IkReal j3array[1], cj3array[1], sj3array[1];
13339 bool j3valid[1]={false};
13340 _nj3 = 1;
13341 CheckValue<IkReal> x1027=IKPowWithIntegerCheck(IKsign(gconst16),-1);
13342 if(!x1027.valid){
13343 continue;
13344 }
13345 CheckValue<IkReal> x1028 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
13346 if(!x1028.valid){
13347 continue;
13348 }
13349 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1027.value)))+(x1028.value));
13350 sj3array[0]=IKsin(j3array[0]);
13351 cj3array[0]=IKcos(j3array[0]);
13352 if( j3array[0] > IKPI )
13353 {
13354  j3array[0]-=IK2PI;
13355 }
13356 else if( j3array[0] < -IKPI )
13357 { j3array[0]+=IK2PI;
13358 }
13359 j3valid[0] = true;
13360 for(int ij3 = 0; ij3 < 1; ++ij3)
13361 {
13362 if( !j3valid[ij3] )
13363 {
13364  continue;
13365 }
13366 _ij3[0] = ij3; _ij3[1] = -1;
13367 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13368 {
13369 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13370 {
13371  j3valid[iij3]=false; _ij3[1] = iij3; break;
13372 }
13373 }
13374 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13375 {
13376 IkReal evalcond[8];
13377 IkReal x1029=IKsin(j3);
13378 IkReal x1030=IKcos(j3);
13379 IkReal x1031=((1.0)*gconst16);
13380 IkReal x1032=((1.0)*x1030);
13381 IkReal x1033=(x1030*x1031);
13382 evalcond[0]=(new_r10*x1029);
13383 evalcond[1]=(gconst16*x1029);
13384 evalcond[2]=(gconst16+(((-1.0)*new_r10*x1032)));
13385 evalcond[3]=((((-1.0)*x1033))+new_r01);
13386 evalcond[4]=((((-1.0)*x1029*x1031))+new_r11);
13387 evalcond[5]=((((-1.0)*x1033))+new_r10);
13388 evalcond[6]=((((-1.0)*new_r11*x1032))+((new_r01*x1029)));
13389 evalcond[7]=((((-1.0)*x1031))+((new_r11*x1029))+((new_r01*x1030)));
13390 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13391 {
13392 continue;
13393 }
13394 }
13395 
13396 {
13397 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13398 vinfos[0].jointtype = 1;
13399 vinfos[0].foffset = j0;
13400 vinfos[0].indices[0] = _ij0[0];
13401 vinfos[0].indices[1] = _ij0[1];
13402 vinfos[0].maxsolutions = _nj0;
13403 vinfos[1].jointtype = 1;
13404 vinfos[1].foffset = j1;
13405 vinfos[1].indices[0] = _ij1[0];
13406 vinfos[1].indices[1] = _ij1[1];
13407 vinfos[1].maxsolutions = _nj1;
13408 vinfos[2].jointtype = 1;
13409 vinfos[2].foffset = j2;
13410 vinfos[2].indices[0] = _ij2[0];
13411 vinfos[2].indices[1] = _ij2[1];
13412 vinfos[2].maxsolutions = _nj2;
13413 vinfos[3].jointtype = 1;
13414 vinfos[3].foffset = j3;
13415 vinfos[3].indices[0] = _ij3[0];
13416 vinfos[3].indices[1] = _ij3[1];
13417 vinfos[3].maxsolutions = _nj3;
13418 vinfos[4].jointtype = 1;
13419 vinfos[4].foffset = j4;
13420 vinfos[4].indices[0] = _ij4[0];
13421 vinfos[4].indices[1] = _ij4[1];
13422 vinfos[4].maxsolutions = _nj4;
13423 vinfos[5].jointtype = 1;
13424 vinfos[5].foffset = j5;
13425 vinfos[5].indices[0] = _ij5[0];
13426 vinfos[5].indices[1] = _ij5[1];
13427 vinfos[5].maxsolutions = _nj5;
13428 std::vector<int> vfree(0);
13429 solutions.AddSolution(vinfos,vfree);
13430 }
13431 }
13432 }
13433 
13434 }
13435 
13436 }
13437 
13438 }
13439 } while(0);
13440 if( bgotonextstatement )
13441 {
13442 bool bgotonextstatement = true;
13443 do
13444 {
13445 if( 1 )
13446 {
13447 bgotonextstatement=false;
13448 continue; // branch miss [j3]
13449 
13450 }
13451 } while(0);
13452 if( bgotonextstatement )
13453 {
13454 }
13455 }
13456 }
13457 }
13458 }
13459 }
13460 
13461 } else
13462 {
13463 {
13464 IkReal j3array[1], cj3array[1], sj3array[1];
13465 bool j3valid[1]={false};
13466 _nj3 = 1;
13467 CheckValue<IkReal> x1034 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst16*gconst17)))),IkReal(((gconst17*gconst17)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
13468 if(!x1034.valid){
13469 continue;
13470 }
13471 CheckValue<IkReal> x1035=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r10))+(((-1.0)*gconst17*new_r00)))),-1);
13472 if(!x1035.valid){
13473 continue;
13474 }
13475 j3array[0]=((-1.5707963267949)+(x1034.value)+(((1.5707963267949)*(x1035.value))));
13476 sj3array[0]=IKsin(j3array[0]);
13477 cj3array[0]=IKcos(j3array[0]);
13478 if( j3array[0] > IKPI )
13479 {
13480  j3array[0]-=IK2PI;
13481 }
13482 else if( j3array[0] < -IKPI )
13483 { j3array[0]+=IK2PI;
13484 }
13485 j3valid[0] = true;
13486 for(int ij3 = 0; ij3 < 1; ++ij3)
13487 {
13488 if( !j3valid[ij3] )
13489 {
13490  continue;
13491 }
13492 _ij3[0] = ij3; _ij3[1] = -1;
13493 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13494 {
13495 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13496 {
13497  j3valid[iij3]=false; _ij3[1] = iij3; break;
13498 }
13499 }
13500 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13501 {
13502 IkReal evalcond[8];
13503 IkReal x1036=IKsin(j3);
13504 IkReal x1037=IKcos(j3);
13505 IkReal x1038=((1.0)*gconst16);
13506 IkReal x1039=(gconst17*x1036);
13507 IkReal x1040=(gconst17*x1037);
13508 IkReal x1041=((1.0)*x1037);
13509 IkReal x1042=(x1037*x1038);
13510 evalcond[0]=(gconst17+((new_r10*x1036))+((new_r00*x1037)));
13511 evalcond[1]=(x1040+((gconst16*x1036))+new_r00);
13512 evalcond[2]=(gconst16+((new_r00*x1036))+(((-1.0)*new_r10*x1041)));
13513 evalcond[3]=(gconst17+((new_r01*x1036))+(((-1.0)*new_r11*x1041)));
13514 evalcond[4]=(x1039+new_r01+(((-1.0)*x1042)));
13515 evalcond[5]=(x1039+new_r10+(((-1.0)*x1042)));
13516 evalcond[6]=((((-1.0)*x1038))+((new_r11*x1036))+((new_r01*x1037)));
13517 evalcond[7]=((((-1.0)*x1036*x1038))+(((-1.0)*x1040))+new_r11);
13518 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13519 {
13520 continue;
13521 }
13522 }
13523 
13524 {
13525 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13526 vinfos[0].jointtype = 1;
13527 vinfos[0].foffset = j0;
13528 vinfos[0].indices[0] = _ij0[0];
13529 vinfos[0].indices[1] = _ij0[1];
13530 vinfos[0].maxsolutions = _nj0;
13531 vinfos[1].jointtype = 1;
13532 vinfos[1].foffset = j1;
13533 vinfos[1].indices[0] = _ij1[0];
13534 vinfos[1].indices[1] = _ij1[1];
13535 vinfos[1].maxsolutions = _nj1;
13536 vinfos[2].jointtype = 1;
13537 vinfos[2].foffset = j2;
13538 vinfos[2].indices[0] = _ij2[0];
13539 vinfos[2].indices[1] = _ij2[1];
13540 vinfos[2].maxsolutions = _nj2;
13541 vinfos[3].jointtype = 1;
13542 vinfos[3].foffset = j3;
13543 vinfos[3].indices[0] = _ij3[0];
13544 vinfos[3].indices[1] = _ij3[1];
13545 vinfos[3].maxsolutions = _nj3;
13546 vinfos[4].jointtype = 1;
13547 vinfos[4].foffset = j4;
13548 vinfos[4].indices[0] = _ij4[0];
13549 vinfos[4].indices[1] = _ij4[1];
13550 vinfos[4].maxsolutions = _nj4;
13551 vinfos[5].jointtype = 1;
13552 vinfos[5].foffset = j5;
13553 vinfos[5].indices[0] = _ij5[0];
13554 vinfos[5].indices[1] = _ij5[1];
13555 vinfos[5].maxsolutions = _nj5;
13556 std::vector<int> vfree(0);
13557 solutions.AddSolution(vinfos,vfree);
13558 }
13559 }
13560 }
13561 
13562 }
13563 
13564 }
13565 
13566 } else
13567 {
13568 {
13569 IkReal j3array[1], cj3array[1], sj3array[1];
13570 bool j3valid[1]={false};
13571 _nj3 = 1;
13572 IkReal x1043=((1.0)*new_r10);
13573 CheckValue<IkReal> x1044=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*x1043))+(((-1.0)*gconst17*new_r00)))),-1);
13574 if(!x1044.valid){
13575 continue;
13576 }
13577 CheckValue<IkReal> x1045 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((gconst16*gconst17)))),IkReal(((((-1.0)*new_r01*x1043))+(gconst17*gconst17))),IKFAST_ATAN2_MAGTHRESH);
13578 if(!x1045.valid){
13579 continue;
13580 }
13581 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1044.value)))+(x1045.value));
13582 sj3array[0]=IKsin(j3array[0]);
13583 cj3array[0]=IKcos(j3array[0]);
13584 if( j3array[0] > IKPI )
13585 {
13586  j3array[0]-=IK2PI;
13587 }
13588 else if( j3array[0] < -IKPI )
13589 { j3array[0]+=IK2PI;
13590 }
13591 j3valid[0] = true;
13592 for(int ij3 = 0; ij3 < 1; ++ij3)
13593 {
13594 if( !j3valid[ij3] )
13595 {
13596  continue;
13597 }
13598 _ij3[0] = ij3; _ij3[1] = -1;
13599 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13600 {
13601 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13602 {
13603  j3valid[iij3]=false; _ij3[1] = iij3; break;
13604 }
13605 }
13606 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13607 {
13608 IkReal evalcond[8];
13609 IkReal x1046=IKsin(j3);
13610 IkReal x1047=IKcos(j3);
13611 IkReal x1048=((1.0)*gconst16);
13612 IkReal x1049=(gconst17*x1046);
13613 IkReal x1050=(gconst17*x1047);
13614 IkReal x1051=((1.0)*x1047);
13615 IkReal x1052=(x1047*x1048);
13616 evalcond[0]=(gconst17+((new_r00*x1047))+((new_r10*x1046)));
13617 evalcond[1]=(x1050+new_r00+((gconst16*x1046)));
13618 evalcond[2]=(gconst16+((new_r00*x1046))+(((-1.0)*new_r10*x1051)));
13619 evalcond[3]=(gconst17+((new_r01*x1046))+(((-1.0)*new_r11*x1051)));
13620 evalcond[4]=((((-1.0)*x1052))+x1049+new_r01);
13621 evalcond[5]=((((-1.0)*x1052))+x1049+new_r10);
13622 evalcond[6]=(((new_r11*x1046))+((new_r01*x1047))+(((-1.0)*x1048)));
13623 evalcond[7]=((((-1.0)*x1046*x1048))+(((-1.0)*x1050))+new_r11);
13624 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13625 {
13626 continue;
13627 }
13628 }
13629 
13630 {
13631 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13632 vinfos[0].jointtype = 1;
13633 vinfos[0].foffset = j0;
13634 vinfos[0].indices[0] = _ij0[0];
13635 vinfos[0].indices[1] = _ij0[1];
13636 vinfos[0].maxsolutions = _nj0;
13637 vinfos[1].jointtype = 1;
13638 vinfos[1].foffset = j1;
13639 vinfos[1].indices[0] = _ij1[0];
13640 vinfos[1].indices[1] = _ij1[1];
13641 vinfos[1].maxsolutions = _nj1;
13642 vinfos[2].jointtype = 1;
13643 vinfos[2].foffset = j2;
13644 vinfos[2].indices[0] = _ij2[0];
13645 vinfos[2].indices[1] = _ij2[1];
13646 vinfos[2].maxsolutions = _nj2;
13647 vinfos[3].jointtype = 1;
13648 vinfos[3].foffset = j3;
13649 vinfos[3].indices[0] = _ij3[0];
13650 vinfos[3].indices[1] = _ij3[1];
13651 vinfos[3].maxsolutions = _nj3;
13652 vinfos[4].jointtype = 1;
13653 vinfos[4].foffset = j4;
13654 vinfos[4].indices[0] = _ij4[0];
13655 vinfos[4].indices[1] = _ij4[1];
13656 vinfos[4].maxsolutions = _nj4;
13657 vinfos[5].jointtype = 1;
13658 vinfos[5].foffset = j5;
13659 vinfos[5].indices[0] = _ij5[0];
13660 vinfos[5].indices[1] = _ij5[1];
13661 vinfos[5].maxsolutions = _nj5;
13662 std::vector<int> vfree(0);
13663 solutions.AddSolution(vinfos,vfree);
13664 }
13665 }
13666 }
13667 
13668 }
13669 
13670 }
13671 
13672 } else
13673 {
13674 {
13675 IkReal j3array[1], cj3array[1], sj3array[1];
13676 bool j3valid[1]={false};
13677 _nj3 = 1;
13678 IkReal x1053=((1.0)*new_r10);
13679 CheckValue<IkReal> x1054 = IKatan2WithCheck(IkReal((((gconst17*new_r11))+((gconst17*new_r00)))),IkReal(((((-1.0)*gconst17*x1053))+((gconst17*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
13680 if(!x1054.valid){
13681 continue;
13682 }
13683 CheckValue<IkReal> x1055=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1053)))),-1);
13684 if(!x1055.valid){
13685 continue;
13686 }
13687 j3array[0]=((-1.5707963267949)+(x1054.value)+(((1.5707963267949)*(x1055.value))));
13688 sj3array[0]=IKsin(j3array[0]);
13689 cj3array[0]=IKcos(j3array[0]);
13690 if( j3array[0] > IKPI )
13691 {
13692  j3array[0]-=IK2PI;
13693 }
13694 else if( j3array[0] < -IKPI )
13695 { j3array[0]+=IK2PI;
13696 }
13697 j3valid[0] = true;
13698 for(int ij3 = 0; ij3 < 1; ++ij3)
13699 {
13700 if( !j3valid[ij3] )
13701 {
13702  continue;
13703 }
13704 _ij3[0] = ij3; _ij3[1] = -1;
13705 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13706 {
13707 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13708 {
13709  j3valid[iij3]=false; _ij3[1] = iij3; break;
13710 }
13711 }
13712 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13713 {
13714 IkReal evalcond[8];
13715 IkReal x1056=IKsin(j3);
13716 IkReal x1057=IKcos(j3);
13717 IkReal x1058=((1.0)*gconst16);
13718 IkReal x1059=(gconst17*x1056);
13719 IkReal x1060=(gconst17*x1057);
13720 IkReal x1061=((1.0)*x1057);
13721 IkReal x1062=(x1057*x1058);
13722 evalcond[0]=(gconst17+((new_r00*x1057))+((new_r10*x1056)));
13723 evalcond[1]=(x1060+new_r00+((gconst16*x1056)));
13724 evalcond[2]=((((-1.0)*new_r10*x1061))+gconst16+((new_r00*x1056)));
13725 evalcond[3]=(gconst17+((new_r01*x1056))+(((-1.0)*new_r11*x1061)));
13726 evalcond[4]=(x1059+new_r01+(((-1.0)*x1062)));
13727 evalcond[5]=(x1059+new_r10+(((-1.0)*x1062)));
13728 evalcond[6]=((((-1.0)*x1058))+((new_r01*x1057))+((new_r11*x1056)));
13729 evalcond[7]=((((-1.0)*x1056*x1058))+new_r11+(((-1.0)*x1060)));
13730 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13731 {
13732 continue;
13733 }
13734 }
13735 
13736 {
13737 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13738 vinfos[0].jointtype = 1;
13739 vinfos[0].foffset = j0;
13740 vinfos[0].indices[0] = _ij0[0];
13741 vinfos[0].indices[1] = _ij0[1];
13742 vinfos[0].maxsolutions = _nj0;
13743 vinfos[1].jointtype = 1;
13744 vinfos[1].foffset = j1;
13745 vinfos[1].indices[0] = _ij1[0];
13746 vinfos[1].indices[1] = _ij1[1];
13747 vinfos[1].maxsolutions = _nj1;
13748 vinfos[2].jointtype = 1;
13749 vinfos[2].foffset = j2;
13750 vinfos[2].indices[0] = _ij2[0];
13751 vinfos[2].indices[1] = _ij2[1];
13752 vinfos[2].maxsolutions = _nj2;
13753 vinfos[3].jointtype = 1;
13754 vinfos[3].foffset = j3;
13755 vinfos[3].indices[0] = _ij3[0];
13756 vinfos[3].indices[1] = _ij3[1];
13757 vinfos[3].maxsolutions = _nj3;
13758 vinfos[4].jointtype = 1;
13759 vinfos[4].foffset = j4;
13760 vinfos[4].indices[0] = _ij4[0];
13761 vinfos[4].indices[1] = _ij4[1];
13762 vinfos[4].maxsolutions = _nj4;
13763 vinfos[5].jointtype = 1;
13764 vinfos[5].foffset = j5;
13765 vinfos[5].indices[0] = _ij5[0];
13766 vinfos[5].indices[1] = _ij5[1];
13767 vinfos[5].maxsolutions = _nj5;
13768 std::vector<int> vfree(0);
13769 solutions.AddSolution(vinfos,vfree);
13770 }
13771 }
13772 }
13773 
13774 }
13775 
13776 }
13777 
13778 }
13779 } while(0);
13780 if( bgotonextstatement )
13781 {
13782 bool bgotonextstatement = true;
13783 do
13784 {
13785 IkReal x1063=((-1.0)*new_r10);
13786 IkReal x1065 = ((new_r10*new_r10)+(new_r00*new_r00));
13787 if(IKabs(x1065)==0){
13788 continue;
13789 }
13790 IkReal x1064=pow(x1065,-0.5);
13791 CheckValue<IkReal> x1066 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1063),IKFAST_ATAN2_MAGTHRESH);
13792 if(!x1066.valid){
13793 continue;
13794 }
13795 IkReal gconst18=((-1.0)*(x1066.value));
13796 IkReal gconst19=(new_r00*x1064);
13797 IkReal gconst20=(x1063*x1064);
13798 CheckValue<IkReal> x1067 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
13799 if(!x1067.valid){
13800 continue;
13801 }
13802 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x1067.value)+j5)))), 6.28318530717959)));
13803 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13804 {
13805 bgotonextstatement=false;
13806 {
13807 IkReal j3eval[3];
13808 IkReal x1068=((-1.0)*new_r10);
13809 CheckValue<IkReal> x1071 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1068),IKFAST_ATAN2_MAGTHRESH);
13810 if(!x1071.valid){
13811 continue;
13812 }
13813 IkReal x1069=((-1.0)*(x1071.value));
13814 IkReal x1070=x1064;
13815 sj4=0;
13816 cj4=-1.0;
13817 j4=3.14159265358979;
13818 sj5=gconst19;
13819 cj5=gconst20;
13820 j5=x1069;
13821 IkReal gconst18=x1069;
13822 IkReal gconst19=(new_r00*x1070);
13823 IkReal gconst20=(x1068*x1070);
13824 IkReal x1072=new_r10*new_r10;
13825 IkReal x1073=((1.0)*new_r00);
13826 IkReal x1074=((1.0)*new_r10*new_r11);
13827 IkReal x1075=((((-1.0)*new_r01*x1073))+(((-1.0)*x1074)));
13828 IkReal x1076=x1064;
13829 IkReal x1077=(new_r10*x1076);
13830 j3eval[0]=x1075;
13831 j3eval[1]=((IKabs((((x1072*x1076))+(((-1.0)*new_r01*x1077)))))+(IKabs(((((-1.0)*x1074*x1076))+(((-1.0)*x1073*x1077))))));
13832 j3eval[2]=IKsign(x1075);
13833 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
13834 {
13835 {
13836 IkReal j3eval[1];
13837 IkReal x1078=((-1.0)*new_r10);
13838 CheckValue<IkReal> x1081 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1078),IKFAST_ATAN2_MAGTHRESH);
13839 if(!x1081.valid){
13840 continue;
13841 }
13842 IkReal x1079=((-1.0)*(x1081.value));
13843 IkReal x1080=x1064;
13844 sj4=0;
13845 cj4=-1.0;
13846 j4=3.14159265358979;
13847 sj5=gconst19;
13848 cj5=gconst20;
13849 j5=x1079;
13850 IkReal gconst18=x1079;
13851 IkReal gconst19=(new_r00*x1080);
13852 IkReal gconst20=(x1078*x1080);
13853 IkReal x1082=new_r10*new_r10;
13854 CheckValue<IkReal> x1085=IKPowWithIntegerCheck((x1082+(new_r00*new_r00)),-1);
13855 if(!x1085.valid){
13856 continue;
13857 }
13858 IkReal x1083=x1085.value;
13859 IkReal x1084=(new_r00*x1083);
13860 j3eval[0]=((IKabs((((new_r01*x1082*x1084))+((new_r10*x1084))+((new_r01*x1084*(new_r00*new_r00))))))+(IKabs((((x1082*x1083))+((new_r00*new_r11))))));
13861 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13862 {
13863 {
13864 IkReal j3eval[1];
13865 IkReal x1086=((-1.0)*new_r10);
13866 CheckValue<IkReal> x1089 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1086),IKFAST_ATAN2_MAGTHRESH);
13867 if(!x1089.valid){
13868 continue;
13869 }
13870 IkReal x1087=((-1.0)*(x1089.value));
13871 IkReal x1088=x1064;
13872 sj4=0;
13873 cj4=-1.0;
13874 j4=3.14159265358979;
13875 sj5=gconst19;
13876 cj5=gconst20;
13877 j5=x1087;
13878 IkReal gconst18=x1087;
13879 IkReal gconst19=(new_r00*x1088);
13880 IkReal gconst20=(x1086*x1088);
13881 IkReal x1090=new_r10*new_r10;
13882 IkReal x1091=new_r00*new_r00;
13883 CheckValue<IkReal> x1095=IKPowWithIntegerCheck((x1090+x1091),-1);
13884 if(!x1095.valid){
13885 continue;
13886 }
13887 IkReal x1092=x1095.value;
13888 IkReal x1093=(new_r10*x1092);
13889 IkReal x1094=(x1090*x1092);
13890 j3eval[0]=((IKabs((x1094+(((-1.0)*x1091*x1094))+(((-1.0)*x1092*(x1091*x1091))))))+(IKabs((((new_r00*x1093*(new_r10*new_r10)))+((new_r00*x1093))+((x1093*(new_r00*new_r00*new_r00)))))));
13891 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13892 {
13893 {
13894 IkReal evalcond[3];
13895 bool bgotonextstatement = true;
13896 do
13897 {
13898 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
13899 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13900 {
13901 bgotonextstatement=false;
13902 {
13903 IkReal j3eval[1];
13904 IkReal x1096=((-1.0)*new_r10);
13905 CheckValue<IkReal> x1098 = IKatan2WithCheck(IkReal(0),IkReal(x1096),IKFAST_ATAN2_MAGTHRESH);
13906 if(!x1098.valid){
13907 continue;
13908 }
13909 IkReal x1097=((-1.0)*(x1098.value));
13910 sj4=0;
13911 cj4=-1.0;
13912 j4=3.14159265358979;
13913 sj5=gconst19;
13914 cj5=gconst20;
13915 j5=x1097;
13916 new_r11=0;
13917 new_r00=0;
13918 IkReal gconst18=x1097;
13919 IkReal gconst19=0;
13920 IkReal x1099 = new_r10*new_r10;
13921 if(IKabs(x1099)==0){
13922 continue;
13923 }
13924 IkReal gconst20=(x1096*(pow(x1099,-0.5)));
13925 j3eval[0]=new_r01;
13926 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13927 {
13928 {
13929 IkReal j3array[2], cj3array[2], sj3array[2];
13930 bool j3valid[2]={false};
13931 _nj3 = 2;
13932 CheckValue<IkReal> x1100=IKPowWithIntegerCheck(gconst20,-1);
13933 if(!x1100.valid){
13934 continue;
13935 }
13936 sj3array[0]=((-1.0)*new_r01*(x1100.value));
13937 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
13938 {
13939  j3valid[0] = j3valid[1] = true;
13940  j3array[0] = IKasin(sj3array[0]);
13941  cj3array[0] = IKcos(j3array[0]);
13942  sj3array[1] = sj3array[0];
13943  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
13944  cj3array[1] = -cj3array[0];
13945 }
13946 else if( isnan(sj3array[0]) )
13947 {
13948  // probably any value will work
13949  j3valid[0] = true;
13950  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13951 }
13952 for(int ij3 = 0; ij3 < 2; ++ij3)
13953 {
13954 if( !j3valid[ij3] )
13955 {
13956  continue;
13957 }
13958 _ij3[0] = ij3; _ij3[1] = -1;
13959 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
13960 {
13961 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13962 {
13963  j3valid[iij3]=false; _ij3[1] = iij3; break;
13964 }
13965 }
13966 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13967 {
13968 IkReal evalcond[6];
13969 IkReal x1101=IKcos(j3);
13970 IkReal x1102=IKsin(j3);
13971 evalcond[0]=(new_r01*x1101);
13972 evalcond[1]=(gconst20*x1101);
13973 evalcond[2]=((-1.0)*new_r10*x1101);
13974 evalcond[3]=(gconst20+((new_r01*x1102)));
13975 evalcond[4]=(gconst20+((new_r10*x1102)));
13976 evalcond[5]=(((gconst20*x1102))+new_r10);
13977 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
13978 {
13979 continue;
13980 }
13981 }
13982 
13983 {
13984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13985 vinfos[0].jointtype = 1;
13986 vinfos[0].foffset = j0;
13987 vinfos[0].indices[0] = _ij0[0];
13988 vinfos[0].indices[1] = _ij0[1];
13989 vinfos[0].maxsolutions = _nj0;
13990 vinfos[1].jointtype = 1;
13991 vinfos[1].foffset = j1;
13992 vinfos[1].indices[0] = _ij1[0];
13993 vinfos[1].indices[1] = _ij1[1];
13994 vinfos[1].maxsolutions = _nj1;
13995 vinfos[2].jointtype = 1;
13996 vinfos[2].foffset = j2;
13997 vinfos[2].indices[0] = _ij2[0];
13998 vinfos[2].indices[1] = _ij2[1];
13999 vinfos[2].maxsolutions = _nj2;
14000 vinfos[3].jointtype = 1;
14001 vinfos[3].foffset = j3;
14002 vinfos[3].indices[0] = _ij3[0];
14003 vinfos[3].indices[1] = _ij3[1];
14004 vinfos[3].maxsolutions = _nj3;
14005 vinfos[4].jointtype = 1;
14006 vinfos[4].foffset = j4;
14007 vinfos[4].indices[0] = _ij4[0];
14008 vinfos[4].indices[1] = _ij4[1];
14009 vinfos[4].maxsolutions = _nj4;
14010 vinfos[5].jointtype = 1;
14011 vinfos[5].foffset = j5;
14012 vinfos[5].indices[0] = _ij5[0];
14013 vinfos[5].indices[1] = _ij5[1];
14014 vinfos[5].maxsolutions = _nj5;
14015 std::vector<int> vfree(0);
14016 solutions.AddSolution(vinfos,vfree);
14017 }
14018 }
14019 }
14020 
14021 } else
14022 {
14023 {
14024 IkReal j3array[2], cj3array[2], sj3array[2];
14025 bool j3valid[2]={false};
14026 _nj3 = 2;
14027 CheckValue<IkReal> x1103=IKPowWithIntegerCheck(new_r01,-1);
14028 if(!x1103.valid){
14029 continue;
14030 }
14031 sj3array[0]=((-1.0)*gconst20*(x1103.value));
14032 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14033 {
14034  j3valid[0] = j3valid[1] = true;
14035  j3array[0] = IKasin(sj3array[0]);
14036  cj3array[0] = IKcos(j3array[0]);
14037  sj3array[1] = sj3array[0];
14038  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14039  cj3array[1] = -cj3array[0];
14040 }
14041 else if( isnan(sj3array[0]) )
14042 {
14043  // probably any value will work
14044  j3valid[0] = true;
14045  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14046 }
14047 for(int ij3 = 0; ij3 < 2; ++ij3)
14048 {
14049 if( !j3valid[ij3] )
14050 {
14051  continue;
14052 }
14053 _ij3[0] = ij3; _ij3[1] = -1;
14054 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14055 {
14056 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14057 {
14058  j3valid[iij3]=false; _ij3[1] = iij3; break;
14059 }
14060 }
14061 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14062 {
14063 IkReal evalcond[6];
14064 IkReal x1104=IKcos(j3);
14065 IkReal x1105=IKsin(j3);
14066 IkReal x1106=(gconst20*x1105);
14067 evalcond[0]=(new_r01*x1104);
14068 evalcond[1]=(gconst20*x1104);
14069 evalcond[2]=((-1.0)*new_r10*x1104);
14070 evalcond[3]=(x1106+new_r01);
14071 evalcond[4]=(gconst20+((new_r10*x1105)));
14072 evalcond[5]=(x1106+new_r10);
14073 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14074 {
14075 continue;
14076 }
14077 }
14078 
14079 {
14080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14081 vinfos[0].jointtype = 1;
14082 vinfos[0].foffset = j0;
14083 vinfos[0].indices[0] = _ij0[0];
14084 vinfos[0].indices[1] = _ij0[1];
14085 vinfos[0].maxsolutions = _nj0;
14086 vinfos[1].jointtype = 1;
14087 vinfos[1].foffset = j1;
14088 vinfos[1].indices[0] = _ij1[0];
14089 vinfos[1].indices[1] = _ij1[1];
14090 vinfos[1].maxsolutions = _nj1;
14091 vinfos[2].jointtype = 1;
14092 vinfos[2].foffset = j2;
14093 vinfos[2].indices[0] = _ij2[0];
14094 vinfos[2].indices[1] = _ij2[1];
14095 vinfos[2].maxsolutions = _nj2;
14096 vinfos[3].jointtype = 1;
14097 vinfos[3].foffset = j3;
14098 vinfos[3].indices[0] = _ij3[0];
14099 vinfos[3].indices[1] = _ij3[1];
14100 vinfos[3].maxsolutions = _nj3;
14101 vinfos[4].jointtype = 1;
14102 vinfos[4].foffset = j4;
14103 vinfos[4].indices[0] = _ij4[0];
14104 vinfos[4].indices[1] = _ij4[1];
14105 vinfos[4].maxsolutions = _nj4;
14106 vinfos[5].jointtype = 1;
14107 vinfos[5].foffset = j5;
14108 vinfos[5].indices[0] = _ij5[0];
14109 vinfos[5].indices[1] = _ij5[1];
14110 vinfos[5].maxsolutions = _nj5;
14111 std::vector<int> vfree(0);
14112 solutions.AddSolution(vinfos,vfree);
14113 }
14114 }
14115 }
14116 
14117 }
14118 
14119 }
14120 
14121 }
14122 } while(0);
14123 if( bgotonextstatement )
14124 {
14125 bool bgotonextstatement = true;
14126 do
14127 {
14128 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14129 evalcond[1]=gconst20;
14130 evalcond[2]=gconst19;
14131 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
14132 {
14133 bgotonextstatement=false;
14134 {
14135 IkReal j3eval[3];
14136 IkReal x1107=((-1.0)*new_r10);
14137 CheckValue<IkReal> x1109 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1107),IKFAST_ATAN2_MAGTHRESH);
14138 if(!x1109.valid){
14139 continue;
14140 }
14141 IkReal x1108=((-1.0)*(x1109.value));
14142 sj4=0;
14143 cj4=-1.0;
14144 j4=3.14159265358979;
14145 sj5=gconst19;
14146 cj5=gconst20;
14147 j5=x1108;
14148 new_r11=0;
14149 new_r01=0;
14150 new_r22=0;
14151 new_r20=0;
14152 IkReal gconst18=x1108;
14153 IkReal gconst19=new_r00;
14154 IkReal gconst20=x1107;
14155 j3eval[0]=-1.0;
14156 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14157 j3eval[2]=-1.0;
14158 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14159 {
14160 {
14161 IkReal j3eval[3];
14162 IkReal x1110=((-1.0)*new_r10);
14163 CheckValue<IkReal> x1112 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1110),IKFAST_ATAN2_MAGTHRESH);
14164 if(!x1112.valid){
14165 continue;
14166 }
14167 IkReal x1111=((-1.0)*(x1112.value));
14168 sj4=0;
14169 cj4=-1.0;
14170 j4=3.14159265358979;
14171 sj5=gconst19;
14172 cj5=gconst20;
14173 j5=x1111;
14174 new_r11=0;
14175 new_r01=0;
14176 new_r22=0;
14177 new_r20=0;
14178 IkReal gconst18=x1111;
14179 IkReal gconst19=new_r00;
14180 IkReal gconst20=x1110;
14181 j3eval[0]=-1.0;
14182 j3eval[1]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14183 j3eval[2]=-1.0;
14184 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14185 {
14186 {
14187 IkReal j3eval[3];
14188 IkReal x1113=((-1.0)*new_r10);
14189 CheckValue<IkReal> x1115 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1113),IKFAST_ATAN2_MAGTHRESH);
14190 if(!x1115.valid){
14191 continue;
14192 }
14193 IkReal x1114=((-1.0)*(x1115.value));
14194 sj4=0;
14195 cj4=-1.0;
14196 j4=3.14159265358979;
14197 sj5=gconst19;
14198 cj5=gconst20;
14199 j5=x1114;
14200 new_r11=0;
14201 new_r01=0;
14202 new_r22=0;
14203 new_r20=0;
14204 IkReal gconst18=x1114;
14205 IkReal gconst19=new_r00;
14206 IkReal gconst20=x1113;
14207 j3eval[0]=1.0;
14208 j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
14209 j3eval[2]=1.0;
14210 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14211 {
14212 continue; // 3 cases reached
14213 
14214 } else
14215 {
14216 {
14217 IkReal j3array[1], cj3array[1], sj3array[1];
14218 bool j3valid[1]={false};
14219 _nj3 = 1;
14220 IkReal x1116=((1.0)*gconst20);
14221 CheckValue<IkReal> x1117 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst20*gconst20))),IkReal(((((-1.0)*gconst19*x1116))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
14222 if(!x1117.valid){
14223 continue;
14224 }
14225 CheckValue<IkReal> x1118=IKPowWithIntegerCheck(IKsign((((gconst19*new_r00))+(((-1.0)*new_r10*x1116)))),-1);
14226 if(!x1118.valid){
14227 continue;
14228 }
14229 j3array[0]=((-1.5707963267949)+(x1117.value)+(((1.5707963267949)*(x1118.value))));
14230 sj3array[0]=IKsin(j3array[0]);
14231 cj3array[0]=IKcos(j3array[0]);
14232 if( j3array[0] > IKPI )
14233 {
14234  j3array[0]-=IK2PI;
14235 }
14236 else if( j3array[0] < -IKPI )
14237 { j3array[0]+=IK2PI;
14238 }
14239 j3valid[0] = true;
14240 for(int ij3 = 0; ij3 < 1; ++ij3)
14241 {
14242 if( !j3valid[ij3] )
14243 {
14244  continue;
14245 }
14246 _ij3[0] = ij3; _ij3[1] = -1;
14247 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14248 {
14249 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14250 {
14251  j3valid[iij3]=false; _ij3[1] = iij3; break;
14252 }
14253 }
14254 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14255 {
14256 IkReal evalcond[6];
14257 IkReal x1119=IKsin(j3);
14258 IkReal x1120=IKcos(j3);
14259 IkReal x1121=(gconst20*x1119);
14260 IkReal x1122=(gconst19*x1119);
14261 IkReal x1123=(gconst20*x1120);
14262 IkReal x1124=((1.0)*x1120);
14263 IkReal x1125=(gconst19*x1124);
14264 evalcond[0]=(x1121+(((-1.0)*x1125)));
14265 evalcond[1]=(gconst20+((new_r10*x1119))+((new_r00*x1120)));
14266 evalcond[2]=(x1122+x1123+new_r00);
14267 evalcond[3]=((((-1.0)*new_r10*x1124))+gconst19+((new_r00*x1119)));
14268 evalcond[4]=((((-1.0)*x1122))+(((-1.0)*x1123)));
14269 evalcond[5]=(x1121+(((-1.0)*x1125))+new_r10);
14270 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14271 {
14272 continue;
14273 }
14274 }
14275 
14276 {
14277 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14278 vinfos[0].jointtype = 1;
14279 vinfos[0].foffset = j0;
14280 vinfos[0].indices[0] = _ij0[0];
14281 vinfos[0].indices[1] = _ij0[1];
14282 vinfos[0].maxsolutions = _nj0;
14283 vinfos[1].jointtype = 1;
14284 vinfos[1].foffset = j1;
14285 vinfos[1].indices[0] = _ij1[0];
14286 vinfos[1].indices[1] = _ij1[1];
14287 vinfos[1].maxsolutions = _nj1;
14288 vinfos[2].jointtype = 1;
14289 vinfos[2].foffset = j2;
14290 vinfos[2].indices[0] = _ij2[0];
14291 vinfos[2].indices[1] = _ij2[1];
14292 vinfos[2].maxsolutions = _nj2;
14293 vinfos[3].jointtype = 1;
14294 vinfos[3].foffset = j3;
14295 vinfos[3].indices[0] = _ij3[0];
14296 vinfos[3].indices[1] = _ij3[1];
14297 vinfos[3].maxsolutions = _nj3;
14298 vinfos[4].jointtype = 1;
14299 vinfos[4].foffset = j4;
14300 vinfos[4].indices[0] = _ij4[0];
14301 vinfos[4].indices[1] = _ij4[1];
14302 vinfos[4].maxsolutions = _nj4;
14303 vinfos[5].jointtype = 1;
14304 vinfos[5].foffset = j5;
14305 vinfos[5].indices[0] = _ij5[0];
14306 vinfos[5].indices[1] = _ij5[1];
14307 vinfos[5].maxsolutions = _nj5;
14308 std::vector<int> vfree(0);
14309 solutions.AddSolution(vinfos,vfree);
14310 }
14311 }
14312 }
14313 
14314 }
14315 
14316 }
14317 
14318 } else
14319 {
14320 {
14321 IkReal j3array[1], cj3array[1], sj3array[1];
14322 bool j3valid[1]={false};
14323 _nj3 = 1;
14324 CheckValue<IkReal> x1126 = IKatan2WithCheck(IkReal((gconst19*new_r00)),IkReal((gconst20*new_r00)),IKFAST_ATAN2_MAGTHRESH);
14325 if(!x1126.valid){
14326 continue;
14327 }
14328 CheckValue<IkReal> x1127=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst20*gconst20)))+(((-1.0)*(gconst19*gconst19))))),-1);
14329 if(!x1127.valid){
14330 continue;
14331 }
14332 j3array[0]=((-1.5707963267949)+(x1126.value)+(((1.5707963267949)*(x1127.value))));
14333 sj3array[0]=IKsin(j3array[0]);
14334 cj3array[0]=IKcos(j3array[0]);
14335 if( j3array[0] > IKPI )
14336 {
14337  j3array[0]-=IK2PI;
14338 }
14339 else if( j3array[0] < -IKPI )
14340 { j3array[0]+=IK2PI;
14341 }
14342 j3valid[0] = true;
14343 for(int ij3 = 0; ij3 < 1; ++ij3)
14344 {
14345 if( !j3valid[ij3] )
14346 {
14347  continue;
14348 }
14349 _ij3[0] = ij3; _ij3[1] = -1;
14350 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14351 {
14352 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14353 {
14354  j3valid[iij3]=false; _ij3[1] = iij3; break;
14355 }
14356 }
14357 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14358 {
14359 IkReal evalcond[6];
14360 IkReal x1128=IKsin(j3);
14361 IkReal x1129=IKcos(j3);
14362 IkReal x1130=(gconst20*x1128);
14363 IkReal x1131=(gconst19*x1128);
14364 IkReal x1132=(gconst20*x1129);
14365 IkReal x1133=((1.0)*x1129);
14366 IkReal x1134=(gconst19*x1133);
14367 evalcond[0]=(x1130+(((-1.0)*x1134)));
14368 evalcond[1]=(gconst20+((new_r10*x1128))+((new_r00*x1129)));
14369 evalcond[2]=(x1132+x1131+new_r00);
14370 evalcond[3]=(gconst19+(((-1.0)*new_r10*x1133))+((new_r00*x1128)));
14371 evalcond[4]=((((-1.0)*x1132))+(((-1.0)*x1131)));
14372 evalcond[5]=(x1130+(((-1.0)*x1134))+new_r10);
14373 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14374 {
14375 continue;
14376 }
14377 }
14378 
14379 {
14380 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14381 vinfos[0].jointtype = 1;
14382 vinfos[0].foffset = j0;
14383 vinfos[0].indices[0] = _ij0[0];
14384 vinfos[0].indices[1] = _ij0[1];
14385 vinfos[0].maxsolutions = _nj0;
14386 vinfos[1].jointtype = 1;
14387 vinfos[1].foffset = j1;
14388 vinfos[1].indices[0] = _ij1[0];
14389 vinfos[1].indices[1] = _ij1[1];
14390 vinfos[1].maxsolutions = _nj1;
14391 vinfos[2].jointtype = 1;
14392 vinfos[2].foffset = j2;
14393 vinfos[2].indices[0] = _ij2[0];
14394 vinfos[2].indices[1] = _ij2[1];
14395 vinfos[2].maxsolutions = _nj2;
14396 vinfos[3].jointtype = 1;
14397 vinfos[3].foffset = j3;
14398 vinfos[3].indices[0] = _ij3[0];
14399 vinfos[3].indices[1] = _ij3[1];
14400 vinfos[3].maxsolutions = _nj3;
14401 vinfos[4].jointtype = 1;
14402 vinfos[4].foffset = j4;
14403 vinfos[4].indices[0] = _ij4[0];
14404 vinfos[4].indices[1] = _ij4[1];
14405 vinfos[4].maxsolutions = _nj4;
14406 vinfos[5].jointtype = 1;
14407 vinfos[5].foffset = j5;
14408 vinfos[5].indices[0] = _ij5[0];
14409 vinfos[5].indices[1] = _ij5[1];
14410 vinfos[5].maxsolutions = _nj5;
14411 std::vector<int> vfree(0);
14412 solutions.AddSolution(vinfos,vfree);
14413 }
14414 }
14415 }
14416 
14417 }
14418 
14419 }
14420 
14421 } else
14422 {
14423 {
14424 IkReal j3array[1], cj3array[1], sj3array[1];
14425 bool j3valid[1]={false};
14426 _nj3 = 1;
14427 CheckValue<IkReal> x1135=IKPowWithIntegerCheck(IKsign((((gconst20*new_r10))+(((-1.0)*gconst19*new_r00)))),-1);
14428 if(!x1135.valid){
14429 continue;
14430 }
14431 CheckValue<IkReal> x1136 = IKatan2WithCheck(IkReal(gconst19*gconst19),IkReal((gconst19*gconst20)),IKFAST_ATAN2_MAGTHRESH);
14432 if(!x1136.valid){
14433 continue;
14434 }
14435 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1135.value)))+(x1136.value));
14436 sj3array[0]=IKsin(j3array[0]);
14437 cj3array[0]=IKcos(j3array[0]);
14438 if( j3array[0] > IKPI )
14439 {
14440  j3array[0]-=IK2PI;
14441 }
14442 else if( j3array[0] < -IKPI )
14443 { j3array[0]+=IK2PI;
14444 }
14445 j3valid[0] = true;
14446 for(int ij3 = 0; ij3 < 1; ++ij3)
14447 {
14448 if( !j3valid[ij3] )
14449 {
14450  continue;
14451 }
14452 _ij3[0] = ij3; _ij3[1] = -1;
14453 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14454 {
14455 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14456 {
14457  j3valid[iij3]=false; _ij3[1] = iij3; break;
14458 }
14459 }
14460 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14461 {
14462 IkReal evalcond[6];
14463 IkReal x1137=IKsin(j3);
14464 IkReal x1138=IKcos(j3);
14465 IkReal x1139=(gconst20*x1137);
14466 IkReal x1140=(gconst19*x1137);
14467 IkReal x1141=(gconst20*x1138);
14468 IkReal x1142=((1.0)*x1138);
14469 IkReal x1143=(gconst19*x1142);
14470 evalcond[0]=(x1139+(((-1.0)*x1143)));
14471 evalcond[1]=(gconst20+((new_r00*x1138))+((new_r10*x1137)));
14472 evalcond[2]=(x1140+x1141+new_r00);
14473 evalcond[3]=(gconst19+((new_r00*x1137))+(((-1.0)*new_r10*x1142)));
14474 evalcond[4]=((((-1.0)*x1140))+(((-1.0)*x1141)));
14475 evalcond[5]=(x1139+(((-1.0)*x1143))+new_r10);
14476 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14477 {
14478 continue;
14479 }
14480 }
14481 
14482 {
14483 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14484 vinfos[0].jointtype = 1;
14485 vinfos[0].foffset = j0;
14486 vinfos[0].indices[0] = _ij0[0];
14487 vinfos[0].indices[1] = _ij0[1];
14488 vinfos[0].maxsolutions = _nj0;
14489 vinfos[1].jointtype = 1;
14490 vinfos[1].foffset = j1;
14491 vinfos[1].indices[0] = _ij1[0];
14492 vinfos[1].indices[1] = _ij1[1];
14493 vinfos[1].maxsolutions = _nj1;
14494 vinfos[2].jointtype = 1;
14495 vinfos[2].foffset = j2;
14496 vinfos[2].indices[0] = _ij2[0];
14497 vinfos[2].indices[1] = _ij2[1];
14498 vinfos[2].maxsolutions = _nj2;
14499 vinfos[3].jointtype = 1;
14500 vinfos[3].foffset = j3;
14501 vinfos[3].indices[0] = _ij3[0];
14502 vinfos[3].indices[1] = _ij3[1];
14503 vinfos[3].maxsolutions = _nj3;
14504 vinfos[4].jointtype = 1;
14505 vinfos[4].foffset = j4;
14506 vinfos[4].indices[0] = _ij4[0];
14507 vinfos[4].indices[1] = _ij4[1];
14508 vinfos[4].maxsolutions = _nj4;
14509 vinfos[5].jointtype = 1;
14510 vinfos[5].foffset = j5;
14511 vinfos[5].indices[0] = _ij5[0];
14512 vinfos[5].indices[1] = _ij5[1];
14513 vinfos[5].maxsolutions = _nj5;
14514 std::vector<int> vfree(0);
14515 solutions.AddSolution(vinfos,vfree);
14516 }
14517 }
14518 }
14519 
14520 }
14521 
14522 }
14523 
14524 }
14525 } while(0);
14526 if( bgotonextstatement )
14527 {
14528 bool bgotonextstatement = true;
14529 do
14530 {
14531 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
14532 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14533 {
14534 bgotonextstatement=false;
14535 {
14536 IkReal j3array[2], cj3array[2], sj3array[2];
14537 bool j3valid[2]={false};
14538 _nj3 = 2;
14539 CheckValue<IkReal> x1144=IKPowWithIntegerCheck(gconst19,-1);
14540 if(!x1144.valid){
14541 continue;
14542 }
14543 sj3array[0]=(new_r11*(x1144.value));
14544 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14545 {
14546  j3valid[0] = j3valid[1] = true;
14547  j3array[0] = IKasin(sj3array[0]);
14548  cj3array[0] = IKcos(j3array[0]);
14549  sj3array[1] = sj3array[0];
14550  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14551  cj3array[1] = -cj3array[0];
14552 }
14553 else if( isnan(sj3array[0]) )
14554 {
14555  // probably any value will work
14556  j3valid[0] = true;
14557  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14558 }
14559 for(int ij3 = 0; ij3 < 2; ++ij3)
14560 {
14561 if( !j3valid[ij3] )
14562 {
14563  continue;
14564 }
14565 _ij3[0] = ij3; _ij3[1] = -1;
14566 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14567 {
14568 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14569 {
14570  j3valid[iij3]=false; _ij3[1] = iij3; break;
14571 }
14572 }
14573 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14574 {
14575 IkReal evalcond[6];
14576 IkReal x1145=IKcos(j3);
14577 IkReal x1146=IKsin(j3);
14578 IkReal x1147=((-1.0)*x1145);
14579 evalcond[0]=(new_r00*x1145);
14580 evalcond[1]=(new_r11*x1147);
14581 evalcond[2]=(gconst19*x1147);
14582 evalcond[3]=(((new_r00*x1146))+gconst19);
14583 evalcond[4]=(((gconst19*x1146))+new_r00);
14584 evalcond[5]=((((-1.0)*gconst19))+((new_r11*x1146)));
14585 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14586 {
14587 continue;
14588 }
14589 }
14590 
14591 {
14592 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14593 vinfos[0].jointtype = 1;
14594 vinfos[0].foffset = j0;
14595 vinfos[0].indices[0] = _ij0[0];
14596 vinfos[0].indices[1] = _ij0[1];
14597 vinfos[0].maxsolutions = _nj0;
14598 vinfos[1].jointtype = 1;
14599 vinfos[1].foffset = j1;
14600 vinfos[1].indices[0] = _ij1[0];
14601 vinfos[1].indices[1] = _ij1[1];
14602 vinfos[1].maxsolutions = _nj1;
14603 vinfos[2].jointtype = 1;
14604 vinfos[2].foffset = j2;
14605 vinfos[2].indices[0] = _ij2[0];
14606 vinfos[2].indices[1] = _ij2[1];
14607 vinfos[2].maxsolutions = _nj2;
14608 vinfos[3].jointtype = 1;
14609 vinfos[3].foffset = j3;
14610 vinfos[3].indices[0] = _ij3[0];
14611 vinfos[3].indices[1] = _ij3[1];
14612 vinfos[3].maxsolutions = _nj3;
14613 vinfos[4].jointtype = 1;
14614 vinfos[4].foffset = j4;
14615 vinfos[4].indices[0] = _ij4[0];
14616 vinfos[4].indices[1] = _ij4[1];
14617 vinfos[4].maxsolutions = _nj4;
14618 vinfos[5].jointtype = 1;
14619 vinfos[5].foffset = j5;
14620 vinfos[5].indices[0] = _ij5[0];
14621 vinfos[5].indices[1] = _ij5[1];
14622 vinfos[5].maxsolutions = _nj5;
14623 std::vector<int> vfree(0);
14624 solutions.AddSolution(vinfos,vfree);
14625 }
14626 }
14627 }
14628 
14629 }
14630 } while(0);
14631 if( bgotonextstatement )
14632 {
14633 bool bgotonextstatement = true;
14634 do
14635 {
14636 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
14637 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14638 {
14639 bgotonextstatement=false;
14640 {
14641 IkReal j3eval[1];
14642 CheckValue<IkReal> x1149 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14643 if(!x1149.valid){
14644 continue;
14645 }
14646 IkReal x1148=((-1.0)*(x1149.value));
14647 sj4=0;
14648 cj4=-1.0;
14649 j4=3.14159265358979;
14650 sj5=gconst19;
14651 cj5=gconst20;
14652 j5=x1148;
14653 new_r11=0;
14654 new_r10=0;
14655 new_r22=0;
14656 new_r02=0;
14657 IkReal gconst18=x1148;
14658 IkReal x1150 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14659 if(IKabs(x1150)==0){
14660 continue;
14661 }
14662 IkReal gconst19=(new_r00*(pow(x1150,-0.5)));
14663 IkReal gconst20=0;
14664 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
14665 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14666 {
14667 {
14668 IkReal j3eval[1];
14669 CheckValue<IkReal> x1152 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14670 if(!x1152.valid){
14671 continue;
14672 }
14673 IkReal x1151=((-1.0)*(x1152.value));
14674 sj4=0;
14675 cj4=-1.0;
14676 j4=3.14159265358979;
14677 sj5=gconst19;
14678 cj5=gconst20;
14679 j5=x1151;
14680 new_r11=0;
14681 new_r10=0;
14682 new_r22=0;
14683 new_r02=0;
14684 IkReal gconst18=x1151;
14685 IkReal x1153 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14686 if(IKabs(x1153)==0){
14687 continue;
14688 }
14689 IkReal gconst19=(new_r00*(pow(x1153,-0.5)));
14690 IkReal gconst20=0;
14691 j3eval[0]=new_r00;
14692 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14693 {
14694 {
14695 IkReal j3eval[2];
14696 CheckValue<IkReal> x1155 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14697 if(!x1155.valid){
14698 continue;
14699 }
14700 IkReal x1154=((-1.0)*(x1155.value));
14701 sj4=0;
14702 cj4=-1.0;
14703 j4=3.14159265358979;
14704 sj5=gconst19;
14705 cj5=gconst20;
14706 j5=x1154;
14707 new_r11=0;
14708 new_r10=0;
14709 new_r22=0;
14710 new_r02=0;
14711 IkReal gconst18=x1154;
14712 IkReal x1156 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14713 if(IKabs(x1156)==0){
14714 continue;
14715 }
14716 IkReal gconst19=(new_r00*(pow(x1156,-0.5)));
14717 IkReal gconst20=0;
14718 j3eval[0]=new_r00;
14719 j3eval[1]=new_r01;
14720 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
14721 {
14722 continue; // 3 cases reached
14723 
14724 } else
14725 {
14726 {
14727 IkReal j3array[1], cj3array[1], sj3array[1];
14728 bool j3valid[1]={false};
14729 _nj3 = 1;
14730 CheckValue<IkReal> x1157=IKPowWithIntegerCheck(new_r00,-1);
14731 if(!x1157.valid){
14732 continue;
14733 }
14734 CheckValue<IkReal> x1158=IKPowWithIntegerCheck(new_r01,-1);
14735 if(!x1158.valid){
14736 continue;
14737 }
14738 if( IKabs(((-1.0)*gconst19*(x1157.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst19*(x1158.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1157.value)))+IKsqr((gconst19*(x1158.value)))-1) <= IKFAST_SINCOS_THRESH )
14739  continue;
14740 j3array[0]=IKatan2(((-1.0)*gconst19*(x1157.value)), (gconst19*(x1158.value)));
14741 sj3array[0]=IKsin(j3array[0]);
14742 cj3array[0]=IKcos(j3array[0]);
14743 if( j3array[0] > IKPI )
14744 {
14745  j3array[0]-=IK2PI;
14746 }
14747 else if( j3array[0] < -IKPI )
14748 { j3array[0]+=IK2PI;
14749 }
14750 j3valid[0] = true;
14751 for(int ij3 = 0; ij3 < 1; ++ij3)
14752 {
14753 if( !j3valid[ij3] )
14754 {
14755  continue;
14756 }
14757 _ij3[0] = ij3; _ij3[1] = -1;
14758 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14759 {
14760 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14761 {
14762  j3valid[iij3]=false; _ij3[1] = iij3; break;
14763 }
14764 }
14765 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14766 {
14767 IkReal evalcond[8];
14768 IkReal x1159=IKsin(j3);
14769 IkReal x1160=IKcos(j3);
14770 IkReal x1161=((1.0)*gconst19);
14771 IkReal x1162=(gconst19*x1159);
14772 evalcond[0]=(new_r01*x1159);
14773 evalcond[1]=(new_r00*x1160);
14774 evalcond[2]=((-1.0)*x1162);
14775 evalcond[3]=((-1.0)*gconst19*x1160);
14776 evalcond[4]=(gconst19+((new_r00*x1159)));
14777 evalcond[5]=(x1162+new_r00);
14778 evalcond[6]=(new_r01+(((-1.0)*x1160*x1161)));
14779 evalcond[7]=(((new_r01*x1160))+(((-1.0)*x1161)));
14780 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14781 {
14782 continue;
14783 }
14784 }
14785 
14786 {
14787 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14788 vinfos[0].jointtype = 1;
14789 vinfos[0].foffset = j0;
14790 vinfos[0].indices[0] = _ij0[0];
14791 vinfos[0].indices[1] = _ij0[1];
14792 vinfos[0].maxsolutions = _nj0;
14793 vinfos[1].jointtype = 1;
14794 vinfos[1].foffset = j1;
14795 vinfos[1].indices[0] = _ij1[0];
14796 vinfos[1].indices[1] = _ij1[1];
14797 vinfos[1].maxsolutions = _nj1;
14798 vinfos[2].jointtype = 1;
14799 vinfos[2].foffset = j2;
14800 vinfos[2].indices[0] = _ij2[0];
14801 vinfos[2].indices[1] = _ij2[1];
14802 vinfos[2].maxsolutions = _nj2;
14803 vinfos[3].jointtype = 1;
14804 vinfos[3].foffset = j3;
14805 vinfos[3].indices[0] = _ij3[0];
14806 vinfos[3].indices[1] = _ij3[1];
14807 vinfos[3].maxsolutions = _nj3;
14808 vinfos[4].jointtype = 1;
14809 vinfos[4].foffset = j4;
14810 vinfos[4].indices[0] = _ij4[0];
14811 vinfos[4].indices[1] = _ij4[1];
14812 vinfos[4].maxsolutions = _nj4;
14813 vinfos[5].jointtype = 1;
14814 vinfos[5].foffset = j5;
14815 vinfos[5].indices[0] = _ij5[0];
14816 vinfos[5].indices[1] = _ij5[1];
14817 vinfos[5].maxsolutions = _nj5;
14818 std::vector<int> vfree(0);
14819 solutions.AddSolution(vinfos,vfree);
14820 }
14821 }
14822 }
14823 
14824 }
14825 
14826 }
14827 
14828 } else
14829 {
14830 {
14831 IkReal j3array[1], cj3array[1], sj3array[1];
14832 bool j3valid[1]={false};
14833 _nj3 = 1;
14834 CheckValue<IkReal> x1163=IKPowWithIntegerCheck(new_r00,-1);
14835 if(!x1163.valid){
14836 continue;
14837 }
14838 CheckValue<IkReal> x1164=IKPowWithIntegerCheck(gconst19,-1);
14839 if(!x1164.valid){
14840 continue;
14841 }
14842 if( IKabs(((-1.0)*gconst19*(x1163.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1164.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1163.value)))+IKsqr((new_r01*(x1164.value)))-1) <= IKFAST_SINCOS_THRESH )
14843  continue;
14844 j3array[0]=IKatan2(((-1.0)*gconst19*(x1163.value)), (new_r01*(x1164.value)));
14845 sj3array[0]=IKsin(j3array[0]);
14846 cj3array[0]=IKcos(j3array[0]);
14847 if( j3array[0] > IKPI )
14848 {
14849  j3array[0]-=IK2PI;
14850 }
14851 else if( j3array[0] < -IKPI )
14852 { j3array[0]+=IK2PI;
14853 }
14854 j3valid[0] = true;
14855 for(int ij3 = 0; ij3 < 1; ++ij3)
14856 {
14857 if( !j3valid[ij3] )
14858 {
14859  continue;
14860 }
14861 _ij3[0] = ij3; _ij3[1] = -1;
14862 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14863 {
14864 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14865 {
14866  j3valid[iij3]=false; _ij3[1] = iij3; break;
14867 }
14868 }
14869 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14870 {
14871 IkReal evalcond[8];
14872 IkReal x1165=IKsin(j3);
14873 IkReal x1166=IKcos(j3);
14874 IkReal x1167=((1.0)*gconst19);
14875 IkReal x1168=(gconst19*x1165);
14876 evalcond[0]=(new_r01*x1165);
14877 evalcond[1]=(new_r00*x1166);
14878 evalcond[2]=((-1.0)*x1168);
14879 evalcond[3]=((-1.0)*gconst19*x1166);
14880 evalcond[4]=(gconst19+((new_r00*x1165)));
14881 evalcond[5]=(x1168+new_r00);
14882 evalcond[6]=((((-1.0)*x1166*x1167))+new_r01);
14883 evalcond[7]=(((new_r01*x1166))+(((-1.0)*x1167)));
14884 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14885 {
14886 continue;
14887 }
14888 }
14889 
14890 {
14891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14892 vinfos[0].jointtype = 1;
14893 vinfos[0].foffset = j0;
14894 vinfos[0].indices[0] = _ij0[0];
14895 vinfos[0].indices[1] = _ij0[1];
14896 vinfos[0].maxsolutions = _nj0;
14897 vinfos[1].jointtype = 1;
14898 vinfos[1].foffset = j1;
14899 vinfos[1].indices[0] = _ij1[0];
14900 vinfos[1].indices[1] = _ij1[1];
14901 vinfos[1].maxsolutions = _nj1;
14902 vinfos[2].jointtype = 1;
14903 vinfos[2].foffset = j2;
14904 vinfos[2].indices[0] = _ij2[0];
14905 vinfos[2].indices[1] = _ij2[1];
14906 vinfos[2].maxsolutions = _nj2;
14907 vinfos[3].jointtype = 1;
14908 vinfos[3].foffset = j3;
14909 vinfos[3].indices[0] = _ij3[0];
14910 vinfos[3].indices[1] = _ij3[1];
14911 vinfos[3].maxsolutions = _nj3;
14912 vinfos[4].jointtype = 1;
14913 vinfos[4].foffset = j4;
14914 vinfos[4].indices[0] = _ij4[0];
14915 vinfos[4].indices[1] = _ij4[1];
14916 vinfos[4].maxsolutions = _nj4;
14917 vinfos[5].jointtype = 1;
14918 vinfos[5].foffset = j5;
14919 vinfos[5].indices[0] = _ij5[0];
14920 vinfos[5].indices[1] = _ij5[1];
14921 vinfos[5].maxsolutions = _nj5;
14922 std::vector<int> vfree(0);
14923 solutions.AddSolution(vinfos,vfree);
14924 }
14925 }
14926 }
14927 
14928 }
14929 
14930 }
14931 
14932 } else
14933 {
14934 {
14935 IkReal j3array[1], cj3array[1], sj3array[1];
14936 bool j3valid[1]={false};
14937 _nj3 = 1;
14938 CheckValue<IkReal> x1169=IKPowWithIntegerCheck(IKsign(gconst19),-1);
14939 if(!x1169.valid){
14940 continue;
14941 }
14942 CheckValue<IkReal> x1170 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
14943 if(!x1170.valid){
14944 continue;
14945 }
14946 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1169.value)))+(x1170.value));
14947 sj3array[0]=IKsin(j3array[0]);
14948 cj3array[0]=IKcos(j3array[0]);
14949 if( j3array[0] > IKPI )
14950 {
14951  j3array[0]-=IK2PI;
14952 }
14953 else if( j3array[0] < -IKPI )
14954 { j3array[0]+=IK2PI;
14955 }
14956 j3valid[0] = true;
14957 for(int ij3 = 0; ij3 < 1; ++ij3)
14958 {
14959 if( !j3valid[ij3] )
14960 {
14961  continue;
14962 }
14963 _ij3[0] = ij3; _ij3[1] = -1;
14964 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14965 {
14966 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14967 {
14968  j3valid[iij3]=false; _ij3[1] = iij3; break;
14969 }
14970 }
14971 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14972 {
14973 IkReal evalcond[8];
14974 IkReal x1171=IKsin(j3);
14975 IkReal x1172=IKcos(j3);
14976 IkReal x1173=((1.0)*gconst19);
14977 IkReal x1174=(gconst19*x1171);
14978 evalcond[0]=(new_r01*x1171);
14979 evalcond[1]=(new_r00*x1172);
14980 evalcond[2]=((-1.0)*x1174);
14981 evalcond[3]=((-1.0)*gconst19*x1172);
14982 evalcond[4]=(gconst19+((new_r00*x1171)));
14983 evalcond[5]=(x1174+new_r00);
14984 evalcond[6]=((((-1.0)*x1172*x1173))+new_r01);
14985 evalcond[7]=((((-1.0)*x1173))+((new_r01*x1172)));
14986 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14987 {
14988 continue;
14989 }
14990 }
14991 
14992 {
14993 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14994 vinfos[0].jointtype = 1;
14995 vinfos[0].foffset = j0;
14996 vinfos[0].indices[0] = _ij0[0];
14997 vinfos[0].indices[1] = _ij0[1];
14998 vinfos[0].maxsolutions = _nj0;
14999 vinfos[1].jointtype = 1;
15000 vinfos[1].foffset = j1;
15001 vinfos[1].indices[0] = _ij1[0];
15002 vinfos[1].indices[1] = _ij1[1];
15003 vinfos[1].maxsolutions = _nj1;
15004 vinfos[2].jointtype = 1;
15005 vinfos[2].foffset = j2;
15006 vinfos[2].indices[0] = _ij2[0];
15007 vinfos[2].indices[1] = _ij2[1];
15008 vinfos[2].maxsolutions = _nj2;
15009 vinfos[3].jointtype = 1;
15010 vinfos[3].foffset = j3;
15011 vinfos[3].indices[0] = _ij3[0];
15012 vinfos[3].indices[1] = _ij3[1];
15013 vinfos[3].maxsolutions = _nj3;
15014 vinfos[4].jointtype = 1;
15015 vinfos[4].foffset = j4;
15016 vinfos[4].indices[0] = _ij4[0];
15017 vinfos[4].indices[1] = _ij4[1];
15018 vinfos[4].maxsolutions = _nj4;
15019 vinfos[5].jointtype = 1;
15020 vinfos[5].foffset = j5;
15021 vinfos[5].indices[0] = _ij5[0];
15022 vinfos[5].indices[1] = _ij5[1];
15023 vinfos[5].maxsolutions = _nj5;
15024 std::vector<int> vfree(0);
15025 solutions.AddSolution(vinfos,vfree);
15026 }
15027 }
15028 }
15029 
15030 }
15031 
15032 }
15033 
15034 }
15035 } while(0);
15036 if( bgotonextstatement )
15037 {
15038 bool bgotonextstatement = true;
15039 do
15040 {
15041 evalcond[0]=IKabs(new_r10);
15042 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15043 {
15044 bgotonextstatement=false;
15045 {
15046 IkReal j3eval[1];
15047 CheckValue<IkReal> x1176 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15048 if(!x1176.valid){
15049 continue;
15050 }
15051 IkReal x1175=((-1.0)*(x1176.value));
15052 sj4=0;
15053 cj4=-1.0;
15054 j4=3.14159265358979;
15055 sj5=gconst19;
15056 cj5=gconst20;
15057 j5=x1175;
15058 new_r10=0;
15059 IkReal gconst18=x1175;
15060 IkReal x1177 = new_r00*new_r00;
15061 if(IKabs(x1177)==0){
15062 continue;
15063 }
15064 IkReal gconst19=(new_r00*(pow(x1177,-0.5)));
15065 IkReal gconst20=0;
15066 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
15067 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15068 {
15069 {
15070 IkReal j3eval[1];
15071 CheckValue<IkReal> x1179 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15072 if(!x1179.valid){
15073 continue;
15074 }
15075 IkReal x1178=((-1.0)*(x1179.value));
15076 sj4=0;
15077 cj4=-1.0;
15078 j4=3.14159265358979;
15079 sj5=gconst19;
15080 cj5=gconst20;
15081 j5=x1178;
15082 new_r10=0;
15083 IkReal gconst18=x1178;
15084 IkReal x1180 = new_r00*new_r00;
15085 if(IKabs(x1180)==0){
15086 continue;
15087 }
15088 IkReal gconst19=(new_r00*(pow(x1180,-0.5)));
15089 IkReal gconst20=0;
15090 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
15091 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15092 {
15093 {
15094 IkReal j3eval[1];
15095 CheckValue<IkReal> x1182 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15096 if(!x1182.valid){
15097 continue;
15098 }
15099 IkReal x1181=((-1.0)*(x1182.value));
15100 sj4=0;
15101 cj4=-1.0;
15102 j4=3.14159265358979;
15103 sj5=gconst19;
15104 cj5=gconst20;
15105 j5=x1181;
15106 new_r10=0;
15107 IkReal gconst18=x1181;
15108 IkReal x1183 = new_r00*new_r00;
15109 if(IKabs(x1183)==0){
15110 continue;
15111 }
15112 IkReal gconst19=(new_r00*(pow(x1183,-0.5)));
15113 IkReal gconst20=0;
15114 j3eval[0]=new_r00;
15115 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15116 {
15117 continue; // 3 cases reached
15118 
15119 } else
15120 {
15121 {
15122 IkReal j3array[1], cj3array[1], sj3array[1];
15123 bool j3valid[1]={false};
15124 _nj3 = 1;
15125 CheckValue<IkReal> x1184=IKPowWithIntegerCheck(new_r00,-1);
15126 if(!x1184.valid){
15127 continue;
15128 }
15129 CheckValue<IkReal> x1185=IKPowWithIntegerCheck(gconst19,-1);
15130 if(!x1185.valid){
15131 continue;
15132 }
15133 if( IKabs(((-1.0)*gconst19*(x1184.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1185.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst19*(x1184.value)))+IKsqr((new_r01*(x1185.value)))-1) <= IKFAST_SINCOS_THRESH )
15134  continue;
15135 j3array[0]=IKatan2(((-1.0)*gconst19*(x1184.value)), (new_r01*(x1185.value)));
15136 sj3array[0]=IKsin(j3array[0]);
15137 cj3array[0]=IKcos(j3array[0]);
15138 if( j3array[0] > IKPI )
15139 {
15140  j3array[0]-=IK2PI;
15141 }
15142 else if( j3array[0] < -IKPI )
15143 { j3array[0]+=IK2PI;
15144 }
15145 j3valid[0] = true;
15146 for(int ij3 = 0; ij3 < 1; ++ij3)
15147 {
15148 if( !j3valid[ij3] )
15149 {
15150  continue;
15151 }
15152 _ij3[0] = ij3; _ij3[1] = -1;
15153 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15154 {
15155 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15156 {
15157  j3valid[iij3]=false; _ij3[1] = iij3; break;
15158 }
15159 }
15160 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15161 {
15162 IkReal evalcond[8];
15163 IkReal x1186=IKcos(j3);
15164 IkReal x1187=IKsin(j3);
15165 IkReal x1188=(gconst19*x1187);
15166 IkReal x1189=((1.0)*x1186);
15167 evalcond[0]=(new_r00*x1186);
15168 evalcond[1]=((-1.0)*gconst19*x1186);
15169 evalcond[2]=(gconst19+((new_r00*x1187)));
15170 evalcond[3]=(x1188+new_r00);
15171 evalcond[4]=((((-1.0)*gconst19*x1189))+new_r01);
15172 evalcond[5]=(new_r11+(((-1.0)*x1188)));
15173 evalcond[6]=((((-1.0)*new_r11*x1189))+((new_r01*x1187)));
15174 evalcond[7]=(((new_r01*x1186))+(((-1.0)*gconst19))+((new_r11*x1187)));
15175 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15176 {
15177 continue;
15178 }
15179 }
15180 
15181 {
15182 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15183 vinfos[0].jointtype = 1;
15184 vinfos[0].foffset = j0;
15185 vinfos[0].indices[0] = _ij0[0];
15186 vinfos[0].indices[1] = _ij0[1];
15187 vinfos[0].maxsolutions = _nj0;
15188 vinfos[1].jointtype = 1;
15189 vinfos[1].foffset = j1;
15190 vinfos[1].indices[0] = _ij1[0];
15191 vinfos[1].indices[1] = _ij1[1];
15192 vinfos[1].maxsolutions = _nj1;
15193 vinfos[2].jointtype = 1;
15194 vinfos[2].foffset = j2;
15195 vinfos[2].indices[0] = _ij2[0];
15196 vinfos[2].indices[1] = _ij2[1];
15197 vinfos[2].maxsolutions = _nj2;
15198 vinfos[3].jointtype = 1;
15199 vinfos[3].foffset = j3;
15200 vinfos[3].indices[0] = _ij3[0];
15201 vinfos[3].indices[1] = _ij3[1];
15202 vinfos[3].maxsolutions = _nj3;
15203 vinfos[4].jointtype = 1;
15204 vinfos[4].foffset = j4;
15205 vinfos[4].indices[0] = _ij4[0];
15206 vinfos[4].indices[1] = _ij4[1];
15207 vinfos[4].maxsolutions = _nj4;
15208 vinfos[5].jointtype = 1;
15209 vinfos[5].foffset = j5;
15210 vinfos[5].indices[0] = _ij5[0];
15211 vinfos[5].indices[1] = _ij5[1];
15212 vinfos[5].maxsolutions = _nj5;
15213 std::vector<int> vfree(0);
15214 solutions.AddSolution(vinfos,vfree);
15215 }
15216 }
15217 }
15218 
15219 }
15220 
15221 }
15222 
15223 } else
15224 {
15225 {
15226 IkReal j3array[1], cj3array[1], sj3array[1];
15227 bool j3valid[1]={false};
15228 _nj3 = 1;
15229 CheckValue<IkReal> x1190=IKPowWithIntegerCheck(IKsign(gconst19),-1);
15230 if(!x1190.valid){
15231 continue;
15232 }
15233 CheckValue<IkReal> x1191 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15234 if(!x1191.valid){
15235 continue;
15236 }
15237 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1190.value)))+(x1191.value));
15238 sj3array[0]=IKsin(j3array[0]);
15239 cj3array[0]=IKcos(j3array[0]);
15240 if( j3array[0] > IKPI )
15241 {
15242  j3array[0]-=IK2PI;
15243 }
15244 else if( j3array[0] < -IKPI )
15245 { j3array[0]+=IK2PI;
15246 }
15247 j3valid[0] = true;
15248 for(int ij3 = 0; ij3 < 1; ++ij3)
15249 {
15250 if( !j3valid[ij3] )
15251 {
15252  continue;
15253 }
15254 _ij3[0] = ij3; _ij3[1] = -1;
15255 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15256 {
15257 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15258 {
15259  j3valid[iij3]=false; _ij3[1] = iij3; break;
15260 }
15261 }
15262 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15263 {
15264 IkReal evalcond[8];
15265 IkReal x1192=IKcos(j3);
15266 IkReal x1193=IKsin(j3);
15267 IkReal x1194=(gconst19*x1193);
15268 IkReal x1195=((1.0)*x1192);
15269 evalcond[0]=(new_r00*x1192);
15270 evalcond[1]=((-1.0)*gconst19*x1192);
15271 evalcond[2]=(gconst19+((new_r00*x1193)));
15272 evalcond[3]=(x1194+new_r00);
15273 evalcond[4]=((((-1.0)*gconst19*x1195))+new_r01);
15274 evalcond[5]=(new_r11+(((-1.0)*x1194)));
15275 evalcond[6]=((((-1.0)*new_r11*x1195))+((new_r01*x1193)));
15276 evalcond[7]=(((new_r11*x1193))+(((-1.0)*gconst19))+((new_r01*x1192)));
15277 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15278 {
15279 continue;
15280 }
15281 }
15282 
15283 {
15284 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15285 vinfos[0].jointtype = 1;
15286 vinfos[0].foffset = j0;
15287 vinfos[0].indices[0] = _ij0[0];
15288 vinfos[0].indices[1] = _ij0[1];
15289 vinfos[0].maxsolutions = _nj0;
15290 vinfos[1].jointtype = 1;
15291 vinfos[1].foffset = j1;
15292 vinfos[1].indices[0] = _ij1[0];
15293 vinfos[1].indices[1] = _ij1[1];
15294 vinfos[1].maxsolutions = _nj1;
15295 vinfos[2].jointtype = 1;
15296 vinfos[2].foffset = j2;
15297 vinfos[2].indices[0] = _ij2[0];
15298 vinfos[2].indices[1] = _ij2[1];
15299 vinfos[2].maxsolutions = _nj2;
15300 vinfos[3].jointtype = 1;
15301 vinfos[3].foffset = j3;
15302 vinfos[3].indices[0] = _ij3[0];
15303 vinfos[3].indices[1] = _ij3[1];
15304 vinfos[3].maxsolutions = _nj3;
15305 vinfos[4].jointtype = 1;
15306 vinfos[4].foffset = j4;
15307 vinfos[4].indices[0] = _ij4[0];
15308 vinfos[4].indices[1] = _ij4[1];
15309 vinfos[4].maxsolutions = _nj4;
15310 vinfos[5].jointtype = 1;
15311 vinfos[5].foffset = j5;
15312 vinfos[5].indices[0] = _ij5[0];
15313 vinfos[5].indices[1] = _ij5[1];
15314 vinfos[5].maxsolutions = _nj5;
15315 std::vector<int> vfree(0);
15316 solutions.AddSolution(vinfos,vfree);
15317 }
15318 }
15319 }
15320 
15321 }
15322 
15323 }
15324 
15325 } else
15326 {
15327 {
15328 IkReal j3array[1], cj3array[1], sj3array[1];
15329 bool j3valid[1]={false};
15330 _nj3 = 1;
15331 CheckValue<IkReal> x1196=IKPowWithIntegerCheck(IKsign(gconst19),-1);
15332 if(!x1196.valid){
15333 continue;
15334 }
15335 CheckValue<IkReal> x1197 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15336 if(!x1197.valid){
15337 continue;
15338 }
15339 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1196.value)))+(x1197.value));
15340 sj3array[0]=IKsin(j3array[0]);
15341 cj3array[0]=IKcos(j3array[0]);
15342 if( j3array[0] > IKPI )
15343 {
15344  j3array[0]-=IK2PI;
15345 }
15346 else if( j3array[0] < -IKPI )
15347 { j3array[0]+=IK2PI;
15348 }
15349 j3valid[0] = true;
15350 for(int ij3 = 0; ij3 < 1; ++ij3)
15351 {
15352 if( !j3valid[ij3] )
15353 {
15354  continue;
15355 }
15356 _ij3[0] = ij3; _ij3[1] = -1;
15357 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15358 {
15359 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15360 {
15361  j3valid[iij3]=false; _ij3[1] = iij3; break;
15362 }
15363 }
15364 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15365 {
15366 IkReal evalcond[8];
15367 IkReal x1198=IKcos(j3);
15368 IkReal x1199=IKsin(j3);
15369 IkReal x1200=(gconst19*x1199);
15370 IkReal x1201=((1.0)*x1198);
15371 evalcond[0]=(new_r00*x1198);
15372 evalcond[1]=((-1.0)*gconst19*x1198);
15373 evalcond[2]=(gconst19+((new_r00*x1199)));
15374 evalcond[3]=(x1200+new_r00);
15375 evalcond[4]=((((-1.0)*gconst19*x1201))+new_r01);
15376 evalcond[5]=((((-1.0)*x1200))+new_r11);
15377 evalcond[6]=((((-1.0)*new_r11*x1201))+((new_r01*x1199)));
15378 evalcond[7]=(((new_r11*x1199))+(((-1.0)*gconst19))+((new_r01*x1198)));
15379 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15380 {
15381 continue;
15382 }
15383 }
15384 
15385 {
15386 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15387 vinfos[0].jointtype = 1;
15388 vinfos[0].foffset = j0;
15389 vinfos[0].indices[0] = _ij0[0];
15390 vinfos[0].indices[1] = _ij0[1];
15391 vinfos[0].maxsolutions = _nj0;
15392 vinfos[1].jointtype = 1;
15393 vinfos[1].foffset = j1;
15394 vinfos[1].indices[0] = _ij1[0];
15395 vinfos[1].indices[1] = _ij1[1];
15396 vinfos[1].maxsolutions = _nj1;
15397 vinfos[2].jointtype = 1;
15398 vinfos[2].foffset = j2;
15399 vinfos[2].indices[0] = _ij2[0];
15400 vinfos[2].indices[1] = _ij2[1];
15401 vinfos[2].maxsolutions = _nj2;
15402 vinfos[3].jointtype = 1;
15403 vinfos[3].foffset = j3;
15404 vinfos[3].indices[0] = _ij3[0];
15405 vinfos[3].indices[1] = _ij3[1];
15406 vinfos[3].maxsolutions = _nj3;
15407 vinfos[4].jointtype = 1;
15408 vinfos[4].foffset = j4;
15409 vinfos[4].indices[0] = _ij4[0];
15410 vinfos[4].indices[1] = _ij4[1];
15411 vinfos[4].maxsolutions = _nj4;
15412 vinfos[5].jointtype = 1;
15413 vinfos[5].foffset = j5;
15414 vinfos[5].indices[0] = _ij5[0];
15415 vinfos[5].indices[1] = _ij5[1];
15416 vinfos[5].maxsolutions = _nj5;
15417 std::vector<int> vfree(0);
15418 solutions.AddSolution(vinfos,vfree);
15419 }
15420 }
15421 }
15422 
15423 }
15424 
15425 }
15426 
15427 }
15428 } while(0);
15429 if( bgotonextstatement )
15430 {
15431 bool bgotonextstatement = true;
15432 do
15433 {
15434 if( 1 )
15435 {
15436 bgotonextstatement=false;
15437 continue; // branch miss [j3]
15438 
15439 }
15440 } while(0);
15441 if( bgotonextstatement )
15442 {
15443 }
15444 }
15445 }
15446 }
15447 }
15448 }
15449 }
15450 
15451 } else
15452 {
15453 {
15454 IkReal j3array[1], cj3array[1], sj3array[1];
15455 bool j3valid[1]={false};
15456 _nj3 = 1;
15457 IkReal x1202=((1.0)*gconst20);
15458 CheckValue<IkReal> x1203=IKPowWithIntegerCheck(IKsign((((gconst19*new_r00))+(((-1.0)*new_r10*x1202)))),-1);
15459 if(!x1203.valid){
15460 continue;
15461 }
15462 CheckValue<IkReal> x1204 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst20*gconst20))),IkReal(((((-1.0)*gconst19*x1202))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
15463 if(!x1204.valid){
15464 continue;
15465 }
15466 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1203.value)))+(x1204.value));
15467 sj3array[0]=IKsin(j3array[0]);
15468 cj3array[0]=IKcos(j3array[0]);
15469 if( j3array[0] > IKPI )
15470 {
15471  j3array[0]-=IK2PI;
15472 }
15473 else if( j3array[0] < -IKPI )
15474 { j3array[0]+=IK2PI;
15475 }
15476 j3valid[0] = true;
15477 for(int ij3 = 0; ij3 < 1; ++ij3)
15478 {
15479 if( !j3valid[ij3] )
15480 {
15481  continue;
15482 }
15483 _ij3[0] = ij3; _ij3[1] = -1;
15484 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15485 {
15486 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15487 {
15488  j3valid[iij3]=false; _ij3[1] = iij3; break;
15489 }
15490 }
15491 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15492 {
15493 IkReal evalcond[8];
15494 IkReal x1205=IKcos(j3);
15495 IkReal x1206=IKsin(j3);
15496 IkReal x1207=(gconst20*x1206);
15497 IkReal x1208=((1.0)*x1205);
15498 IkReal x1209=(gconst19*x1206);
15499 IkReal x1210=(gconst19*x1208);
15500 evalcond[0]=(((new_r00*x1205))+((new_r10*x1206))+gconst20);
15501 evalcond[1]=(((gconst20*x1205))+x1209+new_r00);
15502 evalcond[2]=(((new_r00*x1206))+gconst19+(((-1.0)*new_r10*x1208)));
15503 evalcond[3]=(((new_r01*x1206))+(((-1.0)*new_r11*x1208))+gconst20);
15504 evalcond[4]=(x1207+new_r01+(((-1.0)*x1210)));
15505 evalcond[5]=(x1207+new_r10+(((-1.0)*x1210)));
15506 evalcond[6]=(((new_r11*x1206))+((new_r01*x1205))+(((-1.0)*gconst19)));
15507 evalcond[7]=((((-1.0)*x1209))+new_r11+(((-1.0)*gconst20*x1208)));
15508 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15509 {
15510 continue;
15511 }
15512 }
15513 
15514 {
15515 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15516 vinfos[0].jointtype = 1;
15517 vinfos[0].foffset = j0;
15518 vinfos[0].indices[0] = _ij0[0];
15519 vinfos[0].indices[1] = _ij0[1];
15520 vinfos[0].maxsolutions = _nj0;
15521 vinfos[1].jointtype = 1;
15522 vinfos[1].foffset = j1;
15523 vinfos[1].indices[0] = _ij1[0];
15524 vinfos[1].indices[1] = _ij1[1];
15525 vinfos[1].maxsolutions = _nj1;
15526 vinfos[2].jointtype = 1;
15527 vinfos[2].foffset = j2;
15528 vinfos[2].indices[0] = _ij2[0];
15529 vinfos[2].indices[1] = _ij2[1];
15530 vinfos[2].maxsolutions = _nj2;
15531 vinfos[3].jointtype = 1;
15532 vinfos[3].foffset = j3;
15533 vinfos[3].indices[0] = _ij3[0];
15534 vinfos[3].indices[1] = _ij3[1];
15535 vinfos[3].maxsolutions = _nj3;
15536 vinfos[4].jointtype = 1;
15537 vinfos[4].foffset = j4;
15538 vinfos[4].indices[0] = _ij4[0];
15539 vinfos[4].indices[1] = _ij4[1];
15540 vinfos[4].maxsolutions = _nj4;
15541 vinfos[5].jointtype = 1;
15542 vinfos[5].foffset = j5;
15543 vinfos[5].indices[0] = _ij5[0];
15544 vinfos[5].indices[1] = _ij5[1];
15545 vinfos[5].maxsolutions = _nj5;
15546 std::vector<int> vfree(0);
15547 solutions.AddSolution(vinfos,vfree);
15548 }
15549 }
15550 }
15551 
15552 }
15553 
15554 }
15555 
15556 } else
15557 {
15558 {
15559 IkReal j3array[1], cj3array[1], sj3array[1];
15560 bool j3valid[1]={false};
15561 _nj3 = 1;
15562 IkReal x1211=((1.0)*gconst19);
15563 CheckValue<IkReal> x1212=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst20*new_r01))+(((-1.0)*new_r11*x1211)))),-1);
15564 if(!x1212.valid){
15565 continue;
15566 }
15567 CheckValue<IkReal> x1213 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst20*gconst20))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst20*x1211)))),IKFAST_ATAN2_MAGTHRESH);
15568 if(!x1213.valid){
15569 continue;
15570 }
15571 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1212.value)))+(x1213.value));
15572 sj3array[0]=IKsin(j3array[0]);
15573 cj3array[0]=IKcos(j3array[0]);
15574 if( j3array[0] > IKPI )
15575 {
15576  j3array[0]-=IK2PI;
15577 }
15578 else if( j3array[0] < -IKPI )
15579 { j3array[0]+=IK2PI;
15580 }
15581 j3valid[0] = true;
15582 for(int ij3 = 0; ij3 < 1; ++ij3)
15583 {
15584 if( !j3valid[ij3] )
15585 {
15586  continue;
15587 }
15588 _ij3[0] = ij3; _ij3[1] = -1;
15589 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15590 {
15591 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15592 {
15593  j3valid[iij3]=false; _ij3[1] = iij3; break;
15594 }
15595 }
15596 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15597 {
15598 IkReal evalcond[8];
15599 IkReal x1214=IKcos(j3);
15600 IkReal x1215=IKsin(j3);
15601 IkReal x1216=(gconst20*x1215);
15602 IkReal x1217=((1.0)*x1214);
15603 IkReal x1218=(gconst19*x1215);
15604 IkReal x1219=(gconst19*x1217);
15605 evalcond[0]=(((new_r10*x1215))+gconst20+((new_r00*x1214)));
15606 evalcond[1]=(x1218+((gconst20*x1214))+new_r00);
15607 evalcond[2]=(gconst19+((new_r00*x1215))+(((-1.0)*new_r10*x1217)));
15608 evalcond[3]=(gconst20+((new_r01*x1215))+(((-1.0)*new_r11*x1217)));
15609 evalcond[4]=(x1216+new_r01+(((-1.0)*x1219)));
15610 evalcond[5]=(x1216+new_r10+(((-1.0)*x1219)));
15611 evalcond[6]=(((new_r11*x1215))+((new_r01*x1214))+(((-1.0)*gconst19)));
15612 evalcond[7]=((((-1.0)*gconst20*x1217))+new_r11+(((-1.0)*x1218)));
15613 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15614 {
15615 continue;
15616 }
15617 }
15618 
15619 {
15620 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15621 vinfos[0].jointtype = 1;
15622 vinfos[0].foffset = j0;
15623 vinfos[0].indices[0] = _ij0[0];
15624 vinfos[0].indices[1] = _ij0[1];
15625 vinfos[0].maxsolutions = _nj0;
15626 vinfos[1].jointtype = 1;
15627 vinfos[1].foffset = j1;
15628 vinfos[1].indices[0] = _ij1[0];
15629 vinfos[1].indices[1] = _ij1[1];
15630 vinfos[1].maxsolutions = _nj1;
15631 vinfos[2].jointtype = 1;
15632 vinfos[2].foffset = j2;
15633 vinfos[2].indices[0] = _ij2[0];
15634 vinfos[2].indices[1] = _ij2[1];
15635 vinfos[2].maxsolutions = _nj2;
15636 vinfos[3].jointtype = 1;
15637 vinfos[3].foffset = j3;
15638 vinfos[3].indices[0] = _ij3[0];
15639 vinfos[3].indices[1] = _ij3[1];
15640 vinfos[3].maxsolutions = _nj3;
15641 vinfos[4].jointtype = 1;
15642 vinfos[4].foffset = j4;
15643 vinfos[4].indices[0] = _ij4[0];
15644 vinfos[4].indices[1] = _ij4[1];
15645 vinfos[4].maxsolutions = _nj4;
15646 vinfos[5].jointtype = 1;
15647 vinfos[5].foffset = j5;
15648 vinfos[5].indices[0] = _ij5[0];
15649 vinfos[5].indices[1] = _ij5[1];
15650 vinfos[5].maxsolutions = _nj5;
15651 std::vector<int> vfree(0);
15652 solutions.AddSolution(vinfos,vfree);
15653 }
15654 }
15655 }
15656 
15657 }
15658 
15659 }
15660 
15661 } else
15662 {
15663 {
15664 IkReal j3array[1], cj3array[1], sj3array[1];
15665 bool j3valid[1]={false};
15666 _nj3 = 1;
15667 IkReal x1220=((1.0)*new_r10);
15668 CheckValue<IkReal> x1221=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1220)))),-1);
15669 if(!x1221.valid){
15670 continue;
15671 }
15672 CheckValue<IkReal> x1222 = IKatan2WithCheck(IkReal((((gconst20*new_r00))+((gconst20*new_r11)))),IkReal((((gconst20*new_r01))+(((-1.0)*gconst20*x1220)))),IKFAST_ATAN2_MAGTHRESH);
15673 if(!x1222.valid){
15674 continue;
15675 }
15676 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1221.value)))+(x1222.value));
15677 sj3array[0]=IKsin(j3array[0]);
15678 cj3array[0]=IKcos(j3array[0]);
15679 if( j3array[0] > IKPI )
15680 {
15681  j3array[0]-=IK2PI;
15682 }
15683 else if( j3array[0] < -IKPI )
15684 { j3array[0]+=IK2PI;
15685 }
15686 j3valid[0] = true;
15687 for(int ij3 = 0; ij3 < 1; ++ij3)
15688 {
15689 if( !j3valid[ij3] )
15690 {
15691  continue;
15692 }
15693 _ij3[0] = ij3; _ij3[1] = -1;
15694 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15695 {
15696 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15697 {
15698  j3valid[iij3]=false; _ij3[1] = iij3; break;
15699 }
15700 }
15701 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15702 {
15703 IkReal evalcond[8];
15704 IkReal x1223=IKcos(j3);
15705 IkReal x1224=IKsin(j3);
15706 IkReal x1225=(gconst20*x1224);
15707 IkReal x1226=((1.0)*x1223);
15708 IkReal x1227=(gconst19*x1224);
15709 IkReal x1228=(gconst19*x1226);
15710 evalcond[0]=(gconst20+((new_r00*x1223))+((new_r10*x1224)));
15711 evalcond[1]=(x1227+((gconst20*x1223))+new_r00);
15712 evalcond[2]=(gconst19+((new_r00*x1224))+(((-1.0)*new_r10*x1226)));
15713 evalcond[3]=(gconst20+((new_r01*x1224))+(((-1.0)*new_r11*x1226)));
15714 evalcond[4]=(x1225+(((-1.0)*x1228))+new_r01);
15715 evalcond[5]=(x1225+(((-1.0)*x1228))+new_r10);
15716 evalcond[6]=(((new_r11*x1224))+((new_r01*x1223))+(((-1.0)*gconst19)));
15717 evalcond[7]=((((-1.0)*x1227))+(((-1.0)*gconst20*x1226))+new_r11);
15718 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15719 {
15720 continue;
15721 }
15722 }
15723 
15724 {
15725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15726 vinfos[0].jointtype = 1;
15727 vinfos[0].foffset = j0;
15728 vinfos[0].indices[0] = _ij0[0];
15729 vinfos[0].indices[1] = _ij0[1];
15730 vinfos[0].maxsolutions = _nj0;
15731 vinfos[1].jointtype = 1;
15732 vinfos[1].foffset = j1;
15733 vinfos[1].indices[0] = _ij1[0];
15734 vinfos[1].indices[1] = _ij1[1];
15735 vinfos[1].maxsolutions = _nj1;
15736 vinfos[2].jointtype = 1;
15737 vinfos[2].foffset = j2;
15738 vinfos[2].indices[0] = _ij2[0];
15739 vinfos[2].indices[1] = _ij2[1];
15740 vinfos[2].maxsolutions = _nj2;
15741 vinfos[3].jointtype = 1;
15742 vinfos[3].foffset = j3;
15743 vinfos[3].indices[0] = _ij3[0];
15744 vinfos[3].indices[1] = _ij3[1];
15745 vinfos[3].maxsolutions = _nj3;
15746 vinfos[4].jointtype = 1;
15747 vinfos[4].foffset = j4;
15748 vinfos[4].indices[0] = _ij4[0];
15749 vinfos[4].indices[1] = _ij4[1];
15750 vinfos[4].maxsolutions = _nj4;
15751 vinfos[5].jointtype = 1;
15752 vinfos[5].foffset = j5;
15753 vinfos[5].indices[0] = _ij5[0];
15754 vinfos[5].indices[1] = _ij5[1];
15755 vinfos[5].maxsolutions = _nj5;
15756 std::vector<int> vfree(0);
15757 solutions.AddSolution(vinfos,vfree);
15758 }
15759 }
15760 }
15761 
15762 }
15763 
15764 }
15765 
15766 }
15767 } while(0);
15768 if( bgotonextstatement )
15769 {
15770 bool bgotonextstatement = true;
15771 do
15772 {
15773 IkReal x1229=((-1.0)*new_r00);
15774 IkReal x1231 = ((new_r10*new_r10)+(new_r00*new_r00));
15775 if(IKabs(x1231)==0){
15776 continue;
15777 }
15778 IkReal x1230=pow(x1231,-0.5);
15779 CheckValue<IkReal> x1232 = IKatan2WithCheck(IkReal(x1229),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15780 if(!x1232.valid){
15781 continue;
15782 }
15783 IkReal gconst21=((3.14159265358979)+(((-1.0)*(x1232.value))));
15784 IkReal gconst22=(x1229*x1230);
15785 IkReal gconst23=((1.0)*new_r10*x1230);
15786 CheckValue<IkReal> x1233 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15787 if(!x1233.valid){
15788 continue;
15789 }
15790 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x1233.value)+j5)))), 6.28318530717959)));
15791 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15792 {
15793 bgotonextstatement=false;
15794 {
15795 IkReal j3eval[3];
15796 IkReal x1234=((-1.0)*new_r00);
15797 CheckValue<IkReal> x1237 = IKatan2WithCheck(IkReal(x1234),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15798 if(!x1237.valid){
15799 continue;
15800 }
15801 IkReal x1235=((1.0)*(x1237.value));
15802 IkReal x1236=x1230;
15803 sj4=0;
15804 cj4=-1.0;
15805 j4=3.14159265358979;
15806 sj5=gconst22;
15807 cj5=gconst23;
15808 j5=((3.14159265)+(((-1.0)*x1235)));
15809 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1235)));
15810 IkReal gconst22=(x1234*x1236);
15811 IkReal gconst23=((1.0)*new_r10*x1236);
15812 IkReal x1238=new_r10*new_r10;
15813 IkReal x1239=(new_r10*new_r11);
15814 IkReal x1240=((((-1.0)*x1239))+(((-1.0)*new_r00*new_r01)));
15815 IkReal x1241=x1230;
15816 IkReal x1242=(new_r10*x1241);
15817 j3eval[0]=x1240;
15818 j3eval[1]=((IKabs(((((-1.0)*x1238*x1241))+((new_r01*x1242)))))+(IKabs((((x1239*x1241))+((new_r00*x1242))))));
15819 j3eval[2]=IKsign(x1240);
15820 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
15821 {
15822 {
15823 IkReal j3eval[1];
15824 IkReal x1243=((-1.0)*new_r00);
15825 CheckValue<IkReal> x1246 = IKatan2WithCheck(IkReal(x1243),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15826 if(!x1246.valid){
15827 continue;
15828 }
15829 IkReal x1244=((1.0)*(x1246.value));
15830 IkReal x1245=x1230;
15831 sj4=0;
15832 cj4=-1.0;
15833 j4=3.14159265358979;
15834 sj5=gconst22;
15835 cj5=gconst23;
15836 j5=((3.14159265)+(((-1.0)*x1244)));
15837 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1244)));
15838 IkReal gconst22=(x1243*x1245);
15839 IkReal gconst23=((1.0)*new_r10*x1245);
15840 IkReal x1247=new_r10*new_r10;
15841 IkReal x1248=new_r00*new_r00*new_r00;
15842 CheckValue<IkReal> x1252=IKPowWithIntegerCheck((x1247+(new_r00*new_r00)),-1);
15843 if(!x1252.valid){
15844 continue;
15845 }
15846 IkReal x1249=x1252.value;
15847 IkReal x1250=(x1247*x1249);
15848 IkReal x1251=(x1248*x1249);
15849 j3eval[0]=((IKabs((x1250+((new_r11*x1251))+((new_r00*new_r11*x1250)))))+(IKabs((((new_r01*x1251))+((new_r00*new_r01*x1250))+((new_r00*new_r10*x1249))))));
15850 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15851 {
15852 {
15853 IkReal j3eval[1];
15854 IkReal x1253=((-1.0)*new_r00);
15855 CheckValue<IkReal> x1256 = IKatan2WithCheck(IkReal(x1253),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15856 if(!x1256.valid){
15857 continue;
15858 }
15859 IkReal x1254=((1.0)*(x1256.value));
15860 IkReal x1255=x1230;
15861 sj4=0;
15862 cj4=-1.0;
15863 j4=3.14159265358979;
15864 sj5=gconst22;
15865 cj5=gconst23;
15866 j5=((3.14159265)+(((-1.0)*x1254)));
15867 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1254)));
15868 IkReal gconst22=(x1253*x1255);
15869 IkReal gconst23=((1.0)*new_r10*x1255);
15870 IkReal x1257=new_r10*new_r10;
15871 IkReal x1258=new_r00*new_r00;
15872 CheckValue<IkReal> x1262=IKPowWithIntegerCheck((x1257+x1258),-1);
15873 if(!x1262.valid){
15874 continue;
15875 }
15876 IkReal x1259=x1262.value;
15877 IkReal x1260=(new_r10*x1259);
15878 IkReal x1261=(x1257*x1259);
15879 j3eval[0]=((IKabs((x1261+(((-1.0)*x1259*(x1258*x1258)))+(((-1.0)*x1258*x1261)))))+(IKabs((((new_r00*x1260*(new_r10*new_r10)))+((new_r00*x1260))+((x1260*(new_r00*new_r00*new_r00)))))));
15880 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15881 {
15882 {
15883 IkReal evalcond[3];
15884 bool bgotonextstatement = true;
15885 do
15886 {
15887 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
15888 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15889 {
15890 bgotonextstatement=false;
15891 {
15892 IkReal j3eval[1];
15893 CheckValue<IkReal> x1264 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15894 if(!x1264.valid){
15895 continue;
15896 }
15897 IkReal x1263=((1.0)*(x1264.value));
15898 sj4=0;
15899 cj4=-1.0;
15900 j4=3.14159265358979;
15901 sj5=gconst22;
15902 cj5=gconst23;
15903 j5=((3.14159265)+(((-1.0)*x1263)));
15904 new_r11=0;
15905 new_r00=0;
15906 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1263)));
15907 IkReal gconst22=0;
15908 IkReal x1265 = new_r10*new_r10;
15909 if(IKabs(x1265)==0){
15910 continue;
15911 }
15912 IkReal gconst23=((1.0)*new_r10*(pow(x1265,-0.5)));
15913 j3eval[0]=new_r01;
15914 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15915 {
15916 {
15917 IkReal j3array[2], cj3array[2], sj3array[2];
15918 bool j3valid[2]={false};
15919 _nj3 = 2;
15920 CheckValue<IkReal> x1266=IKPowWithIntegerCheck(gconst23,-1);
15921 if(!x1266.valid){
15922 continue;
15923 }
15924 sj3array[0]=((-1.0)*new_r01*(x1266.value));
15925 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
15926 {
15927  j3valid[0] = j3valid[1] = true;
15928  j3array[0] = IKasin(sj3array[0]);
15929  cj3array[0] = IKcos(j3array[0]);
15930  sj3array[1] = sj3array[0];
15931  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
15932  cj3array[1] = -cj3array[0];
15933 }
15934 else if( isnan(sj3array[0]) )
15935 {
15936  // probably any value will work
15937  j3valid[0] = true;
15938  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
15939 }
15940 for(int ij3 = 0; ij3 < 2; ++ij3)
15941 {
15942 if( !j3valid[ij3] )
15943 {
15944  continue;
15945 }
15946 _ij3[0] = ij3; _ij3[1] = -1;
15947 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
15948 {
15949 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15950 {
15951  j3valid[iij3]=false; _ij3[1] = iij3; break;
15952 }
15953 }
15954 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15955 {
15956 IkReal evalcond[6];
15957 IkReal x1267=IKcos(j3);
15958 IkReal x1268=IKsin(j3);
15959 evalcond[0]=(new_r01*x1267);
15960 evalcond[1]=(gconst23*x1267);
15961 evalcond[2]=((-1.0)*new_r10*x1267);
15962 evalcond[3]=(((new_r01*x1268))+gconst23);
15963 evalcond[4]=(gconst23+((new_r10*x1268)));
15964 evalcond[5]=(((gconst23*x1268))+new_r10);
15965 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
15966 {
15967 continue;
15968 }
15969 }
15970 
15971 {
15972 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15973 vinfos[0].jointtype = 1;
15974 vinfos[0].foffset = j0;
15975 vinfos[0].indices[0] = _ij0[0];
15976 vinfos[0].indices[1] = _ij0[1];
15977 vinfos[0].maxsolutions = _nj0;
15978 vinfos[1].jointtype = 1;
15979 vinfos[1].foffset = j1;
15980 vinfos[1].indices[0] = _ij1[0];
15981 vinfos[1].indices[1] = _ij1[1];
15982 vinfos[1].maxsolutions = _nj1;
15983 vinfos[2].jointtype = 1;
15984 vinfos[2].foffset = j2;
15985 vinfos[2].indices[0] = _ij2[0];
15986 vinfos[2].indices[1] = _ij2[1];
15987 vinfos[2].maxsolutions = _nj2;
15988 vinfos[3].jointtype = 1;
15989 vinfos[3].foffset = j3;
15990 vinfos[3].indices[0] = _ij3[0];
15991 vinfos[3].indices[1] = _ij3[1];
15992 vinfos[3].maxsolutions = _nj3;
15993 vinfos[4].jointtype = 1;
15994 vinfos[4].foffset = j4;
15995 vinfos[4].indices[0] = _ij4[0];
15996 vinfos[4].indices[1] = _ij4[1];
15997 vinfos[4].maxsolutions = _nj4;
15998 vinfos[5].jointtype = 1;
15999 vinfos[5].foffset = j5;
16000 vinfos[5].indices[0] = _ij5[0];
16001 vinfos[5].indices[1] = _ij5[1];
16002 vinfos[5].maxsolutions = _nj5;
16003 std::vector<int> vfree(0);
16004 solutions.AddSolution(vinfos,vfree);
16005 }
16006 }
16007 }
16008 
16009 } else
16010 {
16011 {
16012 IkReal j3array[2], cj3array[2], sj3array[2];
16013 bool j3valid[2]={false};
16014 _nj3 = 2;
16015 CheckValue<IkReal> x1269=IKPowWithIntegerCheck(new_r01,-1);
16016 if(!x1269.valid){
16017 continue;
16018 }
16019 sj3array[0]=((-1.0)*gconst23*(x1269.value));
16020 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16021 {
16022  j3valid[0] = j3valid[1] = true;
16023  j3array[0] = IKasin(sj3array[0]);
16024  cj3array[0] = IKcos(j3array[0]);
16025  sj3array[1] = sj3array[0];
16026  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16027  cj3array[1] = -cj3array[0];
16028 }
16029 else if( isnan(sj3array[0]) )
16030 {
16031  // probably any value will work
16032  j3valid[0] = true;
16033  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16034 }
16035 for(int ij3 = 0; ij3 < 2; ++ij3)
16036 {
16037 if( !j3valid[ij3] )
16038 {
16039  continue;
16040 }
16041 _ij3[0] = ij3; _ij3[1] = -1;
16042 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16043 {
16044 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16045 {
16046  j3valid[iij3]=false; _ij3[1] = iij3; break;
16047 }
16048 }
16049 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16050 {
16051 IkReal evalcond[6];
16052 IkReal x1270=IKcos(j3);
16053 IkReal x1271=IKsin(j3);
16054 IkReal x1272=(gconst23*x1271);
16055 evalcond[0]=(new_r01*x1270);
16056 evalcond[1]=(gconst23*x1270);
16057 evalcond[2]=((-1.0)*new_r10*x1270);
16058 evalcond[3]=(x1272+new_r01);
16059 evalcond[4]=(gconst23+((new_r10*x1271)));
16060 evalcond[5]=(x1272+new_r10);
16061 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16062 {
16063 continue;
16064 }
16065 }
16066 
16067 {
16068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16069 vinfos[0].jointtype = 1;
16070 vinfos[0].foffset = j0;
16071 vinfos[0].indices[0] = _ij0[0];
16072 vinfos[0].indices[1] = _ij0[1];
16073 vinfos[0].maxsolutions = _nj0;
16074 vinfos[1].jointtype = 1;
16075 vinfos[1].foffset = j1;
16076 vinfos[1].indices[0] = _ij1[0];
16077 vinfos[1].indices[1] = _ij1[1];
16078 vinfos[1].maxsolutions = _nj1;
16079 vinfos[2].jointtype = 1;
16080 vinfos[2].foffset = j2;
16081 vinfos[2].indices[0] = _ij2[0];
16082 vinfos[2].indices[1] = _ij2[1];
16083 vinfos[2].maxsolutions = _nj2;
16084 vinfos[3].jointtype = 1;
16085 vinfos[3].foffset = j3;
16086 vinfos[3].indices[0] = _ij3[0];
16087 vinfos[3].indices[1] = _ij3[1];
16088 vinfos[3].maxsolutions = _nj3;
16089 vinfos[4].jointtype = 1;
16090 vinfos[4].foffset = j4;
16091 vinfos[4].indices[0] = _ij4[0];
16092 vinfos[4].indices[1] = _ij4[1];
16093 vinfos[4].maxsolutions = _nj4;
16094 vinfos[5].jointtype = 1;
16095 vinfos[5].foffset = j5;
16096 vinfos[5].indices[0] = _ij5[0];
16097 vinfos[5].indices[1] = _ij5[1];
16098 vinfos[5].maxsolutions = _nj5;
16099 std::vector<int> vfree(0);
16100 solutions.AddSolution(vinfos,vfree);
16101 }
16102 }
16103 }
16104 
16105 }
16106 
16107 }
16108 
16109 }
16110 } while(0);
16111 if( bgotonextstatement )
16112 {
16113 bool bgotonextstatement = true;
16114 do
16115 {
16116 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
16117 evalcond[1]=gconst23;
16118 evalcond[2]=gconst22;
16119 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
16120 {
16121 bgotonextstatement=false;
16122 {
16123 IkReal j3eval[3];
16124 IkReal x1273=((-1.0)*new_r00);
16125 CheckValue<IkReal> x1275 = IKatan2WithCheck(IkReal(x1273),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16126 if(!x1275.valid){
16127 continue;
16128 }
16129 IkReal x1274=((1.0)*(x1275.value));
16130 sj4=0;
16131 cj4=-1.0;
16132 j4=3.14159265358979;
16133 sj5=gconst22;
16134 cj5=gconst23;
16135 j5=((3.14159265)+(((-1.0)*x1274)));
16136 new_r11=0;
16137 new_r01=0;
16138 new_r22=0;
16139 new_r20=0;
16140 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1274)));
16141 IkReal gconst22=x1273;
16142 IkReal gconst23=((1.0)*new_r10);
16143 j3eval[0]=1.0;
16144 j3eval[1]=1.0;
16145 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
16146 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16147 {
16148 {
16149 IkReal j3eval[3];
16150 IkReal x1276=((-1.0)*new_r00);
16151 CheckValue<IkReal> x1278 = IKatan2WithCheck(IkReal(x1276),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16152 if(!x1278.valid){
16153 continue;
16154 }
16155 IkReal x1277=((1.0)*(x1278.value));
16156 sj4=0;
16157 cj4=-1.0;
16158 j4=3.14159265358979;
16159 sj5=gconst22;
16160 cj5=gconst23;
16161 j5=((3.14159265)+(((-1.0)*x1277)));
16162 new_r11=0;
16163 new_r01=0;
16164 new_r22=0;
16165 new_r20=0;
16166 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1277)));
16167 IkReal gconst22=x1276;
16168 IkReal gconst23=((1.0)*new_r10);
16169 j3eval[0]=-1.0;
16170 j3eval[1]=((IKabs(((1.0)*new_r00*new_r10)))+(IKabs(((-1.0)+(new_r10*new_r10)))));
16171 j3eval[2]=-1.0;
16172 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16173 {
16174 {
16175 IkReal j3eval[3];
16176 IkReal x1279=((-1.0)*new_r00);
16177 CheckValue<IkReal> x1281 = IKatan2WithCheck(IkReal(x1279),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16178 if(!x1281.valid){
16179 continue;
16180 }
16181 IkReal x1280=((1.0)*(x1281.value));
16182 sj4=0;
16183 cj4=-1.0;
16184 j4=3.14159265358979;
16185 sj5=gconst22;
16186 cj5=gconst23;
16187 j5=((3.14159265)+(((-1.0)*x1280)));
16188 new_r11=0;
16189 new_r01=0;
16190 new_r22=0;
16191 new_r20=0;
16192 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1280)));
16193 IkReal gconst22=x1279;
16194 IkReal gconst23=((1.0)*new_r10);
16195 j3eval[0]=-1.0;
16196 j3eval[1]=((IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
16197 j3eval[2]=-1.0;
16198 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16199 {
16200 continue; // 3 cases reached
16201 
16202 } else
16203 {
16204 {
16205 IkReal j3array[1], cj3array[1], sj3array[1];
16206 bool j3valid[1]={false};
16207 _nj3 = 1;
16208 IkReal x1282=((1.0)*gconst23);
16209 CheckValue<IkReal> x1283=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x1282))+((gconst22*new_r00)))),-1);
16210 if(!x1283.valid){
16211 continue;
16212 }
16213 CheckValue<IkReal> x1284 = IKatan2WithCheck(IkReal(((gconst23*gconst23)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst22*x1282)))),IKFAST_ATAN2_MAGTHRESH);
16214 if(!x1284.valid){
16215 continue;
16216 }
16217 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1283.value)))+(x1284.value));
16218 sj3array[0]=IKsin(j3array[0]);
16219 cj3array[0]=IKcos(j3array[0]);
16220 if( j3array[0] > IKPI )
16221 {
16222  j3array[0]-=IK2PI;
16223 }
16224 else if( j3array[0] < -IKPI )
16225 { j3array[0]+=IK2PI;
16226 }
16227 j3valid[0] = true;
16228 for(int ij3 = 0; ij3 < 1; ++ij3)
16229 {
16230 if( !j3valid[ij3] )
16231 {
16232  continue;
16233 }
16234 _ij3[0] = ij3; _ij3[1] = -1;
16235 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16236 {
16237 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16238 {
16239  j3valid[iij3]=false; _ij3[1] = iij3; break;
16240 }
16241 }
16242 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16243 {
16244 IkReal evalcond[6];
16245 IkReal x1285=IKsin(j3);
16246 IkReal x1286=IKcos(j3);
16247 IkReal x1287=(gconst23*x1285);
16248 IkReal x1288=(gconst22*x1285);
16249 IkReal x1289=(gconst23*x1286);
16250 IkReal x1290=((1.0)*x1286);
16251 IkReal x1291=(gconst22*x1290);
16252 evalcond[0]=(x1287+(((-1.0)*x1291)));
16253 evalcond[1]=(((new_r10*x1285))+gconst23+((new_r00*x1286)));
16254 evalcond[2]=(x1289+x1288+new_r00);
16255 evalcond[3]=((((-1.0)*new_r10*x1290))+gconst22+((new_r00*x1285)));
16256 evalcond[4]=((((-1.0)*x1288))+(((-1.0)*x1289)));
16257 evalcond[5]=(x1287+(((-1.0)*x1291))+new_r10);
16258 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16259 {
16260 continue;
16261 }
16262 }
16263 
16264 {
16265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16266 vinfos[0].jointtype = 1;
16267 vinfos[0].foffset = j0;
16268 vinfos[0].indices[0] = _ij0[0];
16269 vinfos[0].indices[1] = _ij0[1];
16270 vinfos[0].maxsolutions = _nj0;
16271 vinfos[1].jointtype = 1;
16272 vinfos[1].foffset = j1;
16273 vinfos[1].indices[0] = _ij1[0];
16274 vinfos[1].indices[1] = _ij1[1];
16275 vinfos[1].maxsolutions = _nj1;
16276 vinfos[2].jointtype = 1;
16277 vinfos[2].foffset = j2;
16278 vinfos[2].indices[0] = _ij2[0];
16279 vinfos[2].indices[1] = _ij2[1];
16280 vinfos[2].maxsolutions = _nj2;
16281 vinfos[3].jointtype = 1;
16282 vinfos[3].foffset = j3;
16283 vinfos[3].indices[0] = _ij3[0];
16284 vinfos[3].indices[1] = _ij3[1];
16285 vinfos[3].maxsolutions = _nj3;
16286 vinfos[4].jointtype = 1;
16287 vinfos[4].foffset = j4;
16288 vinfos[4].indices[0] = _ij4[0];
16289 vinfos[4].indices[1] = _ij4[1];
16290 vinfos[4].maxsolutions = _nj4;
16291 vinfos[5].jointtype = 1;
16292 vinfos[5].foffset = j5;
16293 vinfos[5].indices[0] = _ij5[0];
16294 vinfos[5].indices[1] = _ij5[1];
16295 vinfos[5].maxsolutions = _nj5;
16296 std::vector<int> vfree(0);
16297 solutions.AddSolution(vinfos,vfree);
16298 }
16299 }
16300 }
16301 
16302 }
16303 
16304 }
16305 
16306 } else
16307 {
16308 {
16309 IkReal j3array[1], cj3array[1], sj3array[1];
16310 bool j3valid[1]={false};
16311 _nj3 = 1;
16312 CheckValue<IkReal> x1292=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst22*gconst22)))+(((-1.0)*(gconst23*gconst23))))),-1);
16313 if(!x1292.valid){
16314 continue;
16315 }
16316 CheckValue<IkReal> x1293 = IKatan2WithCheck(IkReal((gconst22*new_r00)),IkReal((gconst23*new_r00)),IKFAST_ATAN2_MAGTHRESH);
16317 if(!x1293.valid){
16318 continue;
16319 }
16320 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1292.value)))+(x1293.value));
16321 sj3array[0]=IKsin(j3array[0]);
16322 cj3array[0]=IKcos(j3array[0]);
16323 if( j3array[0] > IKPI )
16324 {
16325  j3array[0]-=IK2PI;
16326 }
16327 else if( j3array[0] < -IKPI )
16328 { j3array[0]+=IK2PI;
16329 }
16330 j3valid[0] = true;
16331 for(int ij3 = 0; ij3 < 1; ++ij3)
16332 {
16333 if( !j3valid[ij3] )
16334 {
16335  continue;
16336 }
16337 _ij3[0] = ij3; _ij3[1] = -1;
16338 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16339 {
16340 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16341 {
16342  j3valid[iij3]=false; _ij3[1] = iij3; break;
16343 }
16344 }
16345 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16346 {
16347 IkReal evalcond[6];
16348 IkReal x1294=IKsin(j3);
16349 IkReal x1295=IKcos(j3);
16350 IkReal x1296=(gconst23*x1294);
16351 IkReal x1297=(gconst22*x1294);
16352 IkReal x1298=(gconst23*x1295);
16353 IkReal x1299=((1.0)*x1295);
16354 IkReal x1300=(gconst22*x1299);
16355 evalcond[0]=(x1296+(((-1.0)*x1300)));
16356 evalcond[1]=(gconst23+((new_r10*x1294))+((new_r00*x1295)));
16357 evalcond[2]=(x1298+x1297+new_r00);
16358 evalcond[3]=((((-1.0)*new_r10*x1299))+gconst22+((new_r00*x1294)));
16359 evalcond[4]=((((-1.0)*x1298))+(((-1.0)*x1297)));
16360 evalcond[5]=(x1296+(((-1.0)*x1300))+new_r10);
16361 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16362 {
16363 continue;
16364 }
16365 }
16366 
16367 {
16368 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16369 vinfos[0].jointtype = 1;
16370 vinfos[0].foffset = j0;
16371 vinfos[0].indices[0] = _ij0[0];
16372 vinfos[0].indices[1] = _ij0[1];
16373 vinfos[0].maxsolutions = _nj0;
16374 vinfos[1].jointtype = 1;
16375 vinfos[1].foffset = j1;
16376 vinfos[1].indices[0] = _ij1[0];
16377 vinfos[1].indices[1] = _ij1[1];
16378 vinfos[1].maxsolutions = _nj1;
16379 vinfos[2].jointtype = 1;
16380 vinfos[2].foffset = j2;
16381 vinfos[2].indices[0] = _ij2[0];
16382 vinfos[2].indices[1] = _ij2[1];
16383 vinfos[2].maxsolutions = _nj2;
16384 vinfos[3].jointtype = 1;
16385 vinfos[3].foffset = j3;
16386 vinfos[3].indices[0] = _ij3[0];
16387 vinfos[3].indices[1] = _ij3[1];
16388 vinfos[3].maxsolutions = _nj3;
16389 vinfos[4].jointtype = 1;
16390 vinfos[4].foffset = j4;
16391 vinfos[4].indices[0] = _ij4[0];
16392 vinfos[4].indices[1] = _ij4[1];
16393 vinfos[4].maxsolutions = _nj4;
16394 vinfos[5].jointtype = 1;
16395 vinfos[5].foffset = j5;
16396 vinfos[5].indices[0] = _ij5[0];
16397 vinfos[5].indices[1] = _ij5[1];
16398 vinfos[5].maxsolutions = _nj5;
16399 std::vector<int> vfree(0);
16400 solutions.AddSolution(vinfos,vfree);
16401 }
16402 }
16403 }
16404 
16405 }
16406 
16407 }
16408 
16409 } else
16410 {
16411 {
16412 IkReal j3array[1], cj3array[1], sj3array[1];
16413 bool j3valid[1]={false};
16414 _nj3 = 1;
16415 CheckValue<IkReal> x1301=IKPowWithIntegerCheck(IKsign((((gconst23*new_r10))+(((-1.0)*gconst22*new_r00)))),-1);
16416 if(!x1301.valid){
16417 continue;
16418 }
16419 CheckValue<IkReal> x1302 = IKatan2WithCheck(IkReal(gconst22*gconst22),IkReal((gconst22*gconst23)),IKFAST_ATAN2_MAGTHRESH);
16420 if(!x1302.valid){
16421 continue;
16422 }
16423 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1301.value)))+(x1302.value));
16424 sj3array[0]=IKsin(j3array[0]);
16425 cj3array[0]=IKcos(j3array[0]);
16426 if( j3array[0] > IKPI )
16427 {
16428  j3array[0]-=IK2PI;
16429 }
16430 else if( j3array[0] < -IKPI )
16431 { j3array[0]+=IK2PI;
16432 }
16433 j3valid[0] = true;
16434 for(int ij3 = 0; ij3 < 1; ++ij3)
16435 {
16436 if( !j3valid[ij3] )
16437 {
16438  continue;
16439 }
16440 _ij3[0] = ij3; _ij3[1] = -1;
16441 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16442 {
16443 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16444 {
16445  j3valid[iij3]=false; _ij3[1] = iij3; break;
16446 }
16447 }
16448 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16449 {
16450 IkReal evalcond[6];
16451 IkReal x1303=IKsin(j3);
16452 IkReal x1304=IKcos(j3);
16453 IkReal x1305=(gconst23*x1303);
16454 IkReal x1306=(gconst22*x1303);
16455 IkReal x1307=(gconst23*x1304);
16456 IkReal x1308=((1.0)*x1304);
16457 IkReal x1309=(gconst22*x1308);
16458 evalcond[0]=(x1305+(((-1.0)*x1309)));
16459 evalcond[1]=(gconst23+((new_r10*x1303))+((new_r00*x1304)));
16460 evalcond[2]=(x1306+x1307+new_r00);
16461 evalcond[3]=((((-1.0)*new_r10*x1308))+gconst22+((new_r00*x1303)));
16462 evalcond[4]=((((-1.0)*x1306))+(((-1.0)*x1307)));
16463 evalcond[5]=(x1305+(((-1.0)*x1309))+new_r10);
16464 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16465 {
16466 continue;
16467 }
16468 }
16469 
16470 {
16471 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16472 vinfos[0].jointtype = 1;
16473 vinfos[0].foffset = j0;
16474 vinfos[0].indices[0] = _ij0[0];
16475 vinfos[0].indices[1] = _ij0[1];
16476 vinfos[0].maxsolutions = _nj0;
16477 vinfos[1].jointtype = 1;
16478 vinfos[1].foffset = j1;
16479 vinfos[1].indices[0] = _ij1[0];
16480 vinfos[1].indices[1] = _ij1[1];
16481 vinfos[1].maxsolutions = _nj1;
16482 vinfos[2].jointtype = 1;
16483 vinfos[2].foffset = j2;
16484 vinfos[2].indices[0] = _ij2[0];
16485 vinfos[2].indices[1] = _ij2[1];
16486 vinfos[2].maxsolutions = _nj2;
16487 vinfos[3].jointtype = 1;
16488 vinfos[3].foffset = j3;
16489 vinfos[3].indices[0] = _ij3[0];
16490 vinfos[3].indices[1] = _ij3[1];
16491 vinfos[3].maxsolutions = _nj3;
16492 vinfos[4].jointtype = 1;
16493 vinfos[4].foffset = j4;
16494 vinfos[4].indices[0] = _ij4[0];
16495 vinfos[4].indices[1] = _ij4[1];
16496 vinfos[4].maxsolutions = _nj4;
16497 vinfos[5].jointtype = 1;
16498 vinfos[5].foffset = j5;
16499 vinfos[5].indices[0] = _ij5[0];
16500 vinfos[5].indices[1] = _ij5[1];
16501 vinfos[5].maxsolutions = _nj5;
16502 std::vector<int> vfree(0);
16503 solutions.AddSolution(vinfos,vfree);
16504 }
16505 }
16506 }
16507 
16508 }
16509 
16510 }
16511 
16512 }
16513 } while(0);
16514 if( bgotonextstatement )
16515 {
16516 bool bgotonextstatement = true;
16517 do
16518 {
16519 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
16520 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16521 {
16522 bgotonextstatement=false;
16523 {
16524 IkReal j3array[2], cj3array[2], sj3array[2];
16525 bool j3valid[2]={false};
16526 _nj3 = 2;
16527 CheckValue<IkReal> x1310=IKPowWithIntegerCheck(gconst22,-1);
16528 if(!x1310.valid){
16529 continue;
16530 }
16531 sj3array[0]=(new_r11*(x1310.value));
16532 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16533 {
16534  j3valid[0] = j3valid[1] = true;
16535  j3array[0] = IKasin(sj3array[0]);
16536  cj3array[0] = IKcos(j3array[0]);
16537  sj3array[1] = sj3array[0];
16538  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16539  cj3array[1] = -cj3array[0];
16540 }
16541 else if( isnan(sj3array[0]) )
16542 {
16543  // probably any value will work
16544  j3valid[0] = true;
16545  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16546 }
16547 for(int ij3 = 0; ij3 < 2; ++ij3)
16548 {
16549 if( !j3valid[ij3] )
16550 {
16551  continue;
16552 }
16553 _ij3[0] = ij3; _ij3[1] = -1;
16554 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16555 {
16556 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16557 {
16558  j3valid[iij3]=false; _ij3[1] = iij3; break;
16559 }
16560 }
16561 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16562 {
16563 IkReal evalcond[6];
16564 IkReal x1311=IKcos(j3);
16565 IkReal x1312=IKsin(j3);
16566 IkReal x1313=((-1.0)*x1311);
16567 evalcond[0]=(new_r00*x1311);
16568 evalcond[1]=(new_r11*x1313);
16569 evalcond[2]=(gconst22*x1313);
16570 evalcond[3]=(((new_r00*x1312))+gconst22);
16571 evalcond[4]=(((gconst22*x1312))+new_r00);
16572 evalcond[5]=(((new_r11*x1312))+(((-1.0)*gconst22)));
16573 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16574 {
16575 continue;
16576 }
16577 }
16578 
16579 {
16580 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16581 vinfos[0].jointtype = 1;
16582 vinfos[0].foffset = j0;
16583 vinfos[0].indices[0] = _ij0[0];
16584 vinfos[0].indices[1] = _ij0[1];
16585 vinfos[0].maxsolutions = _nj0;
16586 vinfos[1].jointtype = 1;
16587 vinfos[1].foffset = j1;
16588 vinfos[1].indices[0] = _ij1[0];
16589 vinfos[1].indices[1] = _ij1[1];
16590 vinfos[1].maxsolutions = _nj1;
16591 vinfos[2].jointtype = 1;
16592 vinfos[2].foffset = j2;
16593 vinfos[2].indices[0] = _ij2[0];
16594 vinfos[2].indices[1] = _ij2[1];
16595 vinfos[2].maxsolutions = _nj2;
16596 vinfos[3].jointtype = 1;
16597 vinfos[3].foffset = j3;
16598 vinfos[3].indices[0] = _ij3[0];
16599 vinfos[3].indices[1] = _ij3[1];
16600 vinfos[3].maxsolutions = _nj3;
16601 vinfos[4].jointtype = 1;
16602 vinfos[4].foffset = j4;
16603 vinfos[4].indices[0] = _ij4[0];
16604 vinfos[4].indices[1] = _ij4[1];
16605 vinfos[4].maxsolutions = _nj4;
16606 vinfos[5].jointtype = 1;
16607 vinfos[5].foffset = j5;
16608 vinfos[5].indices[0] = _ij5[0];
16609 vinfos[5].indices[1] = _ij5[1];
16610 vinfos[5].maxsolutions = _nj5;
16611 std::vector<int> vfree(0);
16612 solutions.AddSolution(vinfos,vfree);
16613 }
16614 }
16615 }
16616 
16617 }
16618 } while(0);
16619 if( bgotonextstatement )
16620 {
16621 bool bgotonextstatement = true;
16622 do
16623 {
16624 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
16625 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16626 {
16627 bgotonextstatement=false;
16628 {
16629 IkReal j3eval[1];
16630 IkReal x1314=((-1.0)*new_r00);
16631 CheckValue<IkReal> x1316 = IKatan2WithCheck(IkReal(x1314),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16632 if(!x1316.valid){
16633 continue;
16634 }
16635 IkReal x1315=((1.0)*(x1316.value));
16636 sj4=0;
16637 cj4=-1.0;
16638 j4=3.14159265358979;
16639 sj5=gconst22;
16640 cj5=gconst23;
16641 j5=((3.14159265)+(((-1.0)*x1315)));
16642 new_r11=0;
16643 new_r10=0;
16644 new_r22=0;
16645 new_r02=0;
16646 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1315)));
16647 IkReal x1317 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16648 if(IKabs(x1317)==0){
16649 continue;
16650 }
16651 IkReal gconst22=(x1314*(pow(x1317,-0.5)));
16652 IkReal gconst23=0;
16653 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
16654 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16655 {
16656 {
16657 IkReal j3eval[1];
16658 IkReal x1318=((-1.0)*new_r00);
16659 CheckValue<IkReal> x1320 = IKatan2WithCheck(IkReal(x1318),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16660 if(!x1320.valid){
16661 continue;
16662 }
16663 IkReal x1319=((1.0)*(x1320.value));
16664 sj4=0;
16665 cj4=-1.0;
16666 j4=3.14159265358979;
16667 sj5=gconst22;
16668 cj5=gconst23;
16669 j5=((3.14159265)+(((-1.0)*x1319)));
16670 new_r11=0;
16671 new_r10=0;
16672 new_r22=0;
16673 new_r02=0;
16674 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1319)));
16675 IkReal x1321 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16676 if(IKabs(x1321)==0){
16677 continue;
16678 }
16679 IkReal gconst22=(x1318*(pow(x1321,-0.5)));
16680 IkReal gconst23=0;
16681 j3eval[0]=new_r00;
16682 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16683 {
16684 {
16685 IkReal j3eval[2];
16686 IkReal x1322=((-1.0)*new_r00);
16687 CheckValue<IkReal> x1324 = IKatan2WithCheck(IkReal(x1322),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16688 if(!x1324.valid){
16689 continue;
16690 }
16691 IkReal x1323=((1.0)*(x1324.value));
16692 sj4=0;
16693 cj4=-1.0;
16694 j4=3.14159265358979;
16695 sj5=gconst22;
16696 cj5=gconst23;
16697 j5=((3.14159265)+(((-1.0)*x1323)));
16698 new_r11=0;
16699 new_r10=0;
16700 new_r22=0;
16701 new_r02=0;
16702 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1323)));
16703 IkReal x1325 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16704 if(IKabs(x1325)==0){
16705 continue;
16706 }
16707 IkReal gconst22=(x1322*(pow(x1325,-0.5)));
16708 IkReal gconst23=0;
16709 j3eval[0]=new_r00;
16710 j3eval[1]=new_r01;
16711 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
16712 {
16713 continue; // 3 cases reached
16714 
16715 } else
16716 {
16717 {
16718 IkReal j3array[1], cj3array[1], sj3array[1];
16719 bool j3valid[1]={false};
16720 _nj3 = 1;
16721 CheckValue<IkReal> x1326=IKPowWithIntegerCheck(new_r00,-1);
16722 if(!x1326.valid){
16723 continue;
16724 }
16725 CheckValue<IkReal> x1327=IKPowWithIntegerCheck(new_r01,-1);
16726 if(!x1327.valid){
16727 continue;
16728 }
16729 if( IKabs(((-1.0)*gconst22*(x1326.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst22*(x1327.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1326.value)))+IKsqr((gconst22*(x1327.value)))-1) <= IKFAST_SINCOS_THRESH )
16730  continue;
16731 j3array[0]=IKatan2(((-1.0)*gconst22*(x1326.value)), (gconst22*(x1327.value)));
16732 sj3array[0]=IKsin(j3array[0]);
16733 cj3array[0]=IKcos(j3array[0]);
16734 if( j3array[0] > IKPI )
16735 {
16736  j3array[0]-=IK2PI;
16737 }
16738 else if( j3array[0] < -IKPI )
16739 { j3array[0]+=IK2PI;
16740 }
16741 j3valid[0] = true;
16742 for(int ij3 = 0; ij3 < 1; ++ij3)
16743 {
16744 if( !j3valid[ij3] )
16745 {
16746  continue;
16747 }
16748 _ij3[0] = ij3; _ij3[1] = -1;
16749 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16750 {
16751 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16752 {
16753  j3valid[iij3]=false; _ij3[1] = iij3; break;
16754 }
16755 }
16756 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16757 {
16758 IkReal evalcond[8];
16759 IkReal x1328=IKsin(j3);
16760 IkReal x1329=IKcos(j3);
16761 IkReal x1330=(gconst22*x1329);
16762 IkReal x1331=(gconst22*x1328);
16763 evalcond[0]=(new_r01*x1328);
16764 evalcond[1]=(new_r00*x1329);
16765 evalcond[2]=((-1.0)*x1331);
16766 evalcond[3]=((-1.0)*x1330);
16767 evalcond[4]=(((new_r00*x1328))+gconst22);
16768 evalcond[5]=(x1331+new_r00);
16769 evalcond[6]=((((-1.0)*x1330))+new_r01);
16770 evalcond[7]=(((new_r01*x1329))+(((-1.0)*gconst22)));
16771 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16772 {
16773 continue;
16774 }
16775 }
16776 
16777 {
16778 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16779 vinfos[0].jointtype = 1;
16780 vinfos[0].foffset = j0;
16781 vinfos[0].indices[0] = _ij0[0];
16782 vinfos[0].indices[1] = _ij0[1];
16783 vinfos[0].maxsolutions = _nj0;
16784 vinfos[1].jointtype = 1;
16785 vinfos[1].foffset = j1;
16786 vinfos[1].indices[0] = _ij1[0];
16787 vinfos[1].indices[1] = _ij1[1];
16788 vinfos[1].maxsolutions = _nj1;
16789 vinfos[2].jointtype = 1;
16790 vinfos[2].foffset = j2;
16791 vinfos[2].indices[0] = _ij2[0];
16792 vinfos[2].indices[1] = _ij2[1];
16793 vinfos[2].maxsolutions = _nj2;
16794 vinfos[3].jointtype = 1;
16795 vinfos[3].foffset = j3;
16796 vinfos[3].indices[0] = _ij3[0];
16797 vinfos[3].indices[1] = _ij3[1];
16798 vinfos[3].maxsolutions = _nj3;
16799 vinfos[4].jointtype = 1;
16800 vinfos[4].foffset = j4;
16801 vinfos[4].indices[0] = _ij4[0];
16802 vinfos[4].indices[1] = _ij4[1];
16803 vinfos[4].maxsolutions = _nj4;
16804 vinfos[5].jointtype = 1;
16805 vinfos[5].foffset = j5;
16806 vinfos[5].indices[0] = _ij5[0];
16807 vinfos[5].indices[1] = _ij5[1];
16808 vinfos[5].maxsolutions = _nj5;
16809 std::vector<int> vfree(0);
16810 solutions.AddSolution(vinfos,vfree);
16811 }
16812 }
16813 }
16814 
16815 }
16816 
16817 }
16818 
16819 } else
16820 {
16821 {
16822 IkReal j3array[1], cj3array[1], sj3array[1];
16823 bool j3valid[1]={false};
16824 _nj3 = 1;
16825 CheckValue<IkReal> x1332=IKPowWithIntegerCheck(new_r00,-1);
16826 if(!x1332.valid){
16827 continue;
16828 }
16829 CheckValue<IkReal> x1333=IKPowWithIntegerCheck(gconst22,-1);
16830 if(!x1333.valid){
16831 continue;
16832 }
16833 if( IKabs(((-1.0)*gconst22*(x1332.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1333.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1332.value)))+IKsqr((new_r01*(x1333.value)))-1) <= IKFAST_SINCOS_THRESH )
16834  continue;
16835 j3array[0]=IKatan2(((-1.0)*gconst22*(x1332.value)), (new_r01*(x1333.value)));
16836 sj3array[0]=IKsin(j3array[0]);
16837 cj3array[0]=IKcos(j3array[0]);
16838 if( j3array[0] > IKPI )
16839 {
16840  j3array[0]-=IK2PI;
16841 }
16842 else if( j3array[0] < -IKPI )
16843 { j3array[0]+=IK2PI;
16844 }
16845 j3valid[0] = true;
16846 for(int ij3 = 0; ij3 < 1; ++ij3)
16847 {
16848 if( !j3valid[ij3] )
16849 {
16850  continue;
16851 }
16852 _ij3[0] = ij3; _ij3[1] = -1;
16853 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16854 {
16855 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16856 {
16857  j3valid[iij3]=false; _ij3[1] = iij3; break;
16858 }
16859 }
16860 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16861 {
16862 IkReal evalcond[8];
16863 IkReal x1334=IKsin(j3);
16864 IkReal x1335=IKcos(j3);
16865 IkReal x1336=(gconst22*x1335);
16866 IkReal x1337=(gconst22*x1334);
16867 evalcond[0]=(new_r01*x1334);
16868 evalcond[1]=(new_r00*x1335);
16869 evalcond[2]=((-1.0)*x1337);
16870 evalcond[3]=((-1.0)*x1336);
16871 evalcond[4]=(gconst22+((new_r00*x1334)));
16872 evalcond[5]=(x1337+new_r00);
16873 evalcond[6]=((((-1.0)*x1336))+new_r01);
16874 evalcond[7]=(((new_r01*x1335))+(((-1.0)*gconst22)));
16875 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16876 {
16877 continue;
16878 }
16879 }
16880 
16881 {
16882 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16883 vinfos[0].jointtype = 1;
16884 vinfos[0].foffset = j0;
16885 vinfos[0].indices[0] = _ij0[0];
16886 vinfos[0].indices[1] = _ij0[1];
16887 vinfos[0].maxsolutions = _nj0;
16888 vinfos[1].jointtype = 1;
16889 vinfos[1].foffset = j1;
16890 vinfos[1].indices[0] = _ij1[0];
16891 vinfos[1].indices[1] = _ij1[1];
16892 vinfos[1].maxsolutions = _nj1;
16893 vinfos[2].jointtype = 1;
16894 vinfos[2].foffset = j2;
16895 vinfos[2].indices[0] = _ij2[0];
16896 vinfos[2].indices[1] = _ij2[1];
16897 vinfos[2].maxsolutions = _nj2;
16898 vinfos[3].jointtype = 1;
16899 vinfos[3].foffset = j3;
16900 vinfos[3].indices[0] = _ij3[0];
16901 vinfos[3].indices[1] = _ij3[1];
16902 vinfos[3].maxsolutions = _nj3;
16903 vinfos[4].jointtype = 1;
16904 vinfos[4].foffset = j4;
16905 vinfos[4].indices[0] = _ij4[0];
16906 vinfos[4].indices[1] = _ij4[1];
16907 vinfos[4].maxsolutions = _nj4;
16908 vinfos[5].jointtype = 1;
16909 vinfos[5].foffset = j5;
16910 vinfos[5].indices[0] = _ij5[0];
16911 vinfos[5].indices[1] = _ij5[1];
16912 vinfos[5].maxsolutions = _nj5;
16913 std::vector<int> vfree(0);
16914 solutions.AddSolution(vinfos,vfree);
16915 }
16916 }
16917 }
16918 
16919 }
16920 
16921 }
16922 
16923 } else
16924 {
16925 {
16926 IkReal j3array[1], cj3array[1], sj3array[1];
16927 bool j3valid[1]={false};
16928 _nj3 = 1;
16929 CheckValue<IkReal> x1338=IKPowWithIntegerCheck(IKsign(gconst22),-1);
16930 if(!x1338.valid){
16931 continue;
16932 }
16933 CheckValue<IkReal> x1339 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
16934 if(!x1339.valid){
16935 continue;
16936 }
16937 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1338.value)))+(x1339.value));
16938 sj3array[0]=IKsin(j3array[0]);
16939 cj3array[0]=IKcos(j3array[0]);
16940 if( j3array[0] > IKPI )
16941 {
16942  j3array[0]-=IK2PI;
16943 }
16944 else if( j3array[0] < -IKPI )
16945 { j3array[0]+=IK2PI;
16946 }
16947 j3valid[0] = true;
16948 for(int ij3 = 0; ij3 < 1; ++ij3)
16949 {
16950 if( !j3valid[ij3] )
16951 {
16952  continue;
16953 }
16954 _ij3[0] = ij3; _ij3[1] = -1;
16955 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16956 {
16957 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16958 {
16959  j3valid[iij3]=false; _ij3[1] = iij3; break;
16960 }
16961 }
16962 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16963 {
16964 IkReal evalcond[8];
16965 IkReal x1340=IKsin(j3);
16966 IkReal x1341=IKcos(j3);
16967 IkReal x1342=(gconst22*x1341);
16968 IkReal x1343=(gconst22*x1340);
16969 evalcond[0]=(new_r01*x1340);
16970 evalcond[1]=(new_r00*x1341);
16971 evalcond[2]=((-1.0)*x1343);
16972 evalcond[3]=((-1.0)*x1342);
16973 evalcond[4]=(gconst22+((new_r00*x1340)));
16974 evalcond[5]=(x1343+new_r00);
16975 evalcond[6]=(new_r01+(((-1.0)*x1342)));
16976 evalcond[7]=(((new_r01*x1341))+(((-1.0)*gconst22)));
16977 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16978 {
16979 continue;
16980 }
16981 }
16982 
16983 {
16984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16985 vinfos[0].jointtype = 1;
16986 vinfos[0].foffset = j0;
16987 vinfos[0].indices[0] = _ij0[0];
16988 vinfos[0].indices[1] = _ij0[1];
16989 vinfos[0].maxsolutions = _nj0;
16990 vinfos[1].jointtype = 1;
16991 vinfos[1].foffset = j1;
16992 vinfos[1].indices[0] = _ij1[0];
16993 vinfos[1].indices[1] = _ij1[1];
16994 vinfos[1].maxsolutions = _nj1;
16995 vinfos[2].jointtype = 1;
16996 vinfos[2].foffset = j2;
16997 vinfos[2].indices[0] = _ij2[0];
16998 vinfos[2].indices[1] = _ij2[1];
16999 vinfos[2].maxsolutions = _nj2;
17000 vinfos[3].jointtype = 1;
17001 vinfos[3].foffset = j3;
17002 vinfos[3].indices[0] = _ij3[0];
17003 vinfos[3].indices[1] = _ij3[1];
17004 vinfos[3].maxsolutions = _nj3;
17005 vinfos[4].jointtype = 1;
17006 vinfos[4].foffset = j4;
17007 vinfos[4].indices[0] = _ij4[0];
17008 vinfos[4].indices[1] = _ij4[1];
17009 vinfos[4].maxsolutions = _nj4;
17010 vinfos[5].jointtype = 1;
17011 vinfos[5].foffset = j5;
17012 vinfos[5].indices[0] = _ij5[0];
17013 vinfos[5].indices[1] = _ij5[1];
17014 vinfos[5].maxsolutions = _nj5;
17015 std::vector<int> vfree(0);
17016 solutions.AddSolution(vinfos,vfree);
17017 }
17018 }
17019 }
17020 
17021 }
17022 
17023 }
17024 
17025 }
17026 } while(0);
17027 if( bgotonextstatement )
17028 {
17029 bool bgotonextstatement = true;
17030 do
17031 {
17032 evalcond[0]=IKabs(new_r10);
17033 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17034 {
17035 bgotonextstatement=false;
17036 {
17037 IkReal j3eval[1];
17038 IkReal x1344=((-1.0)*new_r00);
17039 CheckValue<IkReal> x1346 = IKatan2WithCheck(IkReal(x1344),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17040 if(!x1346.valid){
17041 continue;
17042 }
17043 IkReal x1345=((1.0)*(x1346.value));
17044 sj4=0;
17045 cj4=-1.0;
17046 j4=3.14159265358979;
17047 sj5=gconst22;
17048 cj5=gconst23;
17049 j5=((3.14159265)+(((-1.0)*x1345)));
17050 new_r10=0;
17051 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1345)));
17052 IkReal x1347 = new_r00*new_r00;
17053 if(IKabs(x1347)==0){
17054 continue;
17055 }
17056 IkReal gconst22=(x1344*(pow(x1347,-0.5)));
17057 IkReal gconst23=0;
17058 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
17059 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17060 {
17061 {
17062 IkReal j3eval[1];
17063 IkReal x1348=((-1.0)*new_r00);
17064 CheckValue<IkReal> x1350 = IKatan2WithCheck(IkReal(x1348),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17065 if(!x1350.valid){
17066 continue;
17067 }
17068 IkReal x1349=((1.0)*(x1350.value));
17069 sj4=0;
17070 cj4=-1.0;
17071 j4=3.14159265358979;
17072 sj5=gconst22;
17073 cj5=gconst23;
17074 j5=((3.14159265)+(((-1.0)*x1349)));
17075 new_r10=0;
17076 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1349)));
17077 IkReal x1351 = new_r00*new_r00;
17078 if(IKabs(x1351)==0){
17079 continue;
17080 }
17081 IkReal gconst22=(x1348*(pow(x1351,-0.5)));
17082 IkReal gconst23=0;
17083 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
17084 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17085 {
17086 {
17087 IkReal j3eval[1];
17088 IkReal x1352=((-1.0)*new_r00);
17089 CheckValue<IkReal> x1354 = IKatan2WithCheck(IkReal(x1352),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17090 if(!x1354.valid){
17091 continue;
17092 }
17093 IkReal x1353=((1.0)*(x1354.value));
17094 sj4=0;
17095 cj4=-1.0;
17096 j4=3.14159265358979;
17097 sj5=gconst22;
17098 cj5=gconst23;
17099 j5=((3.14159265)+(((-1.0)*x1353)));
17100 new_r10=0;
17101 IkReal gconst21=((3.14159265358979)+(((-1.0)*x1353)));
17102 IkReal x1355 = new_r00*new_r00;
17103 if(IKabs(x1355)==0){
17104 continue;
17105 }
17106 IkReal gconst22=(x1352*(pow(x1355,-0.5)));
17107 IkReal gconst23=0;
17108 j3eval[0]=new_r00;
17109 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17110 {
17111 continue; // 3 cases reached
17112 
17113 } else
17114 {
17115 {
17116 IkReal j3array[1], cj3array[1], sj3array[1];
17117 bool j3valid[1]={false};
17118 _nj3 = 1;
17119 CheckValue<IkReal> x1356=IKPowWithIntegerCheck(new_r00,-1);
17120 if(!x1356.valid){
17121 continue;
17122 }
17123 CheckValue<IkReal> x1357=IKPowWithIntegerCheck(gconst22,-1);
17124 if(!x1357.valid){
17125 continue;
17126 }
17127 if( IKabs(((-1.0)*gconst22*(x1356.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1357.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst22*(x1356.value)))+IKsqr((new_r01*(x1357.value)))-1) <= IKFAST_SINCOS_THRESH )
17128  continue;
17129 j3array[0]=IKatan2(((-1.0)*gconst22*(x1356.value)), (new_r01*(x1357.value)));
17130 sj3array[0]=IKsin(j3array[0]);
17131 cj3array[0]=IKcos(j3array[0]);
17132 if( j3array[0] > IKPI )
17133 {
17134  j3array[0]-=IK2PI;
17135 }
17136 else if( j3array[0] < -IKPI )
17137 { j3array[0]+=IK2PI;
17138 }
17139 j3valid[0] = true;
17140 for(int ij3 = 0; ij3 < 1; ++ij3)
17141 {
17142 if( !j3valid[ij3] )
17143 {
17144  continue;
17145 }
17146 _ij3[0] = ij3; _ij3[1] = -1;
17147 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17148 {
17149 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17150 {
17151  j3valid[iij3]=false; _ij3[1] = iij3; break;
17152 }
17153 }
17154 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17155 {
17156 IkReal evalcond[8];
17157 IkReal x1358=IKcos(j3);
17158 IkReal x1359=IKsin(j3);
17159 IkReal x1360=(gconst22*x1359);
17160 IkReal x1361=((1.0)*x1358);
17161 evalcond[0]=(new_r00*x1358);
17162 evalcond[1]=((-1.0)*gconst22*x1358);
17163 evalcond[2]=(gconst22+((new_r00*x1359)));
17164 evalcond[3]=(x1360+new_r00);
17165 evalcond[4]=((((-1.0)*gconst22*x1361))+new_r01);
17166 evalcond[5]=((((-1.0)*x1360))+new_r11);
17167 evalcond[6]=((((-1.0)*new_r11*x1361))+((new_r01*x1359)));
17168 evalcond[7]=(((new_r11*x1359))+((new_r01*x1358))+(((-1.0)*gconst22)));
17169 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17170 {
17171 continue;
17172 }
17173 }
17174 
17175 {
17176 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17177 vinfos[0].jointtype = 1;
17178 vinfos[0].foffset = j0;
17179 vinfos[0].indices[0] = _ij0[0];
17180 vinfos[0].indices[1] = _ij0[1];
17181 vinfos[0].maxsolutions = _nj0;
17182 vinfos[1].jointtype = 1;
17183 vinfos[1].foffset = j1;
17184 vinfos[1].indices[0] = _ij1[0];
17185 vinfos[1].indices[1] = _ij1[1];
17186 vinfos[1].maxsolutions = _nj1;
17187 vinfos[2].jointtype = 1;
17188 vinfos[2].foffset = j2;
17189 vinfos[2].indices[0] = _ij2[0];
17190 vinfos[2].indices[1] = _ij2[1];
17191 vinfos[2].maxsolutions = _nj2;
17192 vinfos[3].jointtype = 1;
17193 vinfos[3].foffset = j3;
17194 vinfos[3].indices[0] = _ij3[0];
17195 vinfos[3].indices[1] = _ij3[1];
17196 vinfos[3].maxsolutions = _nj3;
17197 vinfos[4].jointtype = 1;
17198 vinfos[4].foffset = j4;
17199 vinfos[4].indices[0] = _ij4[0];
17200 vinfos[4].indices[1] = _ij4[1];
17201 vinfos[4].maxsolutions = _nj4;
17202 vinfos[5].jointtype = 1;
17203 vinfos[5].foffset = j5;
17204 vinfos[5].indices[0] = _ij5[0];
17205 vinfos[5].indices[1] = _ij5[1];
17206 vinfos[5].maxsolutions = _nj5;
17207 std::vector<int> vfree(0);
17208 solutions.AddSolution(vinfos,vfree);
17209 }
17210 }
17211 }
17212 
17213 }
17214 
17215 }
17216 
17217 } else
17218 {
17219 {
17220 IkReal j3array[1], cj3array[1], sj3array[1];
17221 bool j3valid[1]={false};
17222 _nj3 = 1;
17223 CheckValue<IkReal> x1362=IKPowWithIntegerCheck(IKsign(gconst22),-1);
17224 if(!x1362.valid){
17225 continue;
17226 }
17227 CheckValue<IkReal> x1363 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17228 if(!x1363.valid){
17229 continue;
17230 }
17231 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1362.value)))+(x1363.value));
17232 sj3array[0]=IKsin(j3array[0]);
17233 cj3array[0]=IKcos(j3array[0]);
17234 if( j3array[0] > IKPI )
17235 {
17236  j3array[0]-=IK2PI;
17237 }
17238 else if( j3array[0] < -IKPI )
17239 { j3array[0]+=IK2PI;
17240 }
17241 j3valid[0] = true;
17242 for(int ij3 = 0; ij3 < 1; ++ij3)
17243 {
17244 if( !j3valid[ij3] )
17245 {
17246  continue;
17247 }
17248 _ij3[0] = ij3; _ij3[1] = -1;
17249 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17250 {
17251 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17252 {
17253  j3valid[iij3]=false; _ij3[1] = iij3; break;
17254 }
17255 }
17256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17257 {
17258 IkReal evalcond[8];
17259 IkReal x1364=IKcos(j3);
17260 IkReal x1365=IKsin(j3);
17261 IkReal x1366=(gconst22*x1365);
17262 IkReal x1367=((1.0)*x1364);
17263 evalcond[0]=(new_r00*x1364);
17264 evalcond[1]=((-1.0)*gconst22*x1364);
17265 evalcond[2]=(gconst22+((new_r00*x1365)));
17266 evalcond[3]=(x1366+new_r00);
17267 evalcond[4]=((((-1.0)*gconst22*x1367))+new_r01);
17268 evalcond[5]=((((-1.0)*x1366))+new_r11);
17269 evalcond[6]=((((-1.0)*new_r11*x1367))+((new_r01*x1365)));
17270 evalcond[7]=((((-1.0)*gconst22))+((new_r01*x1364))+((new_r11*x1365)));
17271 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17272 {
17273 continue;
17274 }
17275 }
17276 
17277 {
17278 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17279 vinfos[0].jointtype = 1;
17280 vinfos[0].foffset = j0;
17281 vinfos[0].indices[0] = _ij0[0];
17282 vinfos[0].indices[1] = _ij0[1];
17283 vinfos[0].maxsolutions = _nj0;
17284 vinfos[1].jointtype = 1;
17285 vinfos[1].foffset = j1;
17286 vinfos[1].indices[0] = _ij1[0];
17287 vinfos[1].indices[1] = _ij1[1];
17288 vinfos[1].maxsolutions = _nj1;
17289 vinfos[2].jointtype = 1;
17290 vinfos[2].foffset = j2;
17291 vinfos[2].indices[0] = _ij2[0];
17292 vinfos[2].indices[1] = _ij2[1];
17293 vinfos[2].maxsolutions = _nj2;
17294 vinfos[3].jointtype = 1;
17295 vinfos[3].foffset = j3;
17296 vinfos[3].indices[0] = _ij3[0];
17297 vinfos[3].indices[1] = _ij3[1];
17298 vinfos[3].maxsolutions = _nj3;
17299 vinfos[4].jointtype = 1;
17300 vinfos[4].foffset = j4;
17301 vinfos[4].indices[0] = _ij4[0];
17302 vinfos[4].indices[1] = _ij4[1];
17303 vinfos[4].maxsolutions = _nj4;
17304 vinfos[5].jointtype = 1;
17305 vinfos[5].foffset = j5;
17306 vinfos[5].indices[0] = _ij5[0];
17307 vinfos[5].indices[1] = _ij5[1];
17308 vinfos[5].maxsolutions = _nj5;
17309 std::vector<int> vfree(0);
17310 solutions.AddSolution(vinfos,vfree);
17311 }
17312 }
17313 }
17314 
17315 }
17316 
17317 }
17318 
17319 } else
17320 {
17321 {
17322 IkReal j3array[1], cj3array[1], sj3array[1];
17323 bool j3valid[1]={false};
17324 _nj3 = 1;
17325 CheckValue<IkReal> x1368=IKPowWithIntegerCheck(IKsign(gconst22),-1);
17326 if(!x1368.valid){
17327 continue;
17328 }
17329 CheckValue<IkReal> x1369 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17330 if(!x1369.valid){
17331 continue;
17332 }
17333 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1368.value)))+(x1369.value));
17334 sj3array[0]=IKsin(j3array[0]);
17335 cj3array[0]=IKcos(j3array[0]);
17336 if( j3array[0] > IKPI )
17337 {
17338  j3array[0]-=IK2PI;
17339 }
17340 else if( j3array[0] < -IKPI )
17341 { j3array[0]+=IK2PI;
17342 }
17343 j3valid[0] = true;
17344 for(int ij3 = 0; ij3 < 1; ++ij3)
17345 {
17346 if( !j3valid[ij3] )
17347 {
17348  continue;
17349 }
17350 _ij3[0] = ij3; _ij3[1] = -1;
17351 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17352 {
17353 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17354 {
17355  j3valid[iij3]=false; _ij3[1] = iij3; break;
17356 }
17357 }
17358 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17359 {
17360 IkReal evalcond[8];
17361 IkReal x1370=IKcos(j3);
17362 IkReal x1371=IKsin(j3);
17363 IkReal x1372=(gconst22*x1371);
17364 IkReal x1373=((1.0)*x1370);
17365 evalcond[0]=(new_r00*x1370);
17366 evalcond[1]=((-1.0)*gconst22*x1370);
17367 evalcond[2]=(gconst22+((new_r00*x1371)));
17368 evalcond[3]=(x1372+new_r00);
17369 evalcond[4]=(new_r01+(((-1.0)*gconst22*x1373)));
17370 evalcond[5]=((((-1.0)*x1372))+new_r11);
17371 evalcond[6]=((((-1.0)*new_r11*x1373))+((new_r01*x1371)));
17372 evalcond[7]=(((new_r11*x1371))+(((-1.0)*gconst22))+((new_r01*x1370)));
17373 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17374 {
17375 continue;
17376 }
17377 }
17378 
17379 {
17380 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17381 vinfos[0].jointtype = 1;
17382 vinfos[0].foffset = j0;
17383 vinfos[0].indices[0] = _ij0[0];
17384 vinfos[0].indices[1] = _ij0[1];
17385 vinfos[0].maxsolutions = _nj0;
17386 vinfos[1].jointtype = 1;
17387 vinfos[1].foffset = j1;
17388 vinfos[1].indices[0] = _ij1[0];
17389 vinfos[1].indices[1] = _ij1[1];
17390 vinfos[1].maxsolutions = _nj1;
17391 vinfos[2].jointtype = 1;
17392 vinfos[2].foffset = j2;
17393 vinfos[2].indices[0] = _ij2[0];
17394 vinfos[2].indices[1] = _ij2[1];
17395 vinfos[2].maxsolutions = _nj2;
17396 vinfos[3].jointtype = 1;
17397 vinfos[3].foffset = j3;
17398 vinfos[3].indices[0] = _ij3[0];
17399 vinfos[3].indices[1] = _ij3[1];
17400 vinfos[3].maxsolutions = _nj3;
17401 vinfos[4].jointtype = 1;
17402 vinfos[4].foffset = j4;
17403 vinfos[4].indices[0] = _ij4[0];
17404 vinfos[4].indices[1] = _ij4[1];
17405 vinfos[4].maxsolutions = _nj4;
17406 vinfos[5].jointtype = 1;
17407 vinfos[5].foffset = j5;
17408 vinfos[5].indices[0] = _ij5[0];
17409 vinfos[5].indices[1] = _ij5[1];
17410 vinfos[5].maxsolutions = _nj5;
17411 std::vector<int> vfree(0);
17412 solutions.AddSolution(vinfos,vfree);
17413 }
17414 }
17415 }
17416 
17417 }
17418 
17419 }
17420 
17421 }
17422 } while(0);
17423 if( bgotonextstatement )
17424 {
17425 bool bgotonextstatement = true;
17426 do
17427 {
17428 if( 1 )
17429 {
17430 bgotonextstatement=false;
17431 continue; // branch miss [j3]
17432 
17433 }
17434 } while(0);
17435 if( bgotonextstatement )
17436 {
17437 }
17438 }
17439 }
17440 }
17441 }
17442 }
17443 }
17444 
17445 } else
17446 {
17447 {
17448 IkReal j3array[1], cj3array[1], sj3array[1];
17449 bool j3valid[1]={false};
17450 _nj3 = 1;
17451 IkReal x1374=((1.0)*gconst23);
17452 CheckValue<IkReal> x1375 = IKatan2WithCheck(IkReal(((gconst23*gconst23)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst22*x1374)))),IKFAST_ATAN2_MAGTHRESH);
17453 if(!x1375.valid){
17454 continue;
17455 }
17456 CheckValue<IkReal> x1376=IKPowWithIntegerCheck(IKsign((((gconst22*new_r00))+(((-1.0)*new_r10*x1374)))),-1);
17457 if(!x1376.valid){
17458 continue;
17459 }
17460 j3array[0]=((-1.5707963267949)+(x1375.value)+(((1.5707963267949)*(x1376.value))));
17461 sj3array[0]=IKsin(j3array[0]);
17462 cj3array[0]=IKcos(j3array[0]);
17463 if( j3array[0] > IKPI )
17464 {
17465  j3array[0]-=IK2PI;
17466 }
17467 else if( j3array[0] < -IKPI )
17468 { j3array[0]+=IK2PI;
17469 }
17470 j3valid[0] = true;
17471 for(int ij3 = 0; ij3 < 1; ++ij3)
17472 {
17473 if( !j3valid[ij3] )
17474 {
17475  continue;
17476 }
17477 _ij3[0] = ij3; _ij3[1] = -1;
17478 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17479 {
17480 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17481 {
17482  j3valid[iij3]=false; _ij3[1] = iij3; break;
17483 }
17484 }
17485 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17486 {
17487 IkReal evalcond[8];
17488 IkReal x1377=IKsin(j3);
17489 IkReal x1378=IKcos(j3);
17490 IkReal x1379=((1.0)*gconst22);
17491 IkReal x1380=(gconst23*x1377);
17492 IkReal x1381=((1.0)*x1378);
17493 IkReal x1382=(x1378*x1379);
17494 evalcond[0]=(gconst23+((new_r10*x1377))+((new_r00*x1378)));
17495 evalcond[1]=(((gconst23*x1378))+((gconst22*x1377))+new_r00);
17496 evalcond[2]=(gconst22+(((-1.0)*new_r10*x1381))+((new_r00*x1377)));
17497 evalcond[3]=(gconst23+(((-1.0)*new_r11*x1381))+((new_r01*x1377)));
17498 evalcond[4]=((((-1.0)*x1382))+x1380+new_r01);
17499 evalcond[5]=((((-1.0)*x1382))+x1380+new_r10);
17500 evalcond[6]=((((-1.0)*x1379))+((new_r11*x1377))+((new_r01*x1378)));
17501 evalcond[7]=((((-1.0)*gconst23*x1381))+(((-1.0)*x1377*x1379))+new_r11);
17502 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17503 {
17504 continue;
17505 }
17506 }
17507 
17508 {
17509 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17510 vinfos[0].jointtype = 1;
17511 vinfos[0].foffset = j0;
17512 vinfos[0].indices[0] = _ij0[0];
17513 vinfos[0].indices[1] = _ij0[1];
17514 vinfos[0].maxsolutions = _nj0;
17515 vinfos[1].jointtype = 1;
17516 vinfos[1].foffset = j1;
17517 vinfos[1].indices[0] = _ij1[0];
17518 vinfos[1].indices[1] = _ij1[1];
17519 vinfos[1].maxsolutions = _nj1;
17520 vinfos[2].jointtype = 1;
17521 vinfos[2].foffset = j2;
17522 vinfos[2].indices[0] = _ij2[0];
17523 vinfos[2].indices[1] = _ij2[1];
17524 vinfos[2].maxsolutions = _nj2;
17525 vinfos[3].jointtype = 1;
17526 vinfos[3].foffset = j3;
17527 vinfos[3].indices[0] = _ij3[0];
17528 vinfos[3].indices[1] = _ij3[1];
17529 vinfos[3].maxsolutions = _nj3;
17530 vinfos[4].jointtype = 1;
17531 vinfos[4].foffset = j4;
17532 vinfos[4].indices[0] = _ij4[0];
17533 vinfos[4].indices[1] = _ij4[1];
17534 vinfos[4].maxsolutions = _nj4;
17535 vinfos[5].jointtype = 1;
17536 vinfos[5].foffset = j5;
17537 vinfos[5].indices[0] = _ij5[0];
17538 vinfos[5].indices[1] = _ij5[1];
17539 vinfos[5].maxsolutions = _nj5;
17540 std::vector<int> vfree(0);
17541 solutions.AddSolution(vinfos,vfree);
17542 }
17543 }
17544 }
17545 
17546 }
17547 
17548 }
17549 
17550 } else
17551 {
17552 {
17553 IkReal j3array[1], cj3array[1], sj3array[1];
17554 bool j3valid[1]={false};
17555 _nj3 = 1;
17556 IkReal x1383=((1.0)*gconst22);
17557 CheckValue<IkReal> x1384=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst23*new_r01))+(((-1.0)*new_r11*x1383)))),-1);
17558 if(!x1384.valid){
17559 continue;
17560 }
17561 CheckValue<IkReal> x1385 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst23*gconst23))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst23*x1383)))),IKFAST_ATAN2_MAGTHRESH);
17562 if(!x1385.valid){
17563 continue;
17564 }
17565 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1384.value)))+(x1385.value));
17566 sj3array[0]=IKsin(j3array[0]);
17567 cj3array[0]=IKcos(j3array[0]);
17568 if( j3array[0] > IKPI )
17569 {
17570  j3array[0]-=IK2PI;
17571 }
17572 else if( j3array[0] < -IKPI )
17573 { j3array[0]+=IK2PI;
17574 }
17575 j3valid[0] = true;
17576 for(int ij3 = 0; ij3 < 1; ++ij3)
17577 {
17578 if( !j3valid[ij3] )
17579 {
17580  continue;
17581 }
17582 _ij3[0] = ij3; _ij3[1] = -1;
17583 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17584 {
17585 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17586 {
17587  j3valid[iij3]=false; _ij3[1] = iij3; break;
17588 }
17589 }
17590 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17591 {
17592 IkReal evalcond[8];
17593 IkReal x1386=IKsin(j3);
17594 IkReal x1387=IKcos(j3);
17595 IkReal x1388=((1.0)*gconst22);
17596 IkReal x1389=(gconst23*x1386);
17597 IkReal x1390=((1.0)*x1387);
17598 IkReal x1391=(x1387*x1388);
17599 evalcond[0]=(((new_r10*x1386))+gconst23+((new_r00*x1387)));
17600 evalcond[1]=(((gconst22*x1386))+((gconst23*x1387))+new_r00);
17601 evalcond[2]=(gconst22+((new_r00*x1386))+(((-1.0)*new_r10*x1390)));
17602 evalcond[3]=(gconst23+((new_r01*x1386))+(((-1.0)*new_r11*x1390)));
17603 evalcond[4]=(x1389+new_r01+(((-1.0)*x1391)));
17604 evalcond[5]=(x1389+new_r10+(((-1.0)*x1391)));
17605 evalcond[6]=((((-1.0)*x1388))+((new_r11*x1386))+((new_r01*x1387)));
17606 evalcond[7]=((((-1.0)*gconst23*x1390))+new_r11+(((-1.0)*x1386*x1388)));
17607 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17608 {
17609 continue;
17610 }
17611 }
17612 
17613 {
17614 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17615 vinfos[0].jointtype = 1;
17616 vinfos[0].foffset = j0;
17617 vinfos[0].indices[0] = _ij0[0];
17618 vinfos[0].indices[1] = _ij0[1];
17619 vinfos[0].maxsolutions = _nj0;
17620 vinfos[1].jointtype = 1;
17621 vinfos[1].foffset = j1;
17622 vinfos[1].indices[0] = _ij1[0];
17623 vinfos[1].indices[1] = _ij1[1];
17624 vinfos[1].maxsolutions = _nj1;
17625 vinfos[2].jointtype = 1;
17626 vinfos[2].foffset = j2;
17627 vinfos[2].indices[0] = _ij2[0];
17628 vinfos[2].indices[1] = _ij2[1];
17629 vinfos[2].maxsolutions = _nj2;
17630 vinfos[3].jointtype = 1;
17631 vinfos[3].foffset = j3;
17632 vinfos[3].indices[0] = _ij3[0];
17633 vinfos[3].indices[1] = _ij3[1];
17634 vinfos[3].maxsolutions = _nj3;
17635 vinfos[4].jointtype = 1;
17636 vinfos[4].foffset = j4;
17637 vinfos[4].indices[0] = _ij4[0];
17638 vinfos[4].indices[1] = _ij4[1];
17639 vinfos[4].maxsolutions = _nj4;
17640 vinfos[5].jointtype = 1;
17641 vinfos[5].foffset = j5;
17642 vinfos[5].indices[0] = _ij5[0];
17643 vinfos[5].indices[1] = _ij5[1];
17644 vinfos[5].maxsolutions = _nj5;
17645 std::vector<int> vfree(0);
17646 solutions.AddSolution(vinfos,vfree);
17647 }
17648 }
17649 }
17650 
17651 }
17652 
17653 }
17654 
17655 } else
17656 {
17657 {
17658 IkReal j3array[1], cj3array[1], sj3array[1];
17659 bool j3valid[1]={false};
17660 _nj3 = 1;
17661 IkReal x1392=((1.0)*new_r10);
17662 CheckValue<IkReal> x1393=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x1392))+(((-1.0)*new_r00*new_r01)))),-1);
17663 if(!x1393.valid){
17664 continue;
17665 }
17666 CheckValue<IkReal> x1394 = IKatan2WithCheck(IkReal((((gconst23*new_r11))+((gconst23*new_r00)))),IkReal((((gconst23*new_r01))+(((-1.0)*gconst23*x1392)))),IKFAST_ATAN2_MAGTHRESH);
17667 if(!x1394.valid){
17668 continue;
17669 }
17670 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1393.value)))+(x1394.value));
17671 sj3array[0]=IKsin(j3array[0]);
17672 cj3array[0]=IKcos(j3array[0]);
17673 if( j3array[0] > IKPI )
17674 {
17675  j3array[0]-=IK2PI;
17676 }
17677 else if( j3array[0] < -IKPI )
17678 { j3array[0]+=IK2PI;
17679 }
17680 j3valid[0] = true;
17681 for(int ij3 = 0; ij3 < 1; ++ij3)
17682 {
17683 if( !j3valid[ij3] )
17684 {
17685  continue;
17686 }
17687 _ij3[0] = ij3; _ij3[1] = -1;
17688 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17689 {
17690 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17691 {
17692  j3valid[iij3]=false; _ij3[1] = iij3; break;
17693 }
17694 }
17695 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17696 {
17697 IkReal evalcond[8];
17698 IkReal x1395=IKsin(j3);
17699 IkReal x1396=IKcos(j3);
17700 IkReal x1397=((1.0)*gconst22);
17701 IkReal x1398=(gconst23*x1395);
17702 IkReal x1399=((1.0)*x1396);
17703 IkReal x1400=(x1396*x1397);
17704 evalcond[0]=(((new_r10*x1395))+gconst23+((new_r00*x1396)));
17705 evalcond[1]=(((gconst22*x1395))+((gconst23*x1396))+new_r00);
17706 evalcond[2]=(gconst22+((new_r00*x1395))+(((-1.0)*new_r10*x1399)));
17707 evalcond[3]=(gconst23+(((-1.0)*new_r11*x1399))+((new_r01*x1395)));
17708 evalcond[4]=(x1398+(((-1.0)*x1400))+new_r01);
17709 evalcond[5]=(x1398+(((-1.0)*x1400))+new_r10);
17710 evalcond[6]=(((new_r11*x1395))+((new_r01*x1396))+(((-1.0)*x1397)));
17711 evalcond[7]=((((-1.0)*x1395*x1397))+(((-1.0)*gconst23*x1399))+new_r11);
17712 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17713 {
17714 continue;
17715 }
17716 }
17717 
17718 {
17719 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17720 vinfos[0].jointtype = 1;
17721 vinfos[0].foffset = j0;
17722 vinfos[0].indices[0] = _ij0[0];
17723 vinfos[0].indices[1] = _ij0[1];
17724 vinfos[0].maxsolutions = _nj0;
17725 vinfos[1].jointtype = 1;
17726 vinfos[1].foffset = j1;
17727 vinfos[1].indices[0] = _ij1[0];
17728 vinfos[1].indices[1] = _ij1[1];
17729 vinfos[1].maxsolutions = _nj1;
17730 vinfos[2].jointtype = 1;
17731 vinfos[2].foffset = j2;
17732 vinfos[2].indices[0] = _ij2[0];
17733 vinfos[2].indices[1] = _ij2[1];
17734 vinfos[2].maxsolutions = _nj2;
17735 vinfos[3].jointtype = 1;
17736 vinfos[3].foffset = j3;
17737 vinfos[3].indices[0] = _ij3[0];
17738 vinfos[3].indices[1] = _ij3[1];
17739 vinfos[3].maxsolutions = _nj3;
17740 vinfos[4].jointtype = 1;
17741 vinfos[4].foffset = j4;
17742 vinfos[4].indices[0] = _ij4[0];
17743 vinfos[4].indices[1] = _ij4[1];
17744 vinfos[4].maxsolutions = _nj4;
17745 vinfos[5].jointtype = 1;
17746 vinfos[5].foffset = j5;
17747 vinfos[5].indices[0] = _ij5[0];
17748 vinfos[5].indices[1] = _ij5[1];
17749 vinfos[5].maxsolutions = _nj5;
17750 std::vector<int> vfree(0);
17751 solutions.AddSolution(vinfos,vfree);
17752 }
17753 }
17754 }
17755 
17756 }
17757 
17758 }
17759 
17760 }
17761 } while(0);
17762 if( bgotonextstatement )
17763 {
17764 bool bgotonextstatement = true;
17765 do
17766 {
17767 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17768 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17769 {
17770 bgotonextstatement=false;
17771 {
17772 IkReal j3array[1], cj3array[1], sj3array[1];
17773 bool j3valid[1]={false};
17774 _nj3 = 1;
17775 if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
17776  continue;
17777 j3array[0]=IKatan2(((-1.0)*new_r00), new_r01);
17778 sj3array[0]=IKsin(j3array[0]);
17779 cj3array[0]=IKcos(j3array[0]);
17780 if( j3array[0] > IKPI )
17781 {
17782  j3array[0]-=IK2PI;
17783 }
17784 else if( j3array[0] < -IKPI )
17785 { j3array[0]+=IK2PI;
17786 }
17787 j3valid[0] = true;
17788 for(int ij3 = 0; ij3 < 1; ++ij3)
17789 {
17790 if( !j3valid[ij3] )
17791 {
17792  continue;
17793 }
17794 _ij3[0] = ij3; _ij3[1] = -1;
17795 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17796 {
17797 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17798 {
17799  j3valid[iij3]=false; _ij3[1] = iij3; break;
17800 }
17801 }
17802 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17803 {
17804 IkReal evalcond[8];
17805 IkReal x1401=IKsin(j3);
17806 IkReal x1402=IKcos(j3);
17807 IkReal x1403=((1.0)*x1402);
17808 evalcond[0]=(x1401+new_r00);
17809 evalcond[1]=((((-1.0)*x1403))+new_r01);
17810 evalcond[2]=((((-1.0)*x1401))+new_r11);
17811 evalcond[3]=((((-1.0)*x1403))+new_r10);
17812 evalcond[4]=(((new_r10*x1401))+((new_r00*x1402)));
17813 evalcond[5]=(((new_r01*x1401))+(((-1.0)*new_r11*x1403)));
17814 evalcond[6]=((-1.0)+((new_r01*x1402))+((new_r11*x1401)));
17815 evalcond[7]=((1.0)+((new_r00*x1401))+(((-1.0)*new_r10*x1403)));
17816 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17817 {
17818 continue;
17819 }
17820 }
17821 
17822 {
17823 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17824 vinfos[0].jointtype = 1;
17825 vinfos[0].foffset = j0;
17826 vinfos[0].indices[0] = _ij0[0];
17827 vinfos[0].indices[1] = _ij0[1];
17828 vinfos[0].maxsolutions = _nj0;
17829 vinfos[1].jointtype = 1;
17830 vinfos[1].foffset = j1;
17831 vinfos[1].indices[0] = _ij1[0];
17832 vinfos[1].indices[1] = _ij1[1];
17833 vinfos[1].maxsolutions = _nj1;
17834 vinfos[2].jointtype = 1;
17835 vinfos[2].foffset = j2;
17836 vinfos[2].indices[0] = _ij2[0];
17837 vinfos[2].indices[1] = _ij2[1];
17838 vinfos[2].maxsolutions = _nj2;
17839 vinfos[3].jointtype = 1;
17840 vinfos[3].foffset = j3;
17841 vinfos[3].indices[0] = _ij3[0];
17842 vinfos[3].indices[1] = _ij3[1];
17843 vinfos[3].maxsolutions = _nj3;
17844 vinfos[4].jointtype = 1;
17845 vinfos[4].foffset = j4;
17846 vinfos[4].indices[0] = _ij4[0];
17847 vinfos[4].indices[1] = _ij4[1];
17848 vinfos[4].maxsolutions = _nj4;
17849 vinfos[5].jointtype = 1;
17850 vinfos[5].foffset = j5;
17851 vinfos[5].indices[0] = _ij5[0];
17852 vinfos[5].indices[1] = _ij5[1];
17853 vinfos[5].maxsolutions = _nj5;
17854 std::vector<int> vfree(0);
17855 solutions.AddSolution(vinfos,vfree);
17856 }
17857 }
17858 }
17859 
17860 }
17861 } while(0);
17862 if( bgotonextstatement )
17863 {
17864 bool bgotonextstatement = true;
17865 do
17866 {
17867 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17868 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17869 {
17870 bgotonextstatement=false;
17871 {
17872 IkReal j3array[1], cj3array[1], sj3array[1];
17873 bool j3valid[1]={false};
17874 _nj3 = 1;
17875 if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
17876  continue;
17877 j3array[0]=IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17878 sj3array[0]=IKsin(j3array[0]);
17879 cj3array[0]=IKcos(j3array[0]);
17880 if( j3array[0] > IKPI )
17881 {
17882  j3array[0]-=IK2PI;
17883 }
17884 else if( j3array[0] < -IKPI )
17885 { j3array[0]+=IK2PI;
17886 }
17887 j3valid[0] = true;
17888 for(int ij3 = 0; ij3 < 1; ++ij3)
17889 {
17890 if( !j3valid[ij3] )
17891 {
17892  continue;
17893 }
17894 _ij3[0] = ij3; _ij3[1] = -1;
17895 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17896 {
17897 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17898 {
17899  j3valid[iij3]=false; _ij3[1] = iij3; break;
17900 }
17901 }
17902 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17903 {
17904 IkReal evalcond[8];
17905 IkReal x1404=IKcos(j3);
17906 IkReal x1405=IKsin(j3);
17907 IkReal x1406=((1.0)*x1404);
17908 evalcond[0]=(x1404+new_r01);
17909 evalcond[1]=(x1405+new_r11);
17910 evalcond[2]=(x1404+new_r10);
17911 evalcond[3]=((((-1.0)*x1405))+new_r00);
17912 evalcond[4]=(((new_r10*x1405))+((new_r00*x1404)));
17913 evalcond[5]=(((new_r01*x1405))+(((-1.0)*new_r11*x1406)));
17914 evalcond[6]=((1.0)+((new_r01*x1404))+((new_r11*x1405)));
17915 evalcond[7]=((-1.0)+((new_r00*x1405))+(((-1.0)*new_r10*x1406)));
17916 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17917 {
17918 continue;
17919 }
17920 }
17921 
17922 {
17923 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17924 vinfos[0].jointtype = 1;
17925 vinfos[0].foffset = j0;
17926 vinfos[0].indices[0] = _ij0[0];
17927 vinfos[0].indices[1] = _ij0[1];
17928 vinfos[0].maxsolutions = _nj0;
17929 vinfos[1].jointtype = 1;
17930 vinfos[1].foffset = j1;
17931 vinfos[1].indices[0] = _ij1[0];
17932 vinfos[1].indices[1] = _ij1[1];
17933 vinfos[1].maxsolutions = _nj1;
17934 vinfos[2].jointtype = 1;
17935 vinfos[2].foffset = j2;
17936 vinfos[2].indices[0] = _ij2[0];
17937 vinfos[2].indices[1] = _ij2[1];
17938 vinfos[2].maxsolutions = _nj2;
17939 vinfos[3].jointtype = 1;
17940 vinfos[3].foffset = j3;
17941 vinfos[3].indices[0] = _ij3[0];
17942 vinfos[3].indices[1] = _ij3[1];
17943 vinfos[3].maxsolutions = _nj3;
17944 vinfos[4].jointtype = 1;
17945 vinfos[4].foffset = j4;
17946 vinfos[4].indices[0] = _ij4[0];
17947 vinfos[4].indices[1] = _ij4[1];
17948 vinfos[4].maxsolutions = _nj4;
17949 vinfos[5].jointtype = 1;
17950 vinfos[5].foffset = j5;
17951 vinfos[5].indices[0] = _ij5[0];
17952 vinfos[5].indices[1] = _ij5[1];
17953 vinfos[5].maxsolutions = _nj5;
17954 std::vector<int> vfree(0);
17955 solutions.AddSolution(vinfos,vfree);
17956 }
17957 }
17958 }
17959 
17960 }
17961 } while(0);
17962 if( bgotonextstatement )
17963 {
17964 bool bgotonextstatement = true;
17965 do
17966 {
17967 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
17968 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17969 {
17970 bgotonextstatement=false;
17971 {
17972 IkReal j3eval[3];
17973 sj4=0;
17974 cj4=-1.0;
17975 j4=3.14159265358979;
17976 new_r11=0;
17977 new_r00=0;
17978 j3eval[0]=new_r10;
17979 j3eval[1]=IKsign(new_r10);
17980 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
17981 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
17982 {
17983 {
17984 IkReal j3eval[3];
17985 sj4=0;
17986 cj4=-1.0;
17987 j4=3.14159265358979;
17988 new_r11=0;
17989 new_r00=0;
17990 j3eval[0]=new_r01;
17991 j3eval[1]=IKsign(new_r01);
17992 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
17993 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
17994 {
17995 {
17996 IkReal j3eval[2];
17997 sj4=0;
17998 cj4=-1.0;
17999 j4=3.14159265358979;
18000 new_r11=0;
18001 new_r00=0;
18002 j3eval[0]=new_r01;
18003 j3eval[1]=new_r10;
18004 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18005 {
18006 continue; // no branches [j3]
18007 
18008 } else
18009 {
18010 {
18011 IkReal j3array[1], cj3array[1], sj3array[1];
18012 bool j3valid[1]={false};
18013 _nj3 = 1;
18014 CheckValue<IkReal> x1407=IKPowWithIntegerCheck(new_r01,-1);
18015 if(!x1407.valid){
18016 continue;
18017 }
18018 CheckValue<IkReal> x1408=IKPowWithIntegerCheck(new_r10,-1);
18019 if(!x1408.valid){
18020 continue;
18021 }
18022 if( IKabs(((-1.0)*cj5*(x1407.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x1408.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x1407.value)))+IKsqr((sj5*(x1408.value)))-1) <= IKFAST_SINCOS_THRESH )
18023  continue;
18024 j3array[0]=IKatan2(((-1.0)*cj5*(x1407.value)), (sj5*(x1408.value)));
18025 sj3array[0]=IKsin(j3array[0]);
18026 cj3array[0]=IKcos(j3array[0]);
18027 if( j3array[0] > IKPI )
18028 {
18029  j3array[0]-=IK2PI;
18030 }
18031 else if( j3array[0] < -IKPI )
18032 { j3array[0]+=IK2PI;
18033 }
18034 j3valid[0] = true;
18035 for(int ij3 = 0; ij3 < 1; ++ij3)
18036 {
18037 if( !j3valid[ij3] )
18038 {
18039  continue;
18040 }
18041 _ij3[0] = ij3; _ij3[1] = -1;
18042 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18043 {
18044 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18045 {
18046  j3valid[iij3]=false; _ij3[1] = iij3; break;
18047 }
18048 }
18049 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18050 {
18051 IkReal evalcond[7];
18052 IkReal x1409=IKsin(j3);
18053 IkReal x1410=IKcos(j3);
18054 IkReal x1411=((1.0)*sj5);
18055 IkReal x1412=(cj5*x1409);
18056 IkReal x1413=(x1410*x1411);
18057 evalcond[0]=(cj5+((new_r01*x1409)));
18058 evalcond[1]=(cj5+((new_r10*x1409)));
18059 evalcond[2]=(sj5+(((-1.0)*new_r10*x1410)));
18060 evalcond[3]=(((new_r01*x1410))+(((-1.0)*x1411)));
18061 evalcond[4]=(((sj5*x1409))+((cj5*x1410)));
18062 evalcond[5]=((((-1.0)*x1413))+x1412+new_r01);
18063 evalcond[6]=((((-1.0)*x1413))+x1412+new_r10);
18064 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18065 {
18066 continue;
18067 }
18068 }
18069 
18070 {
18071 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18072 vinfos[0].jointtype = 1;
18073 vinfos[0].foffset = j0;
18074 vinfos[0].indices[0] = _ij0[0];
18075 vinfos[0].indices[1] = _ij0[1];
18076 vinfos[0].maxsolutions = _nj0;
18077 vinfos[1].jointtype = 1;
18078 vinfos[1].foffset = j1;
18079 vinfos[1].indices[0] = _ij1[0];
18080 vinfos[1].indices[1] = _ij1[1];
18081 vinfos[1].maxsolutions = _nj1;
18082 vinfos[2].jointtype = 1;
18083 vinfos[2].foffset = j2;
18084 vinfos[2].indices[0] = _ij2[0];
18085 vinfos[2].indices[1] = _ij2[1];
18086 vinfos[2].maxsolutions = _nj2;
18087 vinfos[3].jointtype = 1;
18088 vinfos[3].foffset = j3;
18089 vinfos[3].indices[0] = _ij3[0];
18090 vinfos[3].indices[1] = _ij3[1];
18091 vinfos[3].maxsolutions = _nj3;
18092 vinfos[4].jointtype = 1;
18093 vinfos[4].foffset = j4;
18094 vinfos[4].indices[0] = _ij4[0];
18095 vinfos[4].indices[1] = _ij4[1];
18096 vinfos[4].maxsolutions = _nj4;
18097 vinfos[5].jointtype = 1;
18098 vinfos[5].foffset = j5;
18099 vinfos[5].indices[0] = _ij5[0];
18100 vinfos[5].indices[1] = _ij5[1];
18101 vinfos[5].maxsolutions = _nj5;
18102 std::vector<int> vfree(0);
18103 solutions.AddSolution(vinfos,vfree);
18104 }
18105 }
18106 }
18107 
18108 }
18109 
18110 }
18111 
18112 } else
18113 {
18114 {
18115 IkReal j3array[1], cj3array[1], sj3array[1];
18116 bool j3valid[1]={false};
18117 _nj3 = 1;
18118 CheckValue<IkReal> x1414=IKPowWithIntegerCheck(IKsign(new_r01),-1);
18119 if(!x1414.valid){
18120 continue;
18121 }
18122 CheckValue<IkReal> x1415 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18123 if(!x1415.valid){
18124 continue;
18125 }
18126 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1414.value)))+(x1415.value));
18127 sj3array[0]=IKsin(j3array[0]);
18128 cj3array[0]=IKcos(j3array[0]);
18129 if( j3array[0] > IKPI )
18130 {
18131  j3array[0]-=IK2PI;
18132 }
18133 else if( j3array[0] < -IKPI )
18134 { j3array[0]+=IK2PI;
18135 }
18136 j3valid[0] = true;
18137 for(int ij3 = 0; ij3 < 1; ++ij3)
18138 {
18139 if( !j3valid[ij3] )
18140 {
18141  continue;
18142 }
18143 _ij3[0] = ij3; _ij3[1] = -1;
18144 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18145 {
18146 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18147 {
18148  j3valid[iij3]=false; _ij3[1] = iij3; break;
18149 }
18150 }
18151 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18152 {
18153 IkReal evalcond[7];
18154 IkReal x1416=IKsin(j3);
18155 IkReal x1417=IKcos(j3);
18156 IkReal x1418=((1.0)*sj5);
18157 IkReal x1419=(cj5*x1416);
18158 IkReal x1420=(x1417*x1418);
18159 evalcond[0]=(cj5+((new_r01*x1416)));
18160 evalcond[1]=(cj5+((new_r10*x1416)));
18161 evalcond[2]=(sj5+(((-1.0)*new_r10*x1417)));
18162 evalcond[3]=(((new_r01*x1417))+(((-1.0)*x1418)));
18163 evalcond[4]=(((sj5*x1416))+((cj5*x1417)));
18164 evalcond[5]=(x1419+new_r01+(((-1.0)*x1420)));
18165 evalcond[6]=(x1419+new_r10+(((-1.0)*x1420)));
18166 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18167 {
18168 continue;
18169 }
18170 }
18171 
18172 {
18173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18174 vinfos[0].jointtype = 1;
18175 vinfos[0].foffset = j0;
18176 vinfos[0].indices[0] = _ij0[0];
18177 vinfos[0].indices[1] = _ij0[1];
18178 vinfos[0].maxsolutions = _nj0;
18179 vinfos[1].jointtype = 1;
18180 vinfos[1].foffset = j1;
18181 vinfos[1].indices[0] = _ij1[0];
18182 vinfos[1].indices[1] = _ij1[1];
18183 vinfos[1].maxsolutions = _nj1;
18184 vinfos[2].jointtype = 1;
18185 vinfos[2].foffset = j2;
18186 vinfos[2].indices[0] = _ij2[0];
18187 vinfos[2].indices[1] = _ij2[1];
18188 vinfos[2].maxsolutions = _nj2;
18189 vinfos[3].jointtype = 1;
18190 vinfos[3].foffset = j3;
18191 vinfos[3].indices[0] = _ij3[0];
18192 vinfos[3].indices[1] = _ij3[1];
18193 vinfos[3].maxsolutions = _nj3;
18194 vinfos[4].jointtype = 1;
18195 vinfos[4].foffset = j4;
18196 vinfos[4].indices[0] = _ij4[0];
18197 vinfos[4].indices[1] = _ij4[1];
18198 vinfos[4].maxsolutions = _nj4;
18199 vinfos[5].jointtype = 1;
18200 vinfos[5].foffset = j5;
18201 vinfos[5].indices[0] = _ij5[0];
18202 vinfos[5].indices[1] = _ij5[1];
18203 vinfos[5].maxsolutions = _nj5;
18204 std::vector<int> vfree(0);
18205 solutions.AddSolution(vinfos,vfree);
18206 }
18207 }
18208 }
18209 
18210 }
18211 
18212 }
18213 
18214 } else
18215 {
18216 {
18217 IkReal j3array[1], cj3array[1], sj3array[1];
18218 bool j3valid[1]={false};
18219 _nj3 = 1;
18220 CheckValue<IkReal> x1421=IKPowWithIntegerCheck(IKsign(new_r10),-1);
18221 if(!x1421.valid){
18222 continue;
18223 }
18224 CheckValue<IkReal> x1422 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18225 if(!x1422.valid){
18226 continue;
18227 }
18228 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1421.value)))+(x1422.value));
18229 sj3array[0]=IKsin(j3array[0]);
18230 cj3array[0]=IKcos(j3array[0]);
18231 if( j3array[0] > IKPI )
18232 {
18233  j3array[0]-=IK2PI;
18234 }
18235 else if( j3array[0] < -IKPI )
18236 { j3array[0]+=IK2PI;
18237 }
18238 j3valid[0] = true;
18239 for(int ij3 = 0; ij3 < 1; ++ij3)
18240 {
18241 if( !j3valid[ij3] )
18242 {
18243  continue;
18244 }
18245 _ij3[0] = ij3; _ij3[1] = -1;
18246 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18247 {
18248 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18249 {
18250  j3valid[iij3]=false; _ij3[1] = iij3; break;
18251 }
18252 }
18253 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18254 {
18255 IkReal evalcond[7];
18256 IkReal x1423=IKsin(j3);
18257 IkReal x1424=IKcos(j3);
18258 IkReal x1425=((1.0)*sj5);
18259 IkReal x1426=(cj5*x1423);
18260 IkReal x1427=(x1424*x1425);
18261 evalcond[0]=(cj5+((new_r01*x1423)));
18262 evalcond[1]=(cj5+((new_r10*x1423)));
18263 evalcond[2]=(sj5+(((-1.0)*new_r10*x1424)));
18264 evalcond[3]=(((new_r01*x1424))+(((-1.0)*x1425)));
18265 evalcond[4]=(((sj5*x1423))+((cj5*x1424)));
18266 evalcond[5]=(x1426+new_r01+(((-1.0)*x1427)));
18267 evalcond[6]=(x1426+new_r10+(((-1.0)*x1427)));
18268 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18269 {
18270 continue;
18271 }
18272 }
18273 
18274 {
18275 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18276 vinfos[0].jointtype = 1;
18277 vinfos[0].foffset = j0;
18278 vinfos[0].indices[0] = _ij0[0];
18279 vinfos[0].indices[1] = _ij0[1];
18280 vinfos[0].maxsolutions = _nj0;
18281 vinfos[1].jointtype = 1;
18282 vinfos[1].foffset = j1;
18283 vinfos[1].indices[0] = _ij1[0];
18284 vinfos[1].indices[1] = _ij1[1];
18285 vinfos[1].maxsolutions = _nj1;
18286 vinfos[2].jointtype = 1;
18287 vinfos[2].foffset = j2;
18288 vinfos[2].indices[0] = _ij2[0];
18289 vinfos[2].indices[1] = _ij2[1];
18290 vinfos[2].maxsolutions = _nj2;
18291 vinfos[3].jointtype = 1;
18292 vinfos[3].foffset = j3;
18293 vinfos[3].indices[0] = _ij3[0];
18294 vinfos[3].indices[1] = _ij3[1];
18295 vinfos[3].maxsolutions = _nj3;
18296 vinfos[4].jointtype = 1;
18297 vinfos[4].foffset = j4;
18298 vinfos[4].indices[0] = _ij4[0];
18299 vinfos[4].indices[1] = _ij4[1];
18300 vinfos[4].maxsolutions = _nj4;
18301 vinfos[5].jointtype = 1;
18302 vinfos[5].foffset = j5;
18303 vinfos[5].indices[0] = _ij5[0];
18304 vinfos[5].indices[1] = _ij5[1];
18305 vinfos[5].maxsolutions = _nj5;
18306 std::vector<int> vfree(0);
18307 solutions.AddSolution(vinfos,vfree);
18308 }
18309 }
18310 }
18311 
18312 }
18313 
18314 }
18315 
18316 }
18317 } while(0);
18318 if( bgotonextstatement )
18319 {
18320 bool bgotonextstatement = true;
18321 do
18322 {
18323 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18324 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18325 {
18326 bgotonextstatement=false;
18327 {
18328 IkReal j3eval[1];
18329 sj4=0;
18330 cj4=-1.0;
18331 j4=3.14159265358979;
18332 new_r11=0;
18333 new_r01=0;
18334 new_r22=0;
18335 new_r20=0;
18336 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18337 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18338 {
18339 continue; // no branches [j3]
18340 
18341 } else
18342 {
18343 {
18344 IkReal j3array[2], cj3array[2], sj3array[2];
18345 bool j3valid[2]={false};
18346 _nj3 = 2;
18347 CheckValue<IkReal> x1429 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
18348 if(!x1429.valid){
18349 continue;
18350 }
18351 IkReal x1428=x1429.value;
18352 j3array[0]=((-1.0)*x1428);
18353 sj3array[0]=IKsin(j3array[0]);
18354 cj3array[0]=IKcos(j3array[0]);
18355 j3array[1]=((3.14159265358979)+(((-1.0)*x1428)));
18356 sj3array[1]=IKsin(j3array[1]);
18357 cj3array[1]=IKcos(j3array[1]);
18358 if( j3array[0] > IKPI )
18359 {
18360  j3array[0]-=IK2PI;
18361 }
18362 else if( j3array[0] < -IKPI )
18363 { j3array[0]+=IK2PI;
18364 }
18365 j3valid[0] = true;
18366 if( j3array[1] > IKPI )
18367 {
18368  j3array[1]-=IK2PI;
18369 }
18370 else if( j3array[1] < -IKPI )
18371 { j3array[1]+=IK2PI;
18372 }
18373 j3valid[1] = true;
18374 for(int ij3 = 0; ij3 < 2; ++ij3)
18375 {
18376 if( !j3valid[ij3] )
18377 {
18378  continue;
18379 }
18380 _ij3[0] = ij3; _ij3[1] = -1;
18381 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18382 {
18383 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18384 {
18385  j3valid[iij3]=false; _ij3[1] = iij3; break;
18386 }
18387 }
18388 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18389 {
18390 IkReal evalcond[1];
18391 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
18392 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18393 {
18394 continue;
18395 }
18396 }
18397 
18398 {
18399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18400 vinfos[0].jointtype = 1;
18401 vinfos[0].foffset = j0;
18402 vinfos[0].indices[0] = _ij0[0];
18403 vinfos[0].indices[1] = _ij0[1];
18404 vinfos[0].maxsolutions = _nj0;
18405 vinfos[1].jointtype = 1;
18406 vinfos[1].foffset = j1;
18407 vinfos[1].indices[0] = _ij1[0];
18408 vinfos[1].indices[1] = _ij1[1];
18409 vinfos[1].maxsolutions = _nj1;
18410 vinfos[2].jointtype = 1;
18411 vinfos[2].foffset = j2;
18412 vinfos[2].indices[0] = _ij2[0];
18413 vinfos[2].indices[1] = _ij2[1];
18414 vinfos[2].maxsolutions = _nj2;
18415 vinfos[3].jointtype = 1;
18416 vinfos[3].foffset = j3;
18417 vinfos[3].indices[0] = _ij3[0];
18418 vinfos[3].indices[1] = _ij3[1];
18419 vinfos[3].maxsolutions = _nj3;
18420 vinfos[4].jointtype = 1;
18421 vinfos[4].foffset = j4;
18422 vinfos[4].indices[0] = _ij4[0];
18423 vinfos[4].indices[1] = _ij4[1];
18424 vinfos[4].maxsolutions = _nj4;
18425 vinfos[5].jointtype = 1;
18426 vinfos[5].foffset = j5;
18427 vinfos[5].indices[0] = _ij5[0];
18428 vinfos[5].indices[1] = _ij5[1];
18429 vinfos[5].maxsolutions = _nj5;
18430 std::vector<int> vfree(0);
18431 solutions.AddSolution(vinfos,vfree);
18432 }
18433 }
18434 }
18435 
18436 }
18437 
18438 }
18439 
18440 }
18441 } while(0);
18442 if( bgotonextstatement )
18443 {
18444 bool bgotonextstatement = true;
18445 do
18446 {
18447 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18448 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18449 {
18450 bgotonextstatement=false;
18451 {
18452 IkReal j3eval[1];
18453 sj4=0;
18454 cj4=-1.0;
18455 j4=3.14159265358979;
18456 new_r00=0;
18457 new_r10=0;
18458 new_r21=0;
18459 new_r22=0;
18460 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18461 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18462 {
18463 continue; // no branches [j3]
18464 
18465 } else
18466 {
18467 {
18468 IkReal j3array[2], cj3array[2], sj3array[2];
18469 bool j3valid[2]={false};
18470 _nj3 = 2;
18471 CheckValue<IkReal> x1431 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
18472 if(!x1431.valid){
18473 continue;
18474 }
18475 IkReal x1430=x1431.value;
18476 j3array[0]=((-1.0)*x1430);
18477 sj3array[0]=IKsin(j3array[0]);
18478 cj3array[0]=IKcos(j3array[0]);
18479 j3array[1]=((3.14159265358979)+(((-1.0)*x1430)));
18480 sj3array[1]=IKsin(j3array[1]);
18481 cj3array[1]=IKcos(j3array[1]);
18482 if( j3array[0] > IKPI )
18483 {
18484  j3array[0]-=IK2PI;
18485 }
18486 else if( j3array[0] < -IKPI )
18487 { j3array[0]+=IK2PI;
18488 }
18489 j3valid[0] = true;
18490 if( j3array[1] > IKPI )
18491 {
18492  j3array[1]-=IK2PI;
18493 }
18494 else if( j3array[1] < -IKPI )
18495 { j3array[1]+=IK2PI;
18496 }
18497 j3valid[1] = true;
18498 for(int ij3 = 0; ij3 < 2; ++ij3)
18499 {
18500 if( !j3valid[ij3] )
18501 {
18502  continue;
18503 }
18504 _ij3[0] = ij3; _ij3[1] = -1;
18505 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18506 {
18507 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18508 {
18509  j3valid[iij3]=false; _ij3[1] = iij3; break;
18510 }
18511 }
18512 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18513 {
18514 IkReal evalcond[1];
18515 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
18516 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18517 {
18518 continue;
18519 }
18520 }
18521 
18522 {
18523 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18524 vinfos[0].jointtype = 1;
18525 vinfos[0].foffset = j0;
18526 vinfos[0].indices[0] = _ij0[0];
18527 vinfos[0].indices[1] = _ij0[1];
18528 vinfos[0].maxsolutions = _nj0;
18529 vinfos[1].jointtype = 1;
18530 vinfos[1].foffset = j1;
18531 vinfos[1].indices[0] = _ij1[0];
18532 vinfos[1].indices[1] = _ij1[1];
18533 vinfos[1].maxsolutions = _nj1;
18534 vinfos[2].jointtype = 1;
18535 vinfos[2].foffset = j2;
18536 vinfos[2].indices[0] = _ij2[0];
18537 vinfos[2].indices[1] = _ij2[1];
18538 vinfos[2].maxsolutions = _nj2;
18539 vinfos[3].jointtype = 1;
18540 vinfos[3].foffset = j3;
18541 vinfos[3].indices[0] = _ij3[0];
18542 vinfos[3].indices[1] = _ij3[1];
18543 vinfos[3].maxsolutions = _nj3;
18544 vinfos[4].jointtype = 1;
18545 vinfos[4].foffset = j4;
18546 vinfos[4].indices[0] = _ij4[0];
18547 vinfos[4].indices[1] = _ij4[1];
18548 vinfos[4].maxsolutions = _nj4;
18549 vinfos[5].jointtype = 1;
18550 vinfos[5].foffset = j5;
18551 vinfos[5].indices[0] = _ij5[0];
18552 vinfos[5].indices[1] = _ij5[1];
18553 vinfos[5].maxsolutions = _nj5;
18554 std::vector<int> vfree(0);
18555 solutions.AddSolution(vinfos,vfree);
18556 }
18557 }
18558 }
18559 
18560 }
18561 
18562 }
18563 
18564 }
18565 } while(0);
18566 if( bgotonextstatement )
18567 {
18568 bool bgotonextstatement = true;
18569 do
18570 {
18571 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
18572 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18573 {
18574 bgotonextstatement=false;
18575 {
18576 IkReal j3eval[3];
18577 sj4=0;
18578 cj4=-1.0;
18579 j4=3.14159265358979;
18580 new_r01=0;
18581 new_r10=0;
18582 j3eval[0]=new_r00;
18583 j3eval[1]=IKsign(new_r00);
18584 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18585 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18586 {
18587 {
18588 IkReal j3eval[2];
18589 sj4=0;
18590 cj4=-1.0;
18591 j4=3.14159265358979;
18592 new_r01=0;
18593 new_r10=0;
18594 j3eval[0]=new_r00;
18595 j3eval[1]=new_r11;
18596 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18597 {
18598 continue; // no branches [j3]
18599 
18600 } else
18601 {
18602 {
18603 IkReal j3array[1], cj3array[1], sj3array[1];
18604 bool j3valid[1]={false};
18605 _nj3 = 1;
18606 CheckValue<IkReal> x1432=IKPowWithIntegerCheck(new_r00,-1);
18607 if(!x1432.valid){
18608 continue;
18609 }
18610 CheckValue<IkReal> x1433=IKPowWithIntegerCheck(new_r11,-1);
18611 if(!x1433.valid){
18612 continue;
18613 }
18614 if( IKabs(((-1.0)*sj5*(x1432.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x1433.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x1432.value)))+IKsqr((cj5*(x1433.value)))-1) <= IKFAST_SINCOS_THRESH )
18615  continue;
18616 j3array[0]=IKatan2(((-1.0)*sj5*(x1432.value)), (cj5*(x1433.value)));
18617 sj3array[0]=IKsin(j3array[0]);
18618 cj3array[0]=IKcos(j3array[0]);
18619 if( j3array[0] > IKPI )
18620 {
18621  j3array[0]-=IK2PI;
18622 }
18623 else if( j3array[0] < -IKPI )
18624 { j3array[0]+=IK2PI;
18625 }
18626 j3valid[0] = true;
18627 for(int ij3 = 0; ij3 < 1; ++ij3)
18628 {
18629 if( !j3valid[ij3] )
18630 {
18631  continue;
18632 }
18633 _ij3[0] = ij3; _ij3[1] = -1;
18634 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18635 {
18636 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18637 {
18638  j3valid[iij3]=false; _ij3[1] = iij3; break;
18639 }
18640 }
18641 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18642 {
18643 IkReal evalcond[7];
18644 IkReal x1434=IKcos(j3);
18645 IkReal x1435=IKsin(j3);
18646 IkReal x1436=((1.0)*sj5);
18647 IkReal x1437=(cj5*x1434);
18648 IkReal x1438=((1.0)*x1434);
18649 evalcond[0]=(sj5+((new_r00*x1435)));
18650 evalcond[1]=(cj5+((new_r00*x1434)));
18651 evalcond[2]=(cj5+(((-1.0)*new_r11*x1438)));
18652 evalcond[3]=(((new_r11*x1435))+(((-1.0)*x1436)));
18653 evalcond[4]=((((-1.0)*x1434*x1436))+((cj5*x1435)));
18654 evalcond[5]=(((sj5*x1435))+x1437+new_r00);
18655 evalcond[6]=((((-1.0)*x1437))+(((-1.0)*x1435*x1436))+new_r11);
18656 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18657 {
18658 continue;
18659 }
18660 }
18661 
18662 {
18663 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18664 vinfos[0].jointtype = 1;
18665 vinfos[0].foffset = j0;
18666 vinfos[0].indices[0] = _ij0[0];
18667 vinfos[0].indices[1] = _ij0[1];
18668 vinfos[0].maxsolutions = _nj0;
18669 vinfos[1].jointtype = 1;
18670 vinfos[1].foffset = j1;
18671 vinfos[1].indices[0] = _ij1[0];
18672 vinfos[1].indices[1] = _ij1[1];
18673 vinfos[1].maxsolutions = _nj1;
18674 vinfos[2].jointtype = 1;
18675 vinfos[2].foffset = j2;
18676 vinfos[2].indices[0] = _ij2[0];
18677 vinfos[2].indices[1] = _ij2[1];
18678 vinfos[2].maxsolutions = _nj2;
18679 vinfos[3].jointtype = 1;
18680 vinfos[3].foffset = j3;
18681 vinfos[3].indices[0] = _ij3[0];
18682 vinfos[3].indices[1] = _ij3[1];
18683 vinfos[3].maxsolutions = _nj3;
18684 vinfos[4].jointtype = 1;
18685 vinfos[4].foffset = j4;
18686 vinfos[4].indices[0] = _ij4[0];
18687 vinfos[4].indices[1] = _ij4[1];
18688 vinfos[4].maxsolutions = _nj4;
18689 vinfos[5].jointtype = 1;
18690 vinfos[5].foffset = j5;
18691 vinfos[5].indices[0] = _ij5[0];
18692 vinfos[5].indices[1] = _ij5[1];
18693 vinfos[5].maxsolutions = _nj5;
18694 std::vector<int> vfree(0);
18695 solutions.AddSolution(vinfos,vfree);
18696 }
18697 }
18698 }
18699 
18700 }
18701 
18702 }
18703 
18704 } else
18705 {
18706 {
18707 IkReal j3array[1], cj3array[1], sj3array[1];
18708 bool j3valid[1]={false};
18709 _nj3 = 1;
18710 CheckValue<IkReal> x1439 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(((-1.0)*cj5)),IKFAST_ATAN2_MAGTHRESH);
18711 if(!x1439.valid){
18712 continue;
18713 }
18714 CheckValue<IkReal> x1440=IKPowWithIntegerCheck(IKsign(new_r00),-1);
18715 if(!x1440.valid){
18716 continue;
18717 }
18718 j3array[0]=((-1.5707963267949)+(x1439.value)+(((1.5707963267949)*(x1440.value))));
18719 sj3array[0]=IKsin(j3array[0]);
18720 cj3array[0]=IKcos(j3array[0]);
18721 if( j3array[0] > IKPI )
18722 {
18723  j3array[0]-=IK2PI;
18724 }
18725 else if( j3array[0] < -IKPI )
18726 { j3array[0]+=IK2PI;
18727 }
18728 j3valid[0] = true;
18729 for(int ij3 = 0; ij3 < 1; ++ij3)
18730 {
18731 if( !j3valid[ij3] )
18732 {
18733  continue;
18734 }
18735 _ij3[0] = ij3; _ij3[1] = -1;
18736 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18737 {
18738 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18739 {
18740  j3valid[iij3]=false; _ij3[1] = iij3; break;
18741 }
18742 }
18743 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18744 {
18745 IkReal evalcond[7];
18746 IkReal x1441=IKcos(j3);
18747 IkReal x1442=IKsin(j3);
18748 IkReal x1443=((1.0)*sj5);
18749 IkReal x1444=(cj5*x1441);
18750 IkReal x1445=((1.0)*x1441);
18751 evalcond[0]=(sj5+((new_r00*x1442)));
18752 evalcond[1]=(cj5+((new_r00*x1441)));
18753 evalcond[2]=(cj5+(((-1.0)*new_r11*x1445)));
18754 evalcond[3]=(((new_r11*x1442))+(((-1.0)*x1443)));
18755 evalcond[4]=((((-1.0)*x1441*x1443))+((cj5*x1442)));
18756 evalcond[5]=(((sj5*x1442))+x1444+new_r00);
18757 evalcond[6]=((((-1.0)*x1444))+new_r11+(((-1.0)*x1442*x1443)));
18758 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18759 {
18760 continue;
18761 }
18762 }
18763 
18764 {
18765 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18766 vinfos[0].jointtype = 1;
18767 vinfos[0].foffset = j0;
18768 vinfos[0].indices[0] = _ij0[0];
18769 vinfos[0].indices[1] = _ij0[1];
18770 vinfos[0].maxsolutions = _nj0;
18771 vinfos[1].jointtype = 1;
18772 vinfos[1].foffset = j1;
18773 vinfos[1].indices[0] = _ij1[0];
18774 vinfos[1].indices[1] = _ij1[1];
18775 vinfos[1].maxsolutions = _nj1;
18776 vinfos[2].jointtype = 1;
18777 vinfos[2].foffset = j2;
18778 vinfos[2].indices[0] = _ij2[0];
18779 vinfos[2].indices[1] = _ij2[1];
18780 vinfos[2].maxsolutions = _nj2;
18781 vinfos[3].jointtype = 1;
18782 vinfos[3].foffset = j3;
18783 vinfos[3].indices[0] = _ij3[0];
18784 vinfos[3].indices[1] = _ij3[1];
18785 vinfos[3].maxsolutions = _nj3;
18786 vinfos[4].jointtype = 1;
18787 vinfos[4].foffset = j4;
18788 vinfos[4].indices[0] = _ij4[0];
18789 vinfos[4].indices[1] = _ij4[1];
18790 vinfos[4].maxsolutions = _nj4;
18791 vinfos[5].jointtype = 1;
18792 vinfos[5].foffset = j5;
18793 vinfos[5].indices[0] = _ij5[0];
18794 vinfos[5].indices[1] = _ij5[1];
18795 vinfos[5].maxsolutions = _nj5;
18796 std::vector<int> vfree(0);
18797 solutions.AddSolution(vinfos,vfree);
18798 }
18799 }
18800 }
18801 
18802 }
18803 
18804 }
18805 
18806 }
18807 } while(0);
18808 if( bgotonextstatement )
18809 {
18810 bool bgotonextstatement = true;
18811 do
18812 {
18813 if( 1 )
18814 {
18815 bgotonextstatement=false;
18816 continue; // branch miss [j3]
18817 
18818 }
18819 } while(0);
18820 if( bgotonextstatement )
18821 {
18822 }
18823 }
18824 }
18825 }
18826 }
18827 }
18828 }
18829 }
18830 }
18831 }
18832 }
18833 }
18834 
18835 } else
18836 {
18837 {
18838 IkReal j3array[1], cj3array[1], sj3array[1];
18839 bool j3valid[1]={false};
18840 _nj3 = 1;
18841 IkReal x1446=((1.0)*new_r00);
18842 CheckValue<IkReal> x1447 = IKatan2WithCheck(IkReal(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))),IkReal((((cj5*sj5))+(((-1.0)*new_r10*x1446)))),IKFAST_ATAN2_MAGTHRESH);
18843 if(!x1447.valid){
18844 continue;
18845 }
18846 CheckValue<IkReal> x1448=IKPowWithIntegerCheck(IKsign(((((-1.0)*sj5*x1446))+((cj5*new_r10)))),-1);
18847 if(!x1448.valid){
18848 continue;
18849 }
18850 j3array[0]=((-1.5707963267949)+(x1447.value)+(((1.5707963267949)*(x1448.value))));
18851 sj3array[0]=IKsin(j3array[0]);
18852 cj3array[0]=IKcos(j3array[0]);
18853 if( j3array[0] > IKPI )
18854 {
18855  j3array[0]-=IK2PI;
18856 }
18857 else if( j3array[0] < -IKPI )
18858 { j3array[0]+=IK2PI;
18859 }
18860 j3valid[0] = true;
18861 for(int ij3 = 0; ij3 < 1; ++ij3)
18862 {
18863 if( !j3valid[ij3] )
18864 {
18865  continue;
18866 }
18867 _ij3[0] = ij3; _ij3[1] = -1;
18868 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18869 {
18870 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18871 {
18872  j3valid[iij3]=false; _ij3[1] = iij3; break;
18873 }
18874 }
18875 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18876 {
18877 IkReal evalcond[8];
18878 IkReal x1449=IKsin(j3);
18879 IkReal x1450=IKcos(j3);
18880 IkReal x1451=((1.0)*sj5);
18881 IkReal x1452=(cj5*x1449);
18882 IkReal x1453=((1.0)*x1450);
18883 IkReal x1454=(x1450*x1451);
18884 evalcond[0]=(((new_r10*x1449))+cj5+((new_r00*x1450)));
18885 evalcond[1]=(((cj5*x1450))+((sj5*x1449))+new_r00);
18886 evalcond[2]=((((-1.0)*new_r10*x1453))+sj5+((new_r00*x1449)));
18887 evalcond[3]=(cj5+((new_r01*x1449))+(((-1.0)*new_r11*x1453)));
18888 evalcond[4]=((((-1.0)*x1454))+x1452+new_r01);
18889 evalcond[5]=((((-1.0)*x1454))+x1452+new_r10);
18890 evalcond[6]=(((new_r11*x1449))+(((-1.0)*x1451))+((new_r01*x1450)));
18891 evalcond[7]=((((-1.0)*cj5*x1453))+new_r11+(((-1.0)*x1449*x1451)));
18892 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18893 {
18894 continue;
18895 }
18896 }
18897 
18898 {
18899 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18900 vinfos[0].jointtype = 1;
18901 vinfos[0].foffset = j0;
18902 vinfos[0].indices[0] = _ij0[0];
18903 vinfos[0].indices[1] = _ij0[1];
18904 vinfos[0].maxsolutions = _nj0;
18905 vinfos[1].jointtype = 1;
18906 vinfos[1].foffset = j1;
18907 vinfos[1].indices[0] = _ij1[0];
18908 vinfos[1].indices[1] = _ij1[1];
18909 vinfos[1].maxsolutions = _nj1;
18910 vinfos[2].jointtype = 1;
18911 vinfos[2].foffset = j2;
18912 vinfos[2].indices[0] = _ij2[0];
18913 vinfos[2].indices[1] = _ij2[1];
18914 vinfos[2].maxsolutions = _nj2;
18915 vinfos[3].jointtype = 1;
18916 vinfos[3].foffset = j3;
18917 vinfos[3].indices[0] = _ij3[0];
18918 vinfos[3].indices[1] = _ij3[1];
18919 vinfos[3].maxsolutions = _nj3;
18920 vinfos[4].jointtype = 1;
18921 vinfos[4].foffset = j4;
18922 vinfos[4].indices[0] = _ij4[0];
18923 vinfos[4].indices[1] = _ij4[1];
18924 vinfos[4].maxsolutions = _nj4;
18925 vinfos[5].jointtype = 1;
18926 vinfos[5].foffset = j5;
18927 vinfos[5].indices[0] = _ij5[0];
18928 vinfos[5].indices[1] = _ij5[1];
18929 vinfos[5].maxsolutions = _nj5;
18930 std::vector<int> vfree(0);
18931 solutions.AddSolution(vinfos,vfree);
18932 }
18933 }
18934 }
18935 
18936 }
18937 
18938 }
18939 
18940 } else
18941 {
18942 {
18943 IkReal j3array[1], cj3array[1], sj3array[1];
18944 bool j3valid[1]={false};
18945 _nj3 = 1;
18946 IkReal x1455=((1.0)*new_r10);
18947 CheckValue<IkReal> x1456 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((cj5*cj5)+(((-1.0)*new_r01*x1455)))),IKFAST_ATAN2_MAGTHRESH);
18948 if(!x1456.valid){
18949 continue;
18950 }
18951 CheckValue<IkReal> x1457=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x1455)))),-1);
18952 if(!x1457.valid){
18953 continue;
18954 }
18955 j3array[0]=((-1.5707963267949)+(x1456.value)+(((1.5707963267949)*(x1457.value))));
18956 sj3array[0]=IKsin(j3array[0]);
18957 cj3array[0]=IKcos(j3array[0]);
18958 if( j3array[0] > IKPI )
18959 {
18960  j3array[0]-=IK2PI;
18961 }
18962 else if( j3array[0] < -IKPI )
18963 { j3array[0]+=IK2PI;
18964 }
18965 j3valid[0] = true;
18966 for(int ij3 = 0; ij3 < 1; ++ij3)
18967 {
18968 if( !j3valid[ij3] )
18969 {
18970  continue;
18971 }
18972 _ij3[0] = ij3; _ij3[1] = -1;
18973 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18974 {
18975 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18976 {
18977  j3valid[iij3]=false; _ij3[1] = iij3; break;
18978 }
18979 }
18980 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18981 {
18982 IkReal evalcond[8];
18983 IkReal x1458=IKsin(j3);
18984 IkReal x1459=IKcos(j3);
18985 IkReal x1460=((1.0)*sj5);
18986 IkReal x1461=(cj5*x1458);
18987 IkReal x1462=((1.0)*x1459);
18988 IkReal x1463=(x1459*x1460);
18989 evalcond[0]=(cj5+((new_r00*x1459))+((new_r10*x1458)));
18990 evalcond[1]=(((cj5*x1459))+((sj5*x1458))+new_r00);
18991 evalcond[2]=((((-1.0)*new_r10*x1462))+sj5+((new_r00*x1458)));
18992 evalcond[3]=(cj5+((new_r01*x1458))+(((-1.0)*new_r11*x1462)));
18993 evalcond[4]=((((-1.0)*x1463))+x1461+new_r01);
18994 evalcond[5]=((((-1.0)*x1463))+x1461+new_r10);
18995 evalcond[6]=((((-1.0)*x1460))+((new_r01*x1459))+((new_r11*x1458)));
18996 evalcond[7]=((((-1.0)*cj5*x1462))+(((-1.0)*x1458*x1460))+new_r11);
18997 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18998 {
18999 continue;
19000 }
19001 }
19002 
19003 {
19004 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19005 vinfos[0].jointtype = 1;
19006 vinfos[0].foffset = j0;
19007 vinfos[0].indices[0] = _ij0[0];
19008 vinfos[0].indices[1] = _ij0[1];
19009 vinfos[0].maxsolutions = _nj0;
19010 vinfos[1].jointtype = 1;
19011 vinfos[1].foffset = j1;
19012 vinfos[1].indices[0] = _ij1[0];
19013 vinfos[1].indices[1] = _ij1[1];
19014 vinfos[1].maxsolutions = _nj1;
19015 vinfos[2].jointtype = 1;
19016 vinfos[2].foffset = j2;
19017 vinfos[2].indices[0] = _ij2[0];
19018 vinfos[2].indices[1] = _ij2[1];
19019 vinfos[2].maxsolutions = _nj2;
19020 vinfos[3].jointtype = 1;
19021 vinfos[3].foffset = j3;
19022 vinfos[3].indices[0] = _ij3[0];
19023 vinfos[3].indices[1] = _ij3[1];
19024 vinfos[3].maxsolutions = _nj3;
19025 vinfos[4].jointtype = 1;
19026 vinfos[4].foffset = j4;
19027 vinfos[4].indices[0] = _ij4[0];
19028 vinfos[4].indices[1] = _ij4[1];
19029 vinfos[4].maxsolutions = _nj4;
19030 vinfos[5].jointtype = 1;
19031 vinfos[5].foffset = j5;
19032 vinfos[5].indices[0] = _ij5[0];
19033 vinfos[5].indices[1] = _ij5[1];
19034 vinfos[5].maxsolutions = _nj5;
19035 std::vector<int> vfree(0);
19036 solutions.AddSolution(vinfos,vfree);
19037 }
19038 }
19039 }
19040 
19041 }
19042 
19043 }
19044 
19045 } else
19046 {
19047 {
19048 IkReal j3array[1], cj3array[1], sj3array[1];
19049 bool j3valid[1]={false};
19050 _nj3 = 1;
19051 IkReal x1464=((1.0)*new_r10);
19052 CheckValue<IkReal> x1465=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1464)))),-1);
19053 if(!x1465.valid){
19054 continue;
19055 }
19056 CheckValue<IkReal> x1466 = IKatan2WithCheck(IkReal((((cj5*new_r11))+((cj5*new_r00)))),IkReal(((((-1.0)*cj5*x1464))+((cj5*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
19057 if(!x1466.valid){
19058 continue;
19059 }
19060 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1465.value)))+(x1466.value));
19061 sj3array[0]=IKsin(j3array[0]);
19062 cj3array[0]=IKcos(j3array[0]);
19063 if( j3array[0] > IKPI )
19064 {
19065  j3array[0]-=IK2PI;
19066 }
19067 else if( j3array[0] < -IKPI )
19068 { j3array[0]+=IK2PI;
19069 }
19070 j3valid[0] = true;
19071 for(int ij3 = 0; ij3 < 1; ++ij3)
19072 {
19073 if( !j3valid[ij3] )
19074 {
19075  continue;
19076 }
19077 _ij3[0] = ij3; _ij3[1] = -1;
19078 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19079 {
19080 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19081 {
19082  j3valid[iij3]=false; _ij3[1] = iij3; break;
19083 }
19084 }
19085 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19086 {
19087 IkReal evalcond[8];
19088 IkReal x1467=IKsin(j3);
19089 IkReal x1468=IKcos(j3);
19090 IkReal x1469=((1.0)*sj5);
19091 IkReal x1470=(cj5*x1467);
19092 IkReal x1471=((1.0)*x1468);
19093 IkReal x1472=(x1468*x1469);
19094 evalcond[0]=(((new_r00*x1468))+cj5+((new_r10*x1467)));
19095 evalcond[1]=(((cj5*x1468))+new_r00+((sj5*x1467)));
19096 evalcond[2]=(((new_r00*x1467))+sj5+(((-1.0)*new_r10*x1471)));
19097 evalcond[3]=(((new_r01*x1467))+cj5+(((-1.0)*new_r11*x1471)));
19098 evalcond[4]=(x1470+(((-1.0)*x1472))+new_r01);
19099 evalcond[5]=(x1470+(((-1.0)*x1472))+new_r10);
19100 evalcond[6]=(((new_r01*x1468))+(((-1.0)*x1469))+((new_r11*x1467)));
19101 evalcond[7]=((((-1.0)*x1467*x1469))+(((-1.0)*cj5*x1471))+new_r11);
19102 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19103 {
19104 continue;
19105 }
19106 }
19107 
19108 {
19109 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19110 vinfos[0].jointtype = 1;
19111 vinfos[0].foffset = j0;
19112 vinfos[0].indices[0] = _ij0[0];
19113 vinfos[0].indices[1] = _ij0[1];
19114 vinfos[0].maxsolutions = _nj0;
19115 vinfos[1].jointtype = 1;
19116 vinfos[1].foffset = j1;
19117 vinfos[1].indices[0] = _ij1[0];
19118 vinfos[1].indices[1] = _ij1[1];
19119 vinfos[1].maxsolutions = _nj1;
19120 vinfos[2].jointtype = 1;
19121 vinfos[2].foffset = j2;
19122 vinfos[2].indices[0] = _ij2[0];
19123 vinfos[2].indices[1] = _ij2[1];
19124 vinfos[2].maxsolutions = _nj2;
19125 vinfos[3].jointtype = 1;
19126 vinfos[3].foffset = j3;
19127 vinfos[3].indices[0] = _ij3[0];
19128 vinfos[3].indices[1] = _ij3[1];
19129 vinfos[3].maxsolutions = _nj3;
19130 vinfos[4].jointtype = 1;
19131 vinfos[4].foffset = j4;
19132 vinfos[4].indices[0] = _ij4[0];
19133 vinfos[4].indices[1] = _ij4[1];
19134 vinfos[4].maxsolutions = _nj4;
19135 vinfos[5].jointtype = 1;
19136 vinfos[5].foffset = j5;
19137 vinfos[5].indices[0] = _ij5[0];
19138 vinfos[5].indices[1] = _ij5[1];
19139 vinfos[5].maxsolutions = _nj5;
19140 std::vector<int> vfree(0);
19141 solutions.AddSolution(vinfos,vfree);
19142 }
19143 }
19144 }
19145 
19146 }
19147 
19148 }
19149 
19150 }
19151 } while(0);
19152 if( bgotonextstatement )
19153 {
19154 bool bgotonextstatement = true;
19155 do
19156 {
19157 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
19158 if( IKabs(evalcond[0]) < 0.0000050000000000 )
19159 {
19160 bgotonextstatement=false;
19161 {
19162 IkReal j3eval[1];
19163 new_r02=0;
19164 new_r12=0;
19165 new_r20=0;
19166 new_r21=0;
19167 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
19168 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19169 {
19170 {
19171 IkReal j3eval[1];
19172 new_r02=0;
19173 new_r12=0;
19174 new_r20=0;
19175 new_r21=0;
19176 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
19177 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19178 {
19179 {
19180 IkReal j3eval[1];
19181 new_r02=0;
19182 new_r12=0;
19183 new_r20=0;
19184 new_r21=0;
19185 j3eval[0]=((IKabs((new_r11*new_r22)))+(IKabs((new_r01*new_r22))));
19186 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19187 {
19188 continue; // no branches [j3]
19189 
19190 } else
19191 {
19192 {
19193 IkReal j3array[2], cj3array[2], sj3array[2];
19194 bool j3valid[2]={false};
19195 _nj3 = 2;
19196 CheckValue<IkReal> x1474 = IKatan2WithCheck(IkReal((new_r01*new_r22)),IkReal((new_r11*new_r22)),IKFAST_ATAN2_MAGTHRESH);
19197 if(!x1474.valid){
19198 continue;
19199 }
19200 IkReal x1473=x1474.value;
19201 j3array[0]=((-1.0)*x1473);
19202 sj3array[0]=IKsin(j3array[0]);
19203 cj3array[0]=IKcos(j3array[0]);
19204 j3array[1]=((3.14159265358979)+(((-1.0)*x1473)));
19205 sj3array[1]=IKsin(j3array[1]);
19206 cj3array[1]=IKcos(j3array[1]);
19207 if( j3array[0] > IKPI )
19208 {
19209  j3array[0]-=IK2PI;
19210 }
19211 else if( j3array[0] < -IKPI )
19212 { j3array[0]+=IK2PI;
19213 }
19214 j3valid[0] = true;
19215 if( j3array[1] > IKPI )
19216 {
19217  j3array[1]-=IK2PI;
19218 }
19219 else if( j3array[1] < -IKPI )
19220 { j3array[1]+=IK2PI;
19221 }
19222 j3valid[1] = true;
19223 for(int ij3 = 0; ij3 < 2; ++ij3)
19224 {
19225 if( !j3valid[ij3] )
19226 {
19227  continue;
19228 }
19229 _ij3[0] = ij3; _ij3[1] = -1;
19230 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19231 {
19232 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19233 {
19234  j3valid[iij3]=false; _ij3[1] = iij3; break;
19235 }
19236 }
19237 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19238 {
19239 IkReal evalcond[5];
19240 IkReal x1475=IKsin(j3);
19241 IkReal x1476=IKcos(j3);
19242 IkReal x1477=(new_r00*x1476);
19243 IkReal x1478=((1.0)*x1476);
19244 IkReal x1479=(new_r10*x1475);
19245 evalcond[0]=(((new_r11*x1475))+((new_r01*x1476)));
19246 evalcond[1]=(x1477+x1479);
19247 evalcond[2]=(((new_r00*x1475))+(((-1.0)*new_r10*x1478)));
19248 evalcond[3]=((((-1.0)*new_r11*x1478))+((new_r01*x1475)));
19249 evalcond[4]=(((new_r22*x1477))+((new_r22*x1479)));
19250 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19251 {
19252 continue;
19253 }
19254 }
19255 
19256 {
19257 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19258 vinfos[0].jointtype = 1;
19259 vinfos[0].foffset = j0;
19260 vinfos[0].indices[0] = _ij0[0];
19261 vinfos[0].indices[1] = _ij0[1];
19262 vinfos[0].maxsolutions = _nj0;
19263 vinfos[1].jointtype = 1;
19264 vinfos[1].foffset = j1;
19265 vinfos[1].indices[0] = _ij1[0];
19266 vinfos[1].indices[1] = _ij1[1];
19267 vinfos[1].maxsolutions = _nj1;
19268 vinfos[2].jointtype = 1;
19269 vinfos[2].foffset = j2;
19270 vinfos[2].indices[0] = _ij2[0];
19271 vinfos[2].indices[1] = _ij2[1];
19272 vinfos[2].maxsolutions = _nj2;
19273 vinfos[3].jointtype = 1;
19274 vinfos[3].foffset = j3;
19275 vinfos[3].indices[0] = _ij3[0];
19276 vinfos[3].indices[1] = _ij3[1];
19277 vinfos[3].maxsolutions = _nj3;
19278 vinfos[4].jointtype = 1;
19279 vinfos[4].foffset = j4;
19280 vinfos[4].indices[0] = _ij4[0];
19281 vinfos[4].indices[1] = _ij4[1];
19282 vinfos[4].maxsolutions = _nj4;
19283 vinfos[5].jointtype = 1;
19284 vinfos[5].foffset = j5;
19285 vinfos[5].indices[0] = _ij5[0];
19286 vinfos[5].indices[1] = _ij5[1];
19287 vinfos[5].maxsolutions = _nj5;
19288 std::vector<int> vfree(0);
19289 solutions.AddSolution(vinfos,vfree);
19290 }
19291 }
19292 }
19293 
19294 }
19295 
19296 }
19297 
19298 } else
19299 {
19300 {
19301 IkReal j3array[2], cj3array[2], sj3array[2];
19302 bool j3valid[2]={false};
19303 _nj3 = 2;
19304 CheckValue<IkReal> x1481 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
19305 if(!x1481.valid){
19306 continue;
19307 }
19308 IkReal x1480=x1481.value;
19309 j3array[0]=((-1.0)*x1480);
19310 sj3array[0]=IKsin(j3array[0]);
19311 cj3array[0]=IKcos(j3array[0]);
19312 j3array[1]=((3.14159265358979)+(((-1.0)*x1480)));
19313 sj3array[1]=IKsin(j3array[1]);
19314 cj3array[1]=IKcos(j3array[1]);
19315 if( j3array[0] > IKPI )
19316 {
19317  j3array[0]-=IK2PI;
19318 }
19319 else if( j3array[0] < -IKPI )
19320 { j3array[0]+=IK2PI;
19321 }
19322 j3valid[0] = true;
19323 if( j3array[1] > IKPI )
19324 {
19325  j3array[1]-=IK2PI;
19326 }
19327 else if( j3array[1] < -IKPI )
19328 { j3array[1]+=IK2PI;
19329 }
19330 j3valid[1] = true;
19331 for(int ij3 = 0; ij3 < 2; ++ij3)
19332 {
19333 if( !j3valid[ij3] )
19334 {
19335  continue;
19336 }
19337 _ij3[0] = ij3; _ij3[1] = -1;
19338 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19339 {
19340 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19341 {
19342  j3valid[iij3]=false; _ij3[1] = iij3; break;
19343 }
19344 }
19345 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19346 {
19347 IkReal evalcond[5];
19348 IkReal x1482=IKsin(j3);
19349 IkReal x1483=IKcos(j3);
19350 IkReal x1484=(new_r22*x1483);
19351 IkReal x1485=(new_r22*x1482);
19352 IkReal x1486=((1.0)*x1483);
19353 evalcond[0]=(((new_r11*x1482))+((new_r01*x1483)));
19354 evalcond[1]=((((-1.0)*new_r10*x1486))+((new_r00*x1482)));
19355 evalcond[2]=((((-1.0)*new_r11*x1486))+((new_r01*x1482)));
19356 evalcond[3]=(((new_r11*x1485))+((new_r01*x1484)));
19357 evalcond[4]=(((new_r00*x1484))+((new_r10*x1485)));
19358 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19359 {
19360 continue;
19361 }
19362 }
19363 
19364 {
19365 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19366 vinfos[0].jointtype = 1;
19367 vinfos[0].foffset = j0;
19368 vinfos[0].indices[0] = _ij0[0];
19369 vinfos[0].indices[1] = _ij0[1];
19370 vinfos[0].maxsolutions = _nj0;
19371 vinfos[1].jointtype = 1;
19372 vinfos[1].foffset = j1;
19373 vinfos[1].indices[0] = _ij1[0];
19374 vinfos[1].indices[1] = _ij1[1];
19375 vinfos[1].maxsolutions = _nj1;
19376 vinfos[2].jointtype = 1;
19377 vinfos[2].foffset = j2;
19378 vinfos[2].indices[0] = _ij2[0];
19379 vinfos[2].indices[1] = _ij2[1];
19380 vinfos[2].maxsolutions = _nj2;
19381 vinfos[3].jointtype = 1;
19382 vinfos[3].foffset = j3;
19383 vinfos[3].indices[0] = _ij3[0];
19384 vinfos[3].indices[1] = _ij3[1];
19385 vinfos[3].maxsolutions = _nj3;
19386 vinfos[4].jointtype = 1;
19387 vinfos[4].foffset = j4;
19388 vinfos[4].indices[0] = _ij4[0];
19389 vinfos[4].indices[1] = _ij4[1];
19390 vinfos[4].maxsolutions = _nj4;
19391 vinfos[5].jointtype = 1;
19392 vinfos[5].foffset = j5;
19393 vinfos[5].indices[0] = _ij5[0];
19394 vinfos[5].indices[1] = _ij5[1];
19395 vinfos[5].maxsolutions = _nj5;
19396 std::vector<int> vfree(0);
19397 solutions.AddSolution(vinfos,vfree);
19398 }
19399 }
19400 }
19401 
19402 }
19403 
19404 }
19405 
19406 } else
19407 {
19408 {
19409 IkReal j3array[2], cj3array[2], sj3array[2];
19410 bool j3valid[2]={false};
19411 _nj3 = 2;
19412 CheckValue<IkReal> x1488 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
19413 if(!x1488.valid){
19414 continue;
19415 }
19416 IkReal x1487=x1488.value;
19417 j3array[0]=((-1.0)*x1487);
19418 sj3array[0]=IKsin(j3array[0]);
19419 cj3array[0]=IKcos(j3array[0]);
19420 j3array[1]=((3.14159265358979)+(((-1.0)*x1487)));
19421 sj3array[1]=IKsin(j3array[1]);
19422 cj3array[1]=IKcos(j3array[1]);
19423 if( j3array[0] > IKPI )
19424 {
19425  j3array[0]-=IK2PI;
19426 }
19427 else if( j3array[0] < -IKPI )
19428 { j3array[0]+=IK2PI;
19429 }
19430 j3valid[0] = true;
19431 if( j3array[1] > IKPI )
19432 {
19433  j3array[1]-=IK2PI;
19434 }
19435 else if( j3array[1] < -IKPI )
19436 { j3array[1]+=IK2PI;
19437 }
19438 j3valid[1] = true;
19439 for(int ij3 = 0; ij3 < 2; ++ij3)
19440 {
19441 if( !j3valid[ij3] )
19442 {
19443  continue;
19444 }
19445 _ij3[0] = ij3; _ij3[1] = -1;
19446 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19447 {
19448 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19449 {
19450  j3valid[iij3]=false; _ij3[1] = iij3; break;
19451 }
19452 }
19453 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19454 {
19455 IkReal evalcond[5];
19456 IkReal x1489=IKsin(j3);
19457 IkReal x1490=IKcos(j3);
19458 IkReal x1491=(new_r22*x1490);
19459 IkReal x1492=(new_r22*x1489);
19460 IkReal x1493=((1.0)*x1490);
19461 evalcond[0]=(((new_r00*x1490))+((new_r10*x1489)));
19462 evalcond[1]=((((-1.0)*new_r10*x1493))+((new_r00*x1489)));
19463 evalcond[2]=((((-1.0)*new_r11*x1493))+((new_r01*x1489)));
19464 evalcond[3]=(((new_r01*x1491))+((new_r11*x1492)));
19465 evalcond[4]=(((new_r00*x1491))+((new_r10*x1492)));
19466 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19467 {
19468 continue;
19469 }
19470 }
19471 
19472 {
19473 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19474 vinfos[0].jointtype = 1;
19475 vinfos[0].foffset = j0;
19476 vinfos[0].indices[0] = _ij0[0];
19477 vinfos[0].indices[1] = _ij0[1];
19478 vinfos[0].maxsolutions = _nj0;
19479 vinfos[1].jointtype = 1;
19480 vinfos[1].foffset = j1;
19481 vinfos[1].indices[0] = _ij1[0];
19482 vinfos[1].indices[1] = _ij1[1];
19483 vinfos[1].maxsolutions = _nj1;
19484 vinfos[2].jointtype = 1;
19485 vinfos[2].foffset = j2;
19486 vinfos[2].indices[0] = _ij2[0];
19487 vinfos[2].indices[1] = _ij2[1];
19488 vinfos[2].maxsolutions = _nj2;
19489 vinfos[3].jointtype = 1;
19490 vinfos[3].foffset = j3;
19491 vinfos[3].indices[0] = _ij3[0];
19492 vinfos[3].indices[1] = _ij3[1];
19493 vinfos[3].maxsolutions = _nj3;
19494 vinfos[4].jointtype = 1;
19495 vinfos[4].foffset = j4;
19496 vinfos[4].indices[0] = _ij4[0];
19497 vinfos[4].indices[1] = _ij4[1];
19498 vinfos[4].maxsolutions = _nj4;
19499 vinfos[5].jointtype = 1;
19500 vinfos[5].foffset = j5;
19501 vinfos[5].indices[0] = _ij5[0];
19502 vinfos[5].indices[1] = _ij5[1];
19503 vinfos[5].maxsolutions = _nj5;
19504 std::vector<int> vfree(0);
19505 solutions.AddSolution(vinfos,vfree);
19506 }
19507 }
19508 }
19509 
19510 }
19511 
19512 }
19513 
19514 }
19515 } while(0);
19516 if( bgotonextstatement )
19517 {
19518 bool bgotonextstatement = true;
19519 do
19520 {
19521 if( 1 )
19522 {
19523 bgotonextstatement=false;
19524 continue; // branch miss [j3]
19525 
19526 }
19527 } while(0);
19528 if( bgotonextstatement )
19529 {
19530 }
19531 }
19532 }
19533 }
19534 }
19535 
19536 } else
19537 {
19538 {
19539 IkReal j3array[1], cj3array[1], sj3array[1];
19540 bool j3valid[1]={false};
19541 _nj3 = 1;
19542 CheckValue<IkReal> x1495=IKPowWithIntegerCheck(sj4,-1);
19543 if(!x1495.valid){
19544 continue;
19545 }
19546 IkReal x1494=x1495.value;
19547 CheckValue<IkReal> x1496=IKPowWithIntegerCheck(new_r12,-1);
19548 if(!x1496.valid){
19549 continue;
19550 }
19551 if( IKabs((x1494*(x1496.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x1494)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x1494*(x1496.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x1494))-1) <= IKFAST_SINCOS_THRESH )
19552  continue;
19553 j3array[0]=IKatan2((x1494*(x1496.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1494));
19554 sj3array[0]=IKsin(j3array[0]);
19555 cj3array[0]=IKcos(j3array[0]);
19556 if( j3array[0] > IKPI )
19557 {
19558  j3array[0]-=IK2PI;
19559 }
19560 else if( j3array[0] < -IKPI )
19561 { j3array[0]+=IK2PI;
19562 }
19563 j3valid[0] = true;
19564 for(int ij3 = 0; ij3 < 1; ++ij3)
19565 {
19566 if( !j3valid[ij3] )
19567 {
19568  continue;
19569 }
19570 _ij3[0] = ij3; _ij3[1] = -1;
19571 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19572 {
19573 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19574 {
19575  j3valid[iij3]=false; _ij3[1] = iij3; break;
19576 }
19577 }
19578 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19579 {
19580 IkReal evalcond[18];
19581 IkReal x1497=IKcos(j3);
19582 IkReal x1498=IKsin(j3);
19583 IkReal x1499=((1.0)*cj4);
19584 IkReal x1500=((1.0)*cj5);
19585 IkReal x1501=(cj4*x1497);
19586 IkReal x1502=(sj4*x1498);
19587 IkReal x1503=((1.0)*x1497);
19588 IkReal x1504=(sj4*x1497);
19589 IkReal x1505=(cj4*x1498);
19590 IkReal x1506=(cj5*x1498);
19591 evalcond[0]=(x1504+new_r02);
19592 evalcond[1]=(x1502+new_r12);
19593 evalcond[2]=((((-1.0)*new_r02*x1498))+((new_r12*x1497)));
19594 evalcond[3]=(sj4+((new_r02*x1497))+((new_r12*x1498)));
19595 evalcond[4]=(sj5+(((-1.0)*new_r10*x1503))+((new_r00*x1498)));
19596 evalcond[5]=((((-1.0)*new_r11*x1503))+cj5+((new_r01*x1498)));
19597 evalcond[6]=(((sj5*x1501))+x1506+new_r01);
19598 evalcond[7]=(((new_r01*x1497))+((cj4*sj5))+((new_r11*x1498)));
19599 evalcond[8]=(((sj5*x1498))+new_r00+(((-1.0)*cj5*x1497*x1499)));
19600 evalcond[9]=(((sj5*x1505))+(((-1.0)*x1497*x1500))+new_r11);
19601 evalcond[10]=(((new_r00*x1497))+((new_r10*x1498))+(((-1.0)*cj5*x1499)));
19602 evalcond[11]=((((-1.0)*x1499*x1506))+(((-1.0)*sj5*x1503))+new_r10);
19603 evalcond[12]=(((new_r02*x1501))+((new_r22*sj4))+((new_r12*x1505)));
19604 evalcond[13]=(((new_r10*x1502))+(((-1.0)*new_r20*x1499))+((new_r00*x1504)));
19605 evalcond[14]=((((-1.0)*new_r21*x1499))+((new_r01*x1504))+((new_r11*x1502)));
19606 evalcond[15]=(sj5+((new_r01*x1501))+((new_r21*sj4))+((new_r11*x1505)));
19607 evalcond[16]=((1.0)+((new_r02*x1504))+((new_r12*x1502))+(((-1.0)*new_r22*x1499)));
19608 evalcond[17]=(((new_r10*x1505))+(((-1.0)*x1500))+((new_r20*sj4))+((new_r00*x1501)));
19609 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
19610 {
19611 continue;
19612 }
19613 }
19614 
19615 {
19616 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19617 vinfos[0].jointtype = 1;
19618 vinfos[0].foffset = j0;
19619 vinfos[0].indices[0] = _ij0[0];
19620 vinfos[0].indices[1] = _ij0[1];
19621 vinfos[0].maxsolutions = _nj0;
19622 vinfos[1].jointtype = 1;
19623 vinfos[1].foffset = j1;
19624 vinfos[1].indices[0] = _ij1[0];
19625 vinfos[1].indices[1] = _ij1[1];
19626 vinfos[1].maxsolutions = _nj1;
19627 vinfos[2].jointtype = 1;
19628 vinfos[2].foffset = j2;
19629 vinfos[2].indices[0] = _ij2[0];
19630 vinfos[2].indices[1] = _ij2[1];
19631 vinfos[2].maxsolutions = _nj2;
19632 vinfos[3].jointtype = 1;
19633 vinfos[3].foffset = j3;
19634 vinfos[3].indices[0] = _ij3[0];
19635 vinfos[3].indices[1] = _ij3[1];
19636 vinfos[3].maxsolutions = _nj3;
19637 vinfos[4].jointtype = 1;
19638 vinfos[4].foffset = j4;
19639 vinfos[4].indices[0] = _ij4[0];
19640 vinfos[4].indices[1] = _ij4[1];
19641 vinfos[4].maxsolutions = _nj4;
19642 vinfos[5].jointtype = 1;
19643 vinfos[5].foffset = j5;
19644 vinfos[5].indices[0] = _ij5[0];
19645 vinfos[5].indices[1] = _ij5[1];
19646 vinfos[5].maxsolutions = _nj5;
19647 std::vector<int> vfree(0);
19648 solutions.AddSolution(vinfos,vfree);
19649 }
19650 }
19651 }
19652 
19653 }
19654 
19655 }
19656 
19657 } else
19658 {
19659 {
19660 IkReal j3array[1], cj3array[1], sj3array[1];
19661 bool j3valid[1]={false};
19662 _nj3 = 1;
19663 CheckValue<IkReal> x1507=IKPowWithIntegerCheck(IKsign(sj4),-1);
19664 if(!x1507.valid){
19665 continue;
19666 }
19667 CheckValue<IkReal> x1508 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
19668 if(!x1508.valid){
19669 continue;
19670 }
19671 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1507.value)))+(x1508.value));
19672 sj3array[0]=IKsin(j3array[0]);
19673 cj3array[0]=IKcos(j3array[0]);
19674 if( j3array[0] > IKPI )
19675 {
19676  j3array[0]-=IK2PI;
19677 }
19678 else if( j3array[0] < -IKPI )
19679 { j3array[0]+=IK2PI;
19680 }
19681 j3valid[0] = true;
19682 for(int ij3 = 0; ij3 < 1; ++ij3)
19683 {
19684 if( !j3valid[ij3] )
19685 {
19686  continue;
19687 }
19688 _ij3[0] = ij3; _ij3[1] = -1;
19689 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19690 {
19691 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19692 {
19693  j3valid[iij3]=false; _ij3[1] = iij3; break;
19694 }
19695 }
19696 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19697 {
19698 IkReal evalcond[18];
19699 IkReal x1509=IKcos(j3);
19700 IkReal x1510=IKsin(j3);
19701 IkReal x1511=((1.0)*cj4);
19702 IkReal x1512=((1.0)*cj5);
19703 IkReal x1513=(cj4*x1509);
19704 IkReal x1514=(sj4*x1510);
19705 IkReal x1515=((1.0)*x1509);
19706 IkReal x1516=(sj4*x1509);
19707 IkReal x1517=(cj4*x1510);
19708 IkReal x1518=(cj5*x1510);
19709 evalcond[0]=(x1516+new_r02);
19710 evalcond[1]=(x1514+new_r12);
19711 evalcond[2]=((((-1.0)*new_r02*x1510))+((new_r12*x1509)));
19712 evalcond[3]=(sj4+((new_r02*x1509))+((new_r12*x1510)));
19713 evalcond[4]=(sj5+(((-1.0)*new_r10*x1515))+((new_r00*x1510)));
19714 evalcond[5]=((((-1.0)*new_r11*x1515))+cj5+((new_r01*x1510)));
19715 evalcond[6]=(((sj5*x1513))+x1518+new_r01);
19716 evalcond[7]=(((cj4*sj5))+((new_r01*x1509))+((new_r11*x1510)));
19717 evalcond[8]=(((sj5*x1510))+(((-1.0)*cj5*x1509*x1511))+new_r00);
19718 evalcond[9]=(((sj5*x1517))+new_r11+(((-1.0)*x1509*x1512)));
19719 evalcond[10]=(((new_r10*x1510))+(((-1.0)*cj5*x1511))+((new_r00*x1509)));
19720 evalcond[11]=((((-1.0)*sj5*x1515))+new_r10+(((-1.0)*x1511*x1518)));
19721 evalcond[12]=(((new_r02*x1513))+((new_r22*sj4))+((new_r12*x1517)));
19722 evalcond[13]=(((new_r10*x1514))+(((-1.0)*new_r20*x1511))+((new_r00*x1516)));
19723 evalcond[14]=((((-1.0)*new_r21*x1511))+((new_r01*x1516))+((new_r11*x1514)));
19724 evalcond[15]=(sj5+((new_r01*x1513))+((new_r21*sj4))+((new_r11*x1517)));
19725 evalcond[16]=((1.0)+(((-1.0)*new_r22*x1511))+((new_r02*x1516))+((new_r12*x1514)));
19726 evalcond[17]=(((new_r10*x1517))+(((-1.0)*x1512))+((new_r20*sj4))+((new_r00*x1513)));
19727 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
19728 {
19729 continue;
19730 }
19731 }
19732 
19733 {
19734 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19735 vinfos[0].jointtype = 1;
19736 vinfos[0].foffset = j0;
19737 vinfos[0].indices[0] = _ij0[0];
19738 vinfos[0].indices[1] = _ij0[1];
19739 vinfos[0].maxsolutions = _nj0;
19740 vinfos[1].jointtype = 1;
19741 vinfos[1].foffset = j1;
19742 vinfos[1].indices[0] = _ij1[0];
19743 vinfos[1].indices[1] = _ij1[1];
19744 vinfos[1].maxsolutions = _nj1;
19745 vinfos[2].jointtype = 1;
19746 vinfos[2].foffset = j2;
19747 vinfos[2].indices[0] = _ij2[0];
19748 vinfos[2].indices[1] = _ij2[1];
19749 vinfos[2].maxsolutions = _nj2;
19750 vinfos[3].jointtype = 1;
19751 vinfos[3].foffset = j3;
19752 vinfos[3].indices[0] = _ij3[0];
19753 vinfos[3].indices[1] = _ij3[1];
19754 vinfos[3].maxsolutions = _nj3;
19755 vinfos[4].jointtype = 1;
19756 vinfos[4].foffset = j4;
19757 vinfos[4].indices[0] = _ij4[0];
19758 vinfos[4].indices[1] = _ij4[1];
19759 vinfos[4].maxsolutions = _nj4;
19760 vinfos[5].jointtype = 1;
19761 vinfos[5].foffset = j5;
19762 vinfos[5].indices[0] = _ij5[0];
19763 vinfos[5].indices[1] = _ij5[1];
19764 vinfos[5].maxsolutions = _nj5;
19765 std::vector<int> vfree(0);
19766 solutions.AddSolution(vinfos,vfree);
19767 }
19768 }
19769 }
19770 
19771 }
19772 
19773 }
19774 }
19775 }
19776 
19777 }
19778 
19779 }
19780 }
19781 }
19782 }
19783 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
19784 {
19785  using std::complex;
19786  if( rawcoeffs[0] == 0 ) {
19787  // solve with one reduced degree
19788  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
19789  return;
19790  }
19791  IKFAST_ASSERT(rawcoeffs[0] != 0);
19792  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19793  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19794  complex<IkReal> coeffs[3];
19795  const int maxsteps = 110;
19796  for(int i = 0; i < 3; ++i) {
19797  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19798  }
19799  complex<IkReal> roots[3];
19800  IkReal err[3];
19801  roots[0] = complex<IkReal>(1,0);
19802  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19803  err[0] = 1.0;
19804  err[1] = 1.0;
19805  for(int i = 2; i < 3; ++i) {
19806  roots[i] = roots[i-1]*roots[1];
19807  err[i] = 1.0;
19808  }
19809  for(int step = 0; step < maxsteps; ++step) {
19810  bool changed = false;
19811  for(int i = 0; i < 3; ++i) {
19812  if ( err[i] >= tol ) {
19813  changed = true;
19814  // evaluate
19815  complex<IkReal> x = roots[i] + coeffs[0];
19816  for(int j = 1; j < 3; ++j) {
19817  x = roots[i] * x + coeffs[j];
19818  }
19819  for(int j = 0; j < 3; ++j) {
19820  if( i != j ) {
19821  if( roots[i] != roots[j] ) {
19822  x /= (roots[i] - roots[j]);
19823  }
19824  }
19825  }
19826  roots[i] -= x;
19827  err[i] = abs(x);
19828  }
19829  }
19830  if( !changed ) {
19831  break;
19832  }
19833  }
19834 
19835  numroots = 0;
19836  bool visited[3] = {false};
19837  for(int i = 0; i < 3; ++i) {
19838  if( !visited[i] ) {
19839  // might be a multiple root, in which case it will have more error than the other roots
19840  // find any neighboring roots, and take the average
19841  complex<IkReal> newroot=roots[i];
19842  int n = 1;
19843  for(int j = i+1; j < 3; ++j) {
19844  // care about error in real much more than imaginary
19845  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19846  newroot += roots[j];
19847  n += 1;
19848  visited[j] = true;
19849  }
19850  }
19851  if( n > 1 ) {
19852  newroot /= n;
19853  }
19854  // 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
19855  if( IKabs(imag(newroot)) < tolsqrt ) {
19856  rawroots[numroots++] = real(newroot);
19857  }
19858  }
19859  }
19860 }
19861 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
19862  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19863  if( det < 0 ) {
19864  numroots=0;
19865  }
19866  else if( det == 0 ) {
19867  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19868  numroots = 1;
19869  }
19870  else {
19871  det = IKsqrt(det);
19872  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19873  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
19874  numroots = 2;
19875  }
19876 }
19877 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
19878 {
19879  using std::complex;
19880  if( rawcoeffs[0] == 0 ) {
19881  // solve with one reduced degree
19882  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19883  return;
19884  }
19885  IKFAST_ASSERT(rawcoeffs[0] != 0);
19886  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19887  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19888  complex<IkReal> coeffs[4];
19889  const int maxsteps = 110;
19890  for(int i = 0; i < 4; ++i) {
19891  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19892  }
19893  complex<IkReal> roots[4];
19894  IkReal err[4];
19895  roots[0] = complex<IkReal>(1,0);
19896  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19897  err[0] = 1.0;
19898  err[1] = 1.0;
19899  for(int i = 2; i < 4; ++i) {
19900  roots[i] = roots[i-1]*roots[1];
19901  err[i] = 1.0;
19902  }
19903  for(int step = 0; step < maxsteps; ++step) {
19904  bool changed = false;
19905  for(int i = 0; i < 4; ++i) {
19906  if ( err[i] >= tol ) {
19907  changed = true;
19908  // evaluate
19909  complex<IkReal> x = roots[i] + coeffs[0];
19910  for(int j = 1; j < 4; ++j) {
19911  x = roots[i] * x + coeffs[j];
19912  }
19913  for(int j = 0; j < 4; ++j) {
19914  if( i != j ) {
19915  if( roots[i] != roots[j] ) {
19916  x /= (roots[i] - roots[j]);
19917  }
19918  }
19919  }
19920  roots[i] -= x;
19921  err[i] = abs(x);
19922  }
19923  }
19924  if( !changed ) {
19925  break;
19926  }
19927  }
19928 
19929  numroots = 0;
19930  bool visited[4] = {false};
19931  for(int i = 0; i < 4; ++i) {
19932  if( !visited[i] ) {
19933  // might be a multiple root, in which case it will have more error than the other roots
19934  // find any neighboring roots, and take the average
19935  complex<IkReal> newroot=roots[i];
19936  int n = 1;
19937  for(int j = i+1; j < 4; ++j) {
19938  // care about error in real much more than imaginary
19939  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19940  newroot += roots[j];
19941  n += 1;
19942  visited[j] = true;
19943  }
19944  }
19945  if( n > 1 ) {
19946  newroot /= n;
19947  }
19948  // 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
19949  if( IKabs(imag(newroot)) < tolsqrt ) {
19950  rawroots[numroots++] = real(newroot);
19951  }
19952  }
19953  }
19954 }
19955 };
19956 
19957 
19960 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
19961 IKSolver solver;
19962 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
19963 }
19964 
19965 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
19966 IKSolver solver;
19967 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
19968 }
19969 
19970 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - fanuc_m20ib25 (e4a7231046ec717e8b7dae906d1a8bba)>"; }
19971 
19972 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
19973 
19974 #ifdef IKFAST_NAMESPACE
19975 } // end namespace
19976 #endif
19977 
19978 #ifndef IKFAST_NO_MAIN
19979 #include <stdio.h>
19980 #include <stdlib.h>
19981 #ifdef IKFAST_NAMESPACE
19982 using namespace IKFAST_NAMESPACE;
19983 #endif
19984 int main(int argc, char** argv)
19985 {
19986  if( argc != 12+GetNumFreeParameters()+1 ) {
19987  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
19988  "Returns the ik solutions given the transformation of the end effector specified by\n"
19989  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
19990  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
19991  return 1;
19992  }
19993 
19994  IkSolutionList<IkReal> solutions;
19995  std::vector<IkReal> vfree(GetNumFreeParameters());
19996  IkReal eerot[9],eetrans[3];
19997  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
19998  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
19999  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20000  for(std::size_t i = 0; i < vfree.size(); ++i)
20001  vfree[i] = atof(argv[13+i]);
20002  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
20003 
20004  if( !bSuccess ) {
20005  fprintf(stderr,"Failed to get ik solution\n");
20006  return -1;
20007  }
20008 
20009  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
20010  std::vector<IkReal> solvalues(GetNumJoints());
20011  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
20012  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
20013  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
20014  std::vector<IkReal> vsolfree(sol.GetFree().size());
20015  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
20016  for( std::size_t j = 0; j < solvalues.size(); ++j)
20017  printf("%.15f, ", solvalues[j]);
20018  printf("\n");
20019  }
20020  return 0;
20021 }
20022 
20023 #endif
ikfast
Definition: ikfast.h:45
IKsqrt
float IKsqrt(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:174
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKtan
float IKtan(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:172
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:361
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19972
IKsign
float IKsign(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:225
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:246
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:117
IKlog
float IKlog(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:97
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
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)
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
IKatan2
float IKatan2(float fy, float fx)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:179
GetIkType
IKFAST_API int GetIkType()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:367
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:102
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:362
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: fanuc_m20ib25_manipulator_ikfast_solver.cpp:816
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:299
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKsin
float IKsin(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:168
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19965
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:211
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19970
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19783
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:363
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:107
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:365
IKcos
float IKcos(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:170
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
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)
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:25
IKasin
float IKasin(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:121
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: fanuc_m20ib25_manipulator_ikfast_solver.cpp:376
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: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19877
IKacos
float IKacos(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:154
IKsqr
float IKsqr(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:94
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_ASSERT
#define IKFAST_ASSERT(b)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:49
IKPI
#define IKPI
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:60
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
IKPI_2
#define IKPI_2
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:61
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19960
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IKabs
float IKabs(float f)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:91
main
int main(int argc, char **argv)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19984
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:112
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:176
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
IKfmod
float IKfmod(float x, float y)
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:137
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: fanuc_m20ib25_manipulator_ikfast_solver.cpp:19861
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
x
double x
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
IK2PI
#define IK2PI
Definition: fanuc_m20ib25_manipulator_ikfast_solver.cpp:59

fanuc_m20ib_moveit_plugins
Author(s): Axel Schroth, Simon Schmeisser
autogenerated on Sun Apr 4 2021 02:20:44