prbt_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
25 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
26 using namespace ikfast;
27 
28 // check if the included ikfast version matches what this file was compiled with
29 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
30 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a);
31 
32 #include <cmath>
33 #include <vector>
34 #include <limits>
35 #include <algorithm>
36 #include <complex>
37 
38 #ifndef IKFAST_ASSERT
39 #include <stdexcept>
40 #include <sstream>
41 #include <iostream>
42 
43 #ifdef _MSC_VER
44 #ifndef __PRETTY_FUNCTION__
45 #define __PRETTY_FUNCTION__ __FUNCDNAME__
46 #endif
47 #endif
48 
49 #ifndef __PRETTY_FUNCTION__
50 #define __PRETTY_FUNCTION__ __func__
51 #endif
52 
53 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
54 
55 #endif
56 
57 #if defined(_MSC_VER)
58 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
59 #else
60 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
61 #endif
62 
63 #define IK2PI ((IkReal)6.28318530717959)
64 #define IKPI ((IkReal)3.14159265358979)
65 #define IKPI_2 ((IkReal)1.57079632679490)
66 
67 #ifdef _MSC_VER
68 #ifndef isnan
69 #define isnan _isnan
70 #endif
71 #ifndef isinf
72 #define isinf _isinf
73 #endif
74 //#ifndef isfinite
75 //#define isfinite _isfinite
76 //#endif
77 #endif // _MSC_VER
78 
79 using namespace std; // necessary to get std math routines
80 
81 #ifdef IKFAST_NAMESPACE
82 namespace IKFAST_NAMESPACE {
83 #endif
84 
85 inline float IKabs(float f) { return fabsf(f); }
86 inline double IKabs(double f) { return fabs(f); }
87 
88 inline float IKsqr(float f) { return f*f; }
89 inline double IKsqr(double f) { return f*f; }
90 
91 inline float IKlog(float f) { return logf(f); }
92 inline double IKlog(double f) { return log(f); }
93 
94 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
95 #ifndef IKFAST_SINCOS_THRESH
96 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
97 #endif
98 
99 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
100 #ifndef IKFAST_ATAN2_MAGTHRESH
101 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
102 #endif
103 
104 // minimum distance of separate solutions
105 #ifndef IKFAST_SOLUTION_THRESH
106 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
107 #endif
108 
109 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
110 #ifndef IKFAST_EVALCOND_THRESH
111 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
112 #endif
113 
114 
115 inline float IKasin(float f)
116 {
117 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
118 if( f <= -1 ) return float(-IKPI_2);
119 else if( f >= 1 ) return float(IKPI_2);
120 return asinf(f);
121 }
122 inline double IKasin(double f)
123 {
124 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
125 if( f <= -1 ) return -IKPI_2;
126 else if( f >= 1 ) return IKPI_2;
127 return asin(f);
128 }
129 
130 // return positive value in [0,y)
131 inline float IKfmod(float x, float y)
132 {
133  while(x < 0) {
134  x += y;
135  }
136  return fmodf(x,y);
137 }
138 
139 // return positive value in [0,y)
140 inline double IKfmod(double x, double y)
141 {
142  while(x < 0) {
143  x += y;
144  }
145  return fmod(x,y);
146 }
147 
148 inline float IKacos(float f)
149 {
150 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
151 if( f <= -1 ) return float(IKPI);
152 else if( f >= 1 ) return float(0);
153 return acosf(f);
154 }
155 inline double IKacos(double f)
156 {
157 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
158 if( f <= -1 ) return IKPI;
159 else if( f >= 1 ) return 0;
160 return acos(f);
161 }
162 inline float IKsin(float f) { return sinf(f); }
163 inline double IKsin(double f) { return sin(f); }
164 inline float IKcos(float f) { return cosf(f); }
165 inline double IKcos(double f) { return cos(f); }
166 inline float IKtan(float f) { return tanf(f); }
167 inline double IKtan(double f) { return tan(f); }
168 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
169 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
170 inline float IKatan2Simple(float fy, float fx) {
171  return atan2f(fy,fx);
172 }
173 inline float IKatan2(float fy, float fx) {
174  if( isnan(fy) ) {
175  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
176  return float(IKPI_2);
177  }
178  else if( isnan(fx) ) {
179  return 0;
180  }
181  return atan2f(fy,fx);
182 }
183 inline double IKatan2Simple(double fy, double fx) {
184  return atan2(fy,fx);
185 }
186 inline double IKatan2(double fy, double fx) {
187  if( isnan(fy) ) {
188  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
189  return IKPI_2;
190  }
191  else if( isnan(fx) ) {
192  return 0;
193  }
194  return atan2(fy,fx);
195 }
196 
197 template <typename T>
198 struct CheckValue
199 {
200  T value;
201  bool valid;
202 };
203 
204 template <typename T>
205 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
206 {
207  CheckValue<T> ret;
208  ret.valid = false;
209  ret.value = 0;
210  if( !isnan(fy) && !isnan(fx) ) {
211  if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
212  ret.value = IKatan2Simple(fy,fx);
213  ret.valid = true;
214  }
215  }
216  return ret;
217 }
218 
219 inline float IKsign(float f) {
220  if( f > 0 ) {
221  return float(1);
222  }
223  else if( f < 0 ) {
224  return float(-1);
225  }
226  return 0;
227 }
228 
229 inline double IKsign(double f) {
230  if( f > 0 ) {
231  return 1.0;
232  }
233  else if( f < 0 ) {
234  return -1.0;
235  }
236  return 0;
237 }
238 
239 template <typename T>
240 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
241 {
242  CheckValue<T> ret;
243  ret.valid = true;
244  if( n == 0 ) {
245  ret.value = 1.0;
246  return ret;
247  }
248  else if( n == 1 )
249  {
250  ret.value = f;
251  return ret;
252  }
253  else if( n < 0 )
254  {
255  if( f == 0 )
256  {
257  ret.valid = false;
258  ret.value = (T)1.0e30;
259  return ret;
260  }
261  if( n == -1 ) {
262  ret.value = T(1.0)/f;
263  return ret;
264  }
265  }
266 
267  int num = n > 0 ? n : -n;
268  if( num == 2 ) {
269  ret.value = f*f;
270  }
271  else if( num == 3 ) {
272  ret.value = f*f*f;
273  }
274  else {
275  ret.value = 1.0;
276  while(num>0) {
277  if( num & 1 ) {
278  ret.value *= f;
279  }
280  num >>= 1;
281  f *= f;
282  }
283  }
284 
285  if( n < 0 ) {
286  ret.value = T(1.0)/ret.value;
287  }
288  return ret;
289 }
290 
293 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
294 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46;
295 x0=IKcos(j[0]);
296 x1=IKcos(j[1]);
297 x2=IKcos(j[2]);
298 x3=IKsin(j[1]);
299 x4=IKsin(j[2]);
300 x5=IKsin(j[3]);
301 x6=IKcos(j[3]);
302 x7=IKsin(j[0]);
303 x8=IKcos(j[5]);
304 x9=IKsin(j[5]);
305 x10=IKsin(j[4]);
306 x11=IKcos(j[4]);
307 x12=((1.0)*x7);
308 x13=((1.0)*x6);
309 x14=((0.307)*x7);
310 x15=((0.084)*x3);
311 x16=((0.084)*x7);
312 x17=((1.0)*x0);
313 x18=((1.0)*x10);
314 x19=((0.35)*x3);
315 x20=((0.084)*x6);
316 x21=((0.307)*x0);
317 x22=((0.084)*x0);
318 x23=(x1*x2);
319 x24=(x1*x4);
320 x25=((-1.0)*x6);
321 x26=(x2*x3);
322 x27=(x5*x7);
323 x28=(x3*x4);
324 x29=(x10*x6);
325 x30=(x17*x5);
326 x31=((1.0)*x26);
327 x32=((((-1.0)*x31))+x24);
328 x33=((((-1.0)*x24))+x31);
329 x34=((((1.0)*x23))+(((1.0)*x28)));
330 x35=(x11*x32);
331 x36=(x33*x5);
332 x37=(x17*(((((-1.0)*x24))+x26)));
333 x38=(x12*(((((-1.0)*x24))+x26)));
334 x39=(x17*(((((-1.0)*x23))+(((-1.0)*x28)))));
335 x40=(x12*(((((-1.0)*x23))+(((-1.0)*x28)))));
336 x41=(x10*x38);
337 x42=(x39*x5);
338 x43=(x39*x6);
339 x44=(((x0*x6))+((x40*x5)));
340 x45=(x30+((x25*x40)));
341 x46=(x11*x45);
342 eerot[0]=(((x9*(((((-1.0)*x12*x6))+x42))))+((x8*((((x10*x37))+((x11*(((((-1.0)*x27))+((x25*x39)))))))))));
343 eerot[1]=((((-1.0)*x9*((((x18*x37))+(((1.0)*x11*(((((-1.0)*x12*x5))+(((-1.0)*x13*x39))))))))))+((x8*((x42+((x25*x7)))))));
344 eerot[2]=(((x10*((x43+x27))))+((x11*x37)));
345 IkReal x47=((1.0)*x24);
346 eetrans[0]=(((x21*x26))+((x0*x19))+((x11*(((((-1.0)*x22*x47))+((x0*x15*x2))))))+(((-1.0)*x21*x47))+((x10*((((x20*x39))+((x16*x5)))))));
347 eerot[3]=(((x44*x9))+((x8*((x46+x41)))));
348 eerot[4]=(((x9*(((((-1.0)*x41))+(((-1.0)*x46))))))+((x44*x8)));
349 eerot[5]=(((x10*(((((-1.0)*x30))+((x40*x6))))))+((x11*x38)));
350 IkReal x48=((1.0)*x24);
351 eetrans[1]=(((x19*x7))+((x14*x26))+((x10*((((x20*x40))+(((-1.0)*x22*x5))))))+((x11*(((((-1.0)*x16*x48))+((x15*x2*x7))))))+(((-1.0)*x14*x48)));
352 eerot[6]=(((x8*((((x10*x34))+((x35*x6))))))+((x36*x9)));
353 eerot[7]=(((x9*(((((-1.0)*x18*x34))+(((-1.0)*x13*x35))))))+((x36*x8)));
354 eerot[8]=(((x29*x33))+((x11*x34)));
355 eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11*(((((0.084)*x23))+((x15*x4))))))+(((0.307)*x23))+(((0.307)*x28)));
356 }
357 
358 IKFAST_API int GetNumFreeParameters() { return 0; }
359 IKFAST_API int* GetFreeParameters() { return NULL; }
360 IKFAST_API int GetNumJoints() { return 6; }
361 
362 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
363 
364 IKFAST_API int GetIkType() { return 0x67000001; }
365 
366 class IKSolver {
367 public:
368 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;
369 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
370 
371 IkReal j100, cj100, sj100;
372 unsigned char _ij100[2], _nj100;
373 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
374 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;
375 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
376  solutions.Clear();
377 r00 = eerot[0*3+0];
378 r01 = eerot[0*3+1];
379 r02 = eerot[0*3+2];
380 r10 = eerot[1*3+0];
381 r11 = eerot[1*3+1];
382 r12 = eerot[1*3+2];
383 r20 = eerot[2*3+0];
384 r21 = eerot[2*3+1];
385 r22 = eerot[2*3+2];
386 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
387 
388 new_r00=r00;
389 new_r01=r01;
390 new_r02=r02;
391 new_px=(px+(((-0.084)*r02)));
392 new_r10=r10;
393 new_r11=r11;
394 new_r12=r12;
395 new_py=(py+(((-0.084)*r12)));
396 new_r20=r20;
397 new_r21=r21;
398 new_r22=r22;
399 new_pz=((-0.2604)+pz+(((-0.084)*r22)));
400 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;
401 IkReal x49=((1.0)*px);
402 IkReal x50=((1.0)*pz);
403 IkReal x51=((1.0)*py);
404 pp=((px*px)+(py*py)+(pz*pz));
405 npx=(((px*r00))+((py*r10))+((pz*r20)));
406 npy=(((px*r01))+((py*r11))+((pz*r21)));
407 npz=(((px*r02))+((py*r12))+((pz*r22)));
408 rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
409 rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
410 rxp0_2=((((-1.0)*r10*x49))+((py*r00)));
411 rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
412 rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
413 rxp1_2=((((-1.0)*r11*x49))+((py*r01)));
414 rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
415 rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
416 rxp2_2=((((-1.0)*r12*x49))+((py*r02)));
417 {
418 IkReal j2array[2], cj2array[2], sj2array[2];
419 bool j2valid[2]={false};
420 _nj2 = 2;
421 cj2array[0]=((-1.00860400186133)+(((4.65332712889716)*pp)));
422 if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
423 {
424  j2valid[0] = j2valid[1] = true;
425  j2array[0] = IKacos(cj2array[0]);
426  sj2array[0] = IKsin(j2array[0]);
427  cj2array[1] = cj2array[0];
428  j2array[1] = -j2array[0];
429  sj2array[1] = -sj2array[0];
430 }
431 else if( isnan(cj2array[0]) )
432 {
433  // probably any value will work
434  j2valid[0] = true;
435  cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
436 }
437 for(int ij2 = 0; ij2 < 2; ++ij2)
438 {
439 if( !j2valid[ij2] )
440 {
441  continue;
442 }
443 _ij2[0] = ij2; _ij2[1] = -1;
444 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
445 {
446 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
447 {
448  j2valid[iij2]=false; _ij2[1] = iij2; break;
449 }
450 }
451 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
452 
453 {
454 IkReal j0eval[1];
455 j0eval[0]=((IKabs(px))+(IKabs(py)));
456 if( IKabs(j0eval[0]) < 0.0000010000000000 )
457 {
458 {
459 IkReal j1eval[2];
460 j1eval[0]=((IKabs(sj2))+(((3.25732899022801)*(IKabs(((0.35)+(((0.307)*cj2))))))));
461 j1eval[1]=((1.29974853844603)+(sj2*sj2)+(cj2*cj2)+(((2.28013029315961)*cj2)));
462 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
463 {
464 continue; // no branches [j0, j1]
465 
466 } else
467 {
468 {
469 IkReal j1array[2], cj1array[2], sj1array[2];
470 bool j1valid[2]={false};
471 _nj1 = 2;
472 IkReal x52=((0.35)+(((0.307)*cj2)));
473 CheckValue<IkReal> x55 = IKatan2WithCheck(IkReal(x52),IkReal(((0.307)*sj2)),IKFAST_ATAN2_MAGTHRESH);
474 if(!x55.valid){
475 continue;
476 }
477 IkReal x53=((1.0)*(x55.value));
478 if((((((0.094249)*(sj2*sj2)))+(x52*x52))) < -0.00001)
479 continue;
480 CheckValue<IkReal> x56=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.094249)*(sj2*sj2)))+(x52*x52)))),-1);
481 if(!x56.valid){
482 continue;
483 }
484 if( ((pz*(x56.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x56.value))) > 1+IKFAST_SINCOS_THRESH )
485  continue;
486 IkReal x54=IKasin((pz*(x56.value)));
487 j1array[0]=(x54+(((-1.0)*x53)));
488 sj1array[0]=IKsin(j1array[0]);
489 cj1array[0]=IKcos(j1array[0]);
490 j1array[1]=((3.14159265358979)+(((-1.0)*x53))+(((-1.0)*x54)));
491 sj1array[1]=IKsin(j1array[1]);
492 cj1array[1]=IKcos(j1array[1]);
493 if( j1array[0] > IKPI )
494 {
495  j1array[0]-=IK2PI;
496 }
497 else if( j1array[0] < -IKPI )
498 { j1array[0]+=IK2PI;
499 }
500 j1valid[0] = true;
501 if( j1array[1] > IKPI )
502 {
503  j1array[1]-=IK2PI;
504 }
505 else if( j1array[1] < -IKPI )
506 { j1array[1]+=IK2PI;
507 }
508 j1valid[1] = true;
509 for(int ij1 = 0; ij1 < 2; ++ij1)
510 {
511 if( !j1valid[ij1] )
512 {
513  continue;
514 }
515 _ij1[0] = ij1; _ij1[1] = -1;
516 for(int iij1 = ij1+1; iij1 < 2; ++iij1)
517 {
518 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
519 {
520  j1valid[iij1]=false; _ij1[1] = iij1; break;
521 }
522 }
523 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
524 
525 // The remainder of this scope is removed as it does not return a solution.
526 // This is the branch (IKabs(px))+(IKabs(py)), from the kinematics we know,
527 // that j0 is free. So we simply choose 4 values and pass directly to the
528 // rotationfunction (orientation computation is unchanged).
529 _nj0=4;
530 for(int ij0=1; ij0 < _nj0; ++ij0)
531 {
532 j0=(IkReal)(ij0 - 1) * (3.14159265358979) / 2.; // -pi/2, 0, pi/2, pi
533 cj0=IKcos(j0);
534 sj0=IKsin(j0);
535 _ij0[0] = ij0;
536 rotationfunction0(solutions);
537 }
538 
539 }
540 }
541 
542 }
543 
544 }
545 
546 } else
547 {
548 {
549 IkReal j0array[2], cj0array[2], sj0array[2];
550 bool j0valid[2]={false};
551 _nj0 = 2;
552 CheckValue<IkReal> x1691 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
553 if(!x1691.valid){
554 continue;
555 }
556 IkReal x1690=x1691.value;
557 j0array[0]=((-1.0)*x1690);
558 sj0array[0]=IKsin(j0array[0]);
559 cj0array[0]=IKcos(j0array[0]);
560 j0array[1]=((3.14159265358979)+(((-1.0)*x1690)));
561 sj0array[1]=IKsin(j0array[1]);
562 cj0array[1]=IKcos(j0array[1]);
563 if( j0array[0] > IKPI )
564 {
565  j0array[0]-=IK2PI;
566 }
567 else if( j0array[0] < -IKPI )
568 { j0array[0]+=IK2PI;
569 }
570 j0valid[0] = true;
571 if( j0array[1] > IKPI )
572 {
573  j0array[1]-=IK2PI;
574 }
575 else if( j0array[1] < -IKPI )
576 { j0array[1]+=IK2PI;
577 }
578 j0valid[1] = true;
579 for(int ij0 = 0; ij0 < 2; ++ij0)
580 {
581 if( !j0valid[ij0] )
582 {
583  continue;
584 }
585 _ij0[0] = ij0; _ij0[1] = -1;
586 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
587 {
588 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
589 {
590  j0valid[iij0]=false; _ij0[1] = iij0; break;
591 }
592 }
593 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
594 
595 {
596 IkReal j1eval[3];
597 IkReal x1692=((307000.0)*cj2);
598 IkReal x1693=(cj0*px);
599 IkReal x1694=((307000.0)*sj2);
600 IkReal x1695=(py*sj0);
601 j1eval[0]=((-1.00860400186133)+(((-1.0)*cj2)));
602 j1eval[1]=((IKabs(((((-1.0)*pz*x1692))+(((-350000.0)*pz))+((x1693*x1694))+((x1694*x1695)))))+(IKabs(((((-1.0)*x1692*x1693))+(((-1.0)*x1692*x1695))+(((-1.0)*pz*x1694))+(((-350000.0)*x1693))+(((-350000.0)*x1695))))));
603 j1eval[2]=IKsign(((-216749.0)+(((-214900.0)*cj2))));
604 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
605 {
606 {
607 IkReal j1eval[3];
608 IkReal x1696=(py*sj0);
609 IkReal x1697=((1000.0)*pz);
610 IkReal x1698=(pz*sj2);
611 IkReal x1699=(cj0*px);
612 IkReal x1700=(cj2*x1699);
613 j1eval[0]=((((-1.0)*x1700))+x1698+(((-1.0)*cj2*x1696))+(((-1.1400651465798)*x1696))+(((-1.1400651465798)*x1699)));
614 j1eval[1]=((IKabs(((((-1.0)*x1696*x1697))+(((-1.0)*x1697*x1699))+(((94.249)*cj2*sj2))+(((107.45)*sj2)))))+(IKabs(((-122.5)+(((-94.249)*(cj2*cj2)))+(((-214.9)*cj2))+((pz*x1697))))));
615 j1eval[2]=IKsign(((((-307.0)*x1700))+(((-350.0)*x1699))+(((-350.0)*x1696))+(((-307.0)*cj2*x1696))+(((307.0)*x1698))));
616 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
617 {
618 {
619 IkReal j1eval[3];
620 IkReal x1701=(cj0*px);
621 IkReal x1702=((1.0)*cj2);
622 IkReal x1703=((7000.0)*pz);
623 IkReal x1704=(py*sj0);
624 IkReal x1705=((2149.0)*cj2);
625 IkReal x1706=(pz*sj2);
626 IkReal x1707=((3070.0)*pp);
627 j1eval[0]=((((-1.1400651465798)*x1704))+(((-1.1400651465798)*x1701))+(((-1.0)*x1702*x1704))+x1706+(((-1.0)*x1701*x1702)));
628 j1eval[1]=((IKabs(((-98.8785)+(((-86.73057)*cj2))+(((-1.0)*cj2*x1707))+(((-3500.0)*pp))+((pz*x1703)))))+(IKabs(((((-1.0)*x1703*x1704))+((sj2*x1707))+(((-1.0)*x1701*x1703))+(((86.73057)*sj2))))));
629 j1eval[2]=IKsign(((((2149.0)*x1706))+(((-2450.0)*x1704))+(((-2450.0)*x1701))+(((-1.0)*x1704*x1705))+(((-1.0)*x1701*x1705))));
630 if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
631 {
632 continue; // no branches [j1]
633 
634 } else
635 {
636 {
637 IkReal j1array[1], cj1array[1], sj1array[1];
638 bool j1valid[1]={false};
639 _nj1 = 1;
640 IkReal x1708=((7000.0)*pz);
641 IkReal x1709=(cj0*px);
642 IkReal x1710=(py*sj0);
643 IkReal x1711=((2149.0)*cj2);
644 IkReal x1712=((3070.0)*pp);
645 CheckValue<IkReal> x1713 = IKatan2WithCheck(IkReal(((-98.8785)+(((-86.73057)*cj2))+(((-3500.0)*pp))+(((-1.0)*cj2*x1712))+((pz*x1708)))),IkReal(((((-1.0)*x1708*x1709))+(((-1.0)*x1708*x1710))+((sj2*x1712))+(((86.73057)*sj2)))),IKFAST_ATAN2_MAGTHRESH);
646 if(!x1713.valid){
647 continue;
648 }
649 CheckValue<IkReal> x1714=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1710*x1711))+(((-1.0)*x1709*x1711))+(((-2450.0)*x1709))+(((-2450.0)*x1710))+(((2149.0)*pz*sj2)))),-1);
650 if(!x1714.valid){
651 continue;
652 }
653 j1array[0]=((-1.5707963267949)+(x1713.value)+(((1.5707963267949)*(x1714.value))));
654 sj1array[0]=IKsin(j1array[0]);
655 cj1array[0]=IKcos(j1array[0]);
656 if( j1array[0] > IKPI )
657 {
658  j1array[0]-=IK2PI;
659 }
660 else if( j1array[0] < -IKPI )
661 { j1array[0]+=IK2PI;
662 }
663 j1valid[0] = true;
664 for(int ij1 = 0; ij1 < 1; ++ij1)
665 {
666 if( !j1valid[ij1] )
667 {
668  continue;
669 }
670 _ij1[0] = ij1; _ij1[1] = -1;
671 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
672 {
673 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
674 {
675  j1valid[iij1]=false; _ij1[1] = iij1; break;
676 }
677 }
678 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
679 {
680 IkReal evalcond[5];
681 IkReal x1715=IKcos(j1);
682 IkReal x1716=IKsin(j1);
683 IkReal x1717=(cj0*px);
684 IkReal x1718=(py*sj0);
685 IkReal x1719=((0.307)*x1715);
686 IkReal x1720=((1.0)*x1718);
687 IkReal x1721=(pz*x1715);
688 IkReal x1722=((0.7)*x1716);
689 IkReal x1723=((0.307)*x1716);
690 evalcond[0]=((((-1.0)*pz))+((cj2*x1719))+(((0.35)*x1715))+((sj2*x1723)));
691 evalcond[1]=((((-1.0)*x1715*x1720))+(((-1.0)*x1715*x1717))+(((-0.307)*sj2))+((pz*x1716)));
692 evalcond[2]=((((-0.35)*x1716))+x1717+x1718+(((-1.0)*cj2*x1723))+((sj2*x1719)));
693 evalcond[3]=((-0.028251)+((x1718*x1722))+((x1717*x1722))+(((-1.0)*pp))+(((0.7)*x1721)));
694 evalcond[4]=((0.35)+(((-1.0)*x1716*x1717))+(((-1.0)*x1721))+(((-1.0)*x1716*x1720))+(((0.307)*cj2)));
695 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
696 {
697 continue;
698 }
699 }
700 
701 rotationfunction0(solutions);
702 }
703 }
704 
705 }
706 
707 }
708 
709 } else
710 {
711 {
712 IkReal j1array[1], cj1array[1], sj1array[1];
713 bool j1valid[1]={false};
714 _nj1 = 1;
715 IkReal x1724=((307.0)*cj2);
716 IkReal x1725=(cj0*px);
717 IkReal x1726=(py*sj0);
718 IkReal x1727=((1000.0)*pz);
719 CheckValue<IkReal> x1728 = IKatan2WithCheck(IkReal(((-122.5)+(((-94.249)*(cj2*cj2)))+((pz*x1727))+(((-214.9)*cj2)))),IkReal(((((-1.0)*x1726*x1727))+(((94.249)*cj2*sj2))+(((107.45)*sj2))+(((-1.0)*x1725*x1727)))),IKFAST_ATAN2_MAGTHRESH);
720 if(!x1728.valid){
721 continue;
722 }
723 CheckValue<IkReal> x1729=IKPowWithIntegerCheck(IKsign(((((-350.0)*x1726))+(((-350.0)*x1725))+(((307.0)*pz*sj2))+(((-1.0)*x1724*x1725))+(((-1.0)*x1724*x1726)))),-1);
724 if(!x1729.valid){
725 continue;
726 }
727 j1array[0]=((-1.5707963267949)+(x1728.value)+(((1.5707963267949)*(x1729.value))));
728 sj1array[0]=IKsin(j1array[0]);
729 cj1array[0]=IKcos(j1array[0]);
730 if( j1array[0] > IKPI )
731 {
732  j1array[0]-=IK2PI;
733 }
734 else if( j1array[0] < -IKPI )
735 { j1array[0]+=IK2PI;
736 }
737 j1valid[0] = true;
738 for(int ij1 = 0; ij1 < 1; ++ij1)
739 {
740 if( !j1valid[ij1] )
741 {
742  continue;
743 }
744 _ij1[0] = ij1; _ij1[1] = -1;
745 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
746 {
747 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
748 {
749  j1valid[iij1]=false; _ij1[1] = iij1; break;
750 }
751 }
752 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
753 {
754 IkReal evalcond[5];
755 IkReal x1730=IKcos(j1);
756 IkReal x1731=IKsin(j1);
757 IkReal x1732=(cj0*px);
758 IkReal x1733=(py*sj0);
759 IkReal x1734=((0.307)*x1730);
760 IkReal x1735=((1.0)*x1733);
761 IkReal x1736=(pz*x1730);
762 IkReal x1737=((0.7)*x1731);
763 IkReal x1738=((0.307)*x1731);
764 evalcond[0]=(((cj2*x1734))+(((0.35)*x1730))+(((-1.0)*pz))+((sj2*x1738)));
765 evalcond[1]=(((pz*x1731))+(((-0.307)*sj2))+(((-1.0)*x1730*x1735))+(((-1.0)*x1730*x1732)));
766 evalcond[2]=(x1733+x1732+(((-1.0)*cj2*x1738))+(((-0.35)*x1731))+((sj2*x1734)));
767 evalcond[3]=((-0.028251)+(((-1.0)*pp))+(((0.7)*x1736))+((x1732*x1737))+((x1733*x1737)));
768 evalcond[4]=((0.35)+(((-1.0)*x1731*x1735))+(((-1.0)*x1731*x1732))+(((-1.0)*x1736))+(((0.307)*cj2)));
769 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
770 {
771 continue;
772 }
773 }
774 
775 rotationfunction0(solutions);
776 }
777 }
778 
779 }
780 
781 }
782 
783 } else
784 {
785 {
786 IkReal j1array[1], cj1array[1], sj1array[1];
787 bool j1valid[1]={false};
788 _nj1 = 1;
789 IkReal x1739=((307000.0)*cj2);
790 IkReal x1740=(cj0*px);
791 IkReal x1741=((307000.0)*sj2);
792 IkReal x1742=(py*sj0);
793 CheckValue<IkReal> x1743=IKPowWithIntegerCheck(IKsign(((-216749.0)+(((-214900.0)*cj2)))),-1);
794 if(!x1743.valid){
795 continue;
796 }
797 CheckValue<IkReal> x1744 = IKatan2WithCheck(IkReal(((((-350000.0)*x1742))+(((-350000.0)*x1740))+(((-1.0)*x1739*x1740))+(((-1.0)*x1739*x1742))+(((-1.0)*pz*x1741)))),IkReal(((((-1.0)*pz*x1739))+(((-350000.0)*pz))+((x1740*x1741))+((x1741*x1742)))),IKFAST_ATAN2_MAGTHRESH);
798 if(!x1744.valid){
799 continue;
800 }
801 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1743.value)))+(x1744.value));
802 sj1array[0]=IKsin(j1array[0]);
803 cj1array[0]=IKcos(j1array[0]);
804 if( j1array[0] > IKPI )
805 {
806  j1array[0]-=IK2PI;
807 }
808 else if( j1array[0] < -IKPI )
809 { j1array[0]+=IK2PI;
810 }
811 j1valid[0] = true;
812 for(int ij1 = 0; ij1 < 1; ++ij1)
813 {
814 if( !j1valid[ij1] )
815 {
816  continue;
817 }
818 _ij1[0] = ij1; _ij1[1] = -1;
819 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
820 {
821 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
822 {
823  j1valid[iij1]=false; _ij1[1] = iij1; break;
824 }
825 }
826 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
827 {
828 IkReal evalcond[5];
829 IkReal x1745=IKcos(j1);
830 IkReal x1746=IKsin(j1);
831 IkReal x1747=(cj0*px);
832 IkReal x1748=(py*sj0);
833 IkReal x1749=((0.307)*x1745);
834 IkReal x1750=((1.0)*x1748);
835 IkReal x1751=(pz*x1745);
836 IkReal x1752=((0.7)*x1746);
837 IkReal x1753=((0.307)*x1746);
838 evalcond[0]=((((0.35)*x1745))+((cj2*x1749))+(((-1.0)*pz))+((sj2*x1753)));
839 evalcond[1]=(((pz*x1746))+(((-0.307)*sj2))+(((-1.0)*x1745*x1750))+(((-1.0)*x1745*x1747)));
840 evalcond[2]=(x1748+x1747+(((-1.0)*cj2*x1753))+((sj2*x1749))+(((-0.35)*x1746)));
841 evalcond[3]=((-0.028251)+((x1747*x1752))+(((-1.0)*pp))+((x1748*x1752))+(((0.7)*x1751)));
842 evalcond[4]=((0.35)+(((-1.0)*x1746*x1747))+(((-1.0)*x1746*x1750))+(((-1.0)*x1751))+(((0.307)*cj2)));
843 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
844 {
845 continue;
846 }
847 }
848 
849 rotationfunction0(solutions);
850 }
851 }
852 
853 }
854 
855 }
856 }
857 }
858 
859 }
860 
861 }
862 }
863 }
864 }
865 return solutions.GetNumSolutions()>0;
866 }
867 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
868 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
869 IkReal x70=(r11*sj0);
870 IkReal x71=(cj0*r02);
871 IkReal x72=(r10*sj0);
872 IkReal x73=((1.0)*sj0);
873 IkReal x74=(cj2*sj1);
874 IkReal x75=(cj1*sj2);
875 IkReal x76=(r12*sj0);
876 IkReal x77=(((sj1*sj2))+((cj1*cj2)));
877 IkReal x78=(x75+(((-1.0)*x74)));
878 IkReal x79=(x74+(((-1.0)*x75)));
879 IkReal x80=(sj0*x79);
880 IkReal x81=(cj0*x79);
881 IkReal x82=(cj0*x77);
882 new_r00=(((r00*x82))+((x72*x77))+((r20*x78)));
883 new_r01=(((r21*x78))+((r01*x82))+((x70*x77)));
884 new_r02=(((r22*x78))+((x76*x77))+((x71*x77)));
885 new_r10=((((-1.0)*r00*x73))+((cj0*r10)));
886 new_r11=((((-1.0)*r01*x73))+((cj0*r11)));
887 new_r12=((((-1.0)*r02*x73))+((cj0*r12)));
888 new_r20=(((r00*x81))+((x72*x79))+((r20*x77)));
889 new_r21=(((r21*x77))+((r01*x81))+((x70*x79)));
890 new_r22=(((r22*x77))+((x76*x79))+((x71*x79)));
891 {
892 IkReal j4array[2], cj4array[2], sj4array[2];
893 bool j4valid[2]={false};
894 _nj4 = 2;
895 cj4array[0]=new_r22;
896 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
897 {
898  j4valid[0] = j4valid[1] = true;
899  j4array[0] = IKacos(cj4array[0]);
900  sj4array[0] = IKsin(j4array[0]);
901  cj4array[1] = cj4array[0];
902  j4array[1] = -j4array[0];
903  sj4array[1] = -sj4array[0];
904 }
905 else if( isnan(cj4array[0]) )
906 {
907  // probably any value will work
908  j4valid[0] = true;
909  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
910 }
911 for(int ij4 = 0; ij4 < 2; ++ij4)
912 {
913 if( !j4valid[ij4] )
914 {
915  continue;
916 }
917 _ij4[0] = ij4; _ij4[1] = -1;
918 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
919 {
920 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
921 {
922  j4valid[iij4]=false; _ij4[1] = iij4; break;
923 }
924 }
925 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
926 
927 {
928 IkReal j5eval[3];
929 j5eval[0]=sj4;
930 j5eval[1]=IKsign(sj4);
931 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
932 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
933 {
934 {
935 IkReal j3eval[3];
936 j3eval[0]=sj4;
937 j3eval[1]=IKsign(sj4);
938 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
939 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
940 {
941 {
942 IkReal j3eval[2];
943 j3eval[0]=new_r12;
944 j3eval[1]=sj4;
945 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
946 {
947 {
948 IkReal evalcond[5];
949 bool bgotonextstatement = true;
950 do
951 {
952 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
953 evalcond[1]=new_r02;
954 evalcond[2]=new_r12;
955 evalcond[3]=new_r21;
956 evalcond[4]=new_r20;
957 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 )
958 {
959 bgotonextstatement=false;
960 IkReal j5mul = 1;
961 j5=0;
962 j3mul=-1.0;
963 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 )
964  continue;
965 j3=IKatan2(((-1.0)*new_r01), new_r00);
966 {
967 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
968 vinfos[0].jointtype = 1;
969 vinfos[0].foffset = j0;
970 vinfos[0].indices[0] = _ij0[0];
971 vinfos[0].indices[1] = _ij0[1];
972 vinfos[0].maxsolutions = _nj0;
973 vinfos[1].jointtype = 1;
974 vinfos[1].foffset = j1;
975 vinfos[1].indices[0] = _ij1[0];
976 vinfos[1].indices[1] = _ij1[1];
977 vinfos[1].maxsolutions = _nj1;
978 vinfos[2].jointtype = 1;
979 vinfos[2].foffset = j2;
980 vinfos[2].indices[0] = _ij2[0];
981 vinfos[2].indices[1] = _ij2[1];
982 vinfos[2].maxsolutions = _nj2;
983 vinfos[3].jointtype = 1;
984 vinfos[3].foffset = j3;
985 vinfos[3].fmul = j3mul;
986 vinfos[3].freeind = 0;
987 vinfos[3].maxsolutions = 0;
988 vinfos[4].jointtype = 1;
989 vinfos[4].foffset = j4;
990 vinfos[4].indices[0] = _ij4[0];
991 vinfos[4].indices[1] = _ij4[1];
992 vinfos[4].maxsolutions = _nj4;
993 vinfos[5].jointtype = 1;
994 vinfos[5].foffset = j5;
995 vinfos[5].fmul = j5mul;
996 vinfos[5].freeind = 0;
997 vinfos[5].maxsolutions = 0;
998 std::vector<int> vfree(1);
999 vfree[0] = 5;
1000 solutions.AddSolution(vinfos,vfree);
1001 }
1002 
1003 }
1004 } while(0);
1005 if( bgotonextstatement )
1006 {
1007 bool bgotonextstatement = true;
1008 do
1009 {
1010 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
1011 evalcond[1]=new_r02;
1012 evalcond[2]=new_r12;
1013 evalcond[3]=new_r21;
1014 evalcond[4]=new_r20;
1015 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 )
1016 {
1017 bgotonextstatement=false;
1018 IkReal j5mul = 1;
1019 j5=0;
1020 j3mul=1.0;
1021 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 )
1022  continue;
1023 j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
1024 {
1025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1026 vinfos[0].jointtype = 1;
1027 vinfos[0].foffset = j0;
1028 vinfos[0].indices[0] = _ij0[0];
1029 vinfos[0].indices[1] = _ij0[1];
1030 vinfos[0].maxsolutions = _nj0;
1031 vinfos[1].jointtype = 1;
1032 vinfos[1].foffset = j1;
1033 vinfos[1].indices[0] = _ij1[0];
1034 vinfos[1].indices[1] = _ij1[1];
1035 vinfos[1].maxsolutions = _nj1;
1036 vinfos[2].jointtype = 1;
1037 vinfos[2].foffset = j2;
1038 vinfos[2].indices[0] = _ij2[0];
1039 vinfos[2].indices[1] = _ij2[1];
1040 vinfos[2].maxsolutions = _nj2;
1041 vinfos[3].jointtype = 1;
1042 vinfos[3].foffset = j3;
1043 vinfos[3].fmul = j3mul;
1044 vinfos[3].freeind = 0;
1045 vinfos[3].maxsolutions = 0;
1046 vinfos[4].jointtype = 1;
1047 vinfos[4].foffset = j4;
1048 vinfos[4].indices[0] = _ij4[0];
1049 vinfos[4].indices[1] = _ij4[1];
1050 vinfos[4].maxsolutions = _nj4;
1051 vinfos[5].jointtype = 1;
1052 vinfos[5].foffset = j5;
1053 vinfos[5].fmul = j5mul;
1054 vinfos[5].freeind = 0;
1055 vinfos[5].maxsolutions = 0;
1056 std::vector<int> vfree(1);
1057 vfree[0] = 5;
1058 solutions.AddSolution(vinfos,vfree);
1059 }
1060 
1061 }
1062 } while(0);
1063 if( bgotonextstatement )
1064 {
1065 bool bgotonextstatement = true;
1066 do
1067 {
1068 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1069 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1070 {
1071 bgotonextstatement=false;
1072 {
1073 IkReal j3eval[1];
1074 new_r21=0;
1075 new_r20=0;
1076 new_r02=0;
1077 new_r12=0;
1078 IkReal x83=new_r22*new_r22;
1079 IkReal x84=((16.0)*new_r10);
1080 IkReal x85=((16.0)*new_r01);
1081 IkReal x86=((16.0)*new_r22);
1082 IkReal x87=((8.0)*new_r11);
1083 IkReal x88=((8.0)*new_r00);
1084 IkReal x89=(x83*x84);
1085 IkReal x90=(x83*x85);
1086 j3eval[0]=((IKabs((x89+(((-1.0)*x84)))))+(IKabs((((new_r22*x88))+(((-1.0)*x83*x87)))))+(IKabs(((((16.0)*new_r00))+((new_r11*x86))+(((-32.0)*new_r00*x83)))))+(IKabs((x85+(((-1.0)*x90)))))+(IKabs((((new_r22*x87))+(((-1.0)*x88)))))+(IKabs(((((16.0)*new_r11*x83))+((new_r00*x86))+(((-32.0)*new_r11)))))+(IKabs((x90+(((-1.0)*x85)))))+(IKabs((x84+(((-1.0)*x89))))));
1087 if( IKabs(j3eval[0]) < 0.0000000100000000 )
1088 {
1089 continue; // no branches [j3, j5]
1090 
1091 } else
1092 {
1093 IkReal op[4+1], zeror[4];
1094 int numroots;
1095 IkReal j3evalpoly[1];
1096 IkReal x91=new_r22*new_r22;
1097 IkReal x92=((16.0)*new_r10);
1098 IkReal x93=(new_r11*new_r22);
1099 IkReal x94=(x91*x92);
1100 IkReal x95=((((8.0)*x93))+(((-8.0)*new_r00)));
1101 op[0]=x95;
1102 op[1]=(x92+(((-1.0)*x94)));
1103 op[2]=((((16.0)*x93))+(((16.0)*new_r00))+(((-32.0)*new_r00*x91)));
1104 op[3]=(x94+(((-1.0)*x92)));
1105 op[4]=x95;
1106 polyroots4(op,zeror,numroots);
1107 IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1108 int numsolutions = 0;
1109 for(int ij3 = 0; ij3 < numroots; ++ij3)
1110 {
1111 IkReal htj3 = zeror[ij3];
1112 tempj3array[0]=((2.0)*(atan(htj3)));
1113 for(int kj3 = 0; kj3 < 1; ++kj3)
1114 {
1115 j3array[numsolutions] = tempj3array[kj3];
1116 if( j3array[numsolutions] > IKPI )
1117 {
1118  j3array[numsolutions]-=IK2PI;
1119 }
1120 else if( j3array[numsolutions] < -IKPI )
1121 {
1122  j3array[numsolutions]+=IK2PI;
1123 }
1124 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1125 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1126 numsolutions++;
1127 }
1128 }
1129 bool j3valid[4]={true,true,true,true};
1130 _nj3 = 4;
1131 for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1132  {
1133 if( !j3valid[ij3] )
1134 {
1135  continue;
1136 }
1137  j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1138 htj3 = IKtan(j3/2);
1139 
1140 IkReal x96=((16.0)*new_r01);
1141 IkReal x97=new_r22*new_r22;
1142 IkReal x98=(new_r00*new_r22);
1143 IkReal x99=((8.0)*x98);
1144 IkReal x100=(new_r11*x97);
1145 IkReal x101=(x96*x97);
1146 IkReal x102=((8.0)*x100);
1147 j3evalpoly[0]=((((-1.0)*x102))+(((htj3*htj3*htj3)*(((((-1.0)*x101))+x96))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x102))+x99))))+((htj3*((x101+(((-1.0)*x96))))))+(((htj3*htj3)*(((((16.0)*x98))+(((16.0)*x100))+(((-32.0)*new_r11))))))+x99);
1148 if( IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1149 {
1150  continue;
1151 }
1152 _ij3[0] = ij3; _ij3[1] = -1;
1153 for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1154 {
1155 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1156 {
1157  j3valid[iij3]=false; _ij3[1] = iij3; break;
1158 }
1159 }
1160 {
1161 IkReal j5eval[3];
1162 new_r21=0;
1163 new_r20=0;
1164 new_r02=0;
1165 new_r12=0;
1166 IkReal x103=cj3*cj3;
1167 IkReal x104=(cj3*new_r22);
1168 IkReal x105=((-1.0)+x103+(((-1.0)*x103*(new_r22*new_r22))));
1169 j5eval[0]=x105;
1170 j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x104)))))+(IKabs((((new_r00*sj3))+((new_r01*x104))))));
1171 j5eval[2]=IKsign(x105);
1172 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1173 {
1174 {
1175 IkReal j5eval[1];
1176 new_r21=0;
1177 new_r20=0;
1178 new_r02=0;
1179 new_r12=0;
1180 j5eval[0]=new_r22;
1181 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1182 {
1183 {
1184 IkReal j5eval[1];
1185 new_r21=0;
1186 new_r20=0;
1187 new_r02=0;
1188 new_r12=0;
1189 j5eval[0]=sj3;
1190 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1191 {
1192 {
1193 IkReal evalcond[1];
1194 bool bgotonextstatement = true;
1195 do
1196 {
1197 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1198 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1199 {
1200 bgotonextstatement=false;
1201 {
1202 IkReal j5array[1], cj5array[1], sj5array[1];
1203 bool j5valid[1]={false};
1204 _nj5 = 1;
1205 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1206  continue;
1207 j5array[0]=IKatan2(new_r10, new_r11);
1208 sj5array[0]=IKsin(j5array[0]);
1209 cj5array[0]=IKcos(j5array[0]);
1210 if( j5array[0] > IKPI )
1211 {
1212  j5array[0]-=IK2PI;
1213 }
1214 else if( j5array[0] < -IKPI )
1215 { j5array[0]+=IK2PI;
1216 }
1217 j5valid[0] = true;
1218 for(int ij5 = 0; ij5 < 1; ++ij5)
1219 {
1220 if( !j5valid[ij5] )
1221 {
1222  continue;
1223 }
1224 _ij5[0] = ij5; _ij5[1] = -1;
1225 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1226 {
1227 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1228 {
1229  j5valid[iij5]=false; _ij5[1] = iij5; break;
1230 }
1231 }
1232 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1233 {
1234 IkReal evalcond[4];
1235 IkReal x106=IKsin(j5);
1236 IkReal x107=IKcos(j5);
1237 evalcond[0]=x107;
1238 evalcond[1]=((-1.0)*x106);
1239 evalcond[2]=(x106+(((-1.0)*new_r10)));
1240 evalcond[3]=(x107+(((-1.0)*new_r11)));
1241 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1242 {
1243 continue;
1244 }
1245 }
1246 
1247 {
1248 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1249 vinfos[0].jointtype = 1;
1250 vinfos[0].foffset = j0;
1251 vinfos[0].indices[0] = _ij0[0];
1252 vinfos[0].indices[1] = _ij0[1];
1253 vinfos[0].maxsolutions = _nj0;
1254 vinfos[1].jointtype = 1;
1255 vinfos[1].foffset = j1;
1256 vinfos[1].indices[0] = _ij1[0];
1257 vinfos[1].indices[1] = _ij1[1];
1258 vinfos[1].maxsolutions = _nj1;
1259 vinfos[2].jointtype = 1;
1260 vinfos[2].foffset = j2;
1261 vinfos[2].indices[0] = _ij2[0];
1262 vinfos[2].indices[1] = _ij2[1];
1263 vinfos[2].maxsolutions = _nj2;
1264 vinfos[3].jointtype = 1;
1265 vinfos[3].foffset = j3;
1266 vinfos[3].indices[0] = _ij3[0];
1267 vinfos[3].indices[1] = _ij3[1];
1268 vinfos[3].maxsolutions = _nj3;
1269 vinfos[4].jointtype = 1;
1270 vinfos[4].foffset = j4;
1271 vinfos[4].indices[0] = _ij4[0];
1272 vinfos[4].indices[1] = _ij4[1];
1273 vinfos[4].maxsolutions = _nj4;
1274 vinfos[5].jointtype = 1;
1275 vinfos[5].foffset = j5;
1276 vinfos[5].indices[0] = _ij5[0];
1277 vinfos[5].indices[1] = _ij5[1];
1278 vinfos[5].maxsolutions = _nj5;
1279 std::vector<int> vfree(0);
1280 solutions.AddSolution(vinfos,vfree);
1281 }
1282 }
1283 }
1284 
1285 }
1286 } while(0);
1287 if( bgotonextstatement )
1288 {
1289 bool bgotonextstatement = true;
1290 do
1291 {
1292 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1293 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1294 {
1295 bgotonextstatement=false;
1296 {
1297 IkReal j5array[1], cj5array[1], sj5array[1];
1298 bool j5valid[1]={false};
1299 _nj5 = 1;
1300 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 )
1301  continue;
1302 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1303 sj5array[0]=IKsin(j5array[0]);
1304 cj5array[0]=IKcos(j5array[0]);
1305 if( j5array[0] > IKPI )
1306 {
1307  j5array[0]-=IK2PI;
1308 }
1309 else if( j5array[0] < -IKPI )
1310 { j5array[0]+=IK2PI;
1311 }
1312 j5valid[0] = true;
1313 for(int ij5 = 0; ij5 < 1; ++ij5)
1314 {
1315 if( !j5valid[ij5] )
1316 {
1317  continue;
1318 }
1319 _ij5[0] = ij5; _ij5[1] = -1;
1320 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1321 {
1322 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1323 {
1324  j5valid[iij5]=false; _ij5[1] = iij5; break;
1325 }
1326 }
1327 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1328 {
1329 IkReal evalcond[4];
1330 IkReal x108=IKcos(j5);
1331 IkReal x109=IKsin(j5);
1332 evalcond[0]=x108;
1333 evalcond[1]=(x109+new_r10);
1334 evalcond[2]=(x108+new_r11);
1335 evalcond[3]=((-1.0)*x109);
1336 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1337 {
1338 continue;
1339 }
1340 }
1341 
1342 {
1343 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1344 vinfos[0].jointtype = 1;
1345 vinfos[0].foffset = j0;
1346 vinfos[0].indices[0] = _ij0[0];
1347 vinfos[0].indices[1] = _ij0[1];
1348 vinfos[0].maxsolutions = _nj0;
1349 vinfos[1].jointtype = 1;
1350 vinfos[1].foffset = j1;
1351 vinfos[1].indices[0] = _ij1[0];
1352 vinfos[1].indices[1] = _ij1[1];
1353 vinfos[1].maxsolutions = _nj1;
1354 vinfos[2].jointtype = 1;
1355 vinfos[2].foffset = j2;
1356 vinfos[2].indices[0] = _ij2[0];
1357 vinfos[2].indices[1] = _ij2[1];
1358 vinfos[2].maxsolutions = _nj2;
1359 vinfos[3].jointtype = 1;
1360 vinfos[3].foffset = j3;
1361 vinfos[3].indices[0] = _ij3[0];
1362 vinfos[3].indices[1] = _ij3[1];
1363 vinfos[3].maxsolutions = _nj3;
1364 vinfos[4].jointtype = 1;
1365 vinfos[4].foffset = j4;
1366 vinfos[4].indices[0] = _ij4[0];
1367 vinfos[4].indices[1] = _ij4[1];
1368 vinfos[4].maxsolutions = _nj4;
1369 vinfos[5].jointtype = 1;
1370 vinfos[5].foffset = j5;
1371 vinfos[5].indices[0] = _ij5[0];
1372 vinfos[5].indices[1] = _ij5[1];
1373 vinfos[5].maxsolutions = _nj5;
1374 std::vector<int> vfree(0);
1375 solutions.AddSolution(vinfos,vfree);
1376 }
1377 }
1378 }
1379 
1380 }
1381 } while(0);
1382 if( bgotonextstatement )
1383 {
1384 bool bgotonextstatement = true;
1385 do
1386 {
1387 CheckValue<IkReal> x110=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1388 if(!x110.valid){
1389 continue;
1390 }
1391 if((x110.value) < -0.00001)
1392 continue;
1393 IkReal gconst18=((-1.0)*(IKsqrt(x110.value)));
1394 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1395 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1396 {
1397 bgotonextstatement=false;
1398 {
1399 IkReal j5eval[1];
1400 new_r21=0;
1401 new_r20=0;
1402 new_r02=0;
1403 new_r12=0;
1404 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1405 continue;
1406 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))));
1407 cj3=gconst18;
1408 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1409  continue;
1410 j3=IKacos(gconst18);
1411 CheckValue<IkReal> x111=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1412 if(!x111.valid){
1413 continue;
1414 }
1415 if((x111.value) < -0.00001)
1416 continue;
1417 IkReal gconst18=((-1.0)*(IKsqrt(x111.value)));
1418 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1419 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1420 {
1421 {
1422 IkReal j5array[1], cj5array[1], sj5array[1];
1423 bool j5valid[1]={false};
1424 _nj5 = 1;
1425 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1426 continue;
1427 CheckValue<IkReal> x112=IKPowWithIntegerCheck(gconst18,-1);
1428 if(!x112.valid){
1429 continue;
1430 }
1431 if( IKabs((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x112.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x112.value)))-1) <= IKFAST_SINCOS_THRESH )
1432  continue;
1433 j5array[0]=IKatan2((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x112.value)));
1434 sj5array[0]=IKsin(j5array[0]);
1435 cj5array[0]=IKcos(j5array[0]);
1436 if( j5array[0] > IKPI )
1437 {
1438  j5array[0]-=IK2PI;
1439 }
1440 else if( j5array[0] < -IKPI )
1441 { j5array[0]+=IK2PI;
1442 }
1443 j5valid[0] = true;
1444 for(int ij5 = 0; ij5 < 1; ++ij5)
1445 {
1446 if( !j5valid[ij5] )
1447 {
1448  continue;
1449 }
1450 _ij5[0] = ij5; _ij5[1] = -1;
1451 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1452 {
1453 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1454 {
1455  j5valid[iij5]=false; _ij5[1] = iij5; break;
1456 }
1457 }
1458 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1459 {
1460 IkReal evalcond[8];
1461 IkReal x113=IKcos(j5);
1462 IkReal x114=IKsin(j5);
1463 IkReal x115=((1.0)*gconst18);
1464 if((((1.0)+(((-1.0)*gconst18*x115)))) < -0.00001)
1465 continue;
1466 IkReal x116=IKsqrt(((1.0)+(((-1.0)*gconst18*x115))));
1467 evalcond[0]=x113;
1468 evalcond[1]=((-1.0)*x114);
1469 evalcond[2]=((((-1.0)*x113*x115))+new_r11);
1470 evalcond[3]=((((-1.0)*x114*x115))+new_r10);
1471 evalcond[4]=(((x113*x116))+new_r01);
1472 evalcond[5]=(((x114*x116))+new_r00);
1473 evalcond[6]=((((-1.0)*new_r10*x115))+x114+((new_r00*x116)));
1474 evalcond[7]=((((-1.0)*new_r11*x115))+x113+((new_r01*x116)));
1475 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1476 {
1477 continue;
1478 }
1479 }
1480 
1481 {
1482 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1483 vinfos[0].jointtype = 1;
1484 vinfos[0].foffset = j0;
1485 vinfos[0].indices[0] = _ij0[0];
1486 vinfos[0].indices[1] = _ij0[1];
1487 vinfos[0].maxsolutions = _nj0;
1488 vinfos[1].jointtype = 1;
1489 vinfos[1].foffset = j1;
1490 vinfos[1].indices[0] = _ij1[0];
1491 vinfos[1].indices[1] = _ij1[1];
1492 vinfos[1].maxsolutions = _nj1;
1493 vinfos[2].jointtype = 1;
1494 vinfos[2].foffset = j2;
1495 vinfos[2].indices[0] = _ij2[0];
1496 vinfos[2].indices[1] = _ij2[1];
1497 vinfos[2].maxsolutions = _nj2;
1498 vinfos[3].jointtype = 1;
1499 vinfos[3].foffset = j3;
1500 vinfos[3].indices[0] = _ij3[0];
1501 vinfos[3].indices[1] = _ij3[1];
1502 vinfos[3].maxsolutions = _nj3;
1503 vinfos[4].jointtype = 1;
1504 vinfos[4].foffset = j4;
1505 vinfos[4].indices[0] = _ij4[0];
1506 vinfos[4].indices[1] = _ij4[1];
1507 vinfos[4].maxsolutions = _nj4;
1508 vinfos[5].jointtype = 1;
1509 vinfos[5].foffset = j5;
1510 vinfos[5].indices[0] = _ij5[0];
1511 vinfos[5].indices[1] = _ij5[1];
1512 vinfos[5].maxsolutions = _nj5;
1513 std::vector<int> vfree(0);
1514 solutions.AddSolution(vinfos,vfree);
1515 }
1516 }
1517 }
1518 
1519 } else
1520 {
1521 {
1522 IkReal j5array[1], cj5array[1], sj5array[1];
1523 bool j5valid[1]={false};
1524 _nj5 = 1;
1525 CheckValue<IkReal> x117=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1526 if(!x117.valid){
1527 continue;
1528 }
1529 CheckValue<IkReal> x118 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1530 if(!x118.valid){
1531 continue;
1532 }
1533 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.value)))+(x118.value));
1534 sj5array[0]=IKsin(j5array[0]);
1535 cj5array[0]=IKcos(j5array[0]);
1536 if( j5array[0] > IKPI )
1537 {
1538  j5array[0]-=IK2PI;
1539 }
1540 else if( j5array[0] < -IKPI )
1541 { j5array[0]+=IK2PI;
1542 }
1543 j5valid[0] = true;
1544 for(int ij5 = 0; ij5 < 1; ++ij5)
1545 {
1546 if( !j5valid[ij5] )
1547 {
1548  continue;
1549 }
1550 _ij5[0] = ij5; _ij5[1] = -1;
1551 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1552 {
1553 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1554 {
1555  j5valid[iij5]=false; _ij5[1] = iij5; break;
1556 }
1557 }
1558 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1559 {
1560 IkReal evalcond[8];
1561 IkReal x119=IKcos(j5);
1562 IkReal x120=IKsin(j5);
1563 IkReal x121=((1.0)*gconst18);
1564 if((((1.0)+(((-1.0)*gconst18*x121)))) < -0.00001)
1565 continue;
1566 IkReal x122=IKsqrt(((1.0)+(((-1.0)*gconst18*x121))));
1567 evalcond[0]=x119;
1568 evalcond[1]=((-1.0)*x120);
1569 evalcond[2]=((((-1.0)*x119*x121))+new_r11);
1570 evalcond[3]=((((-1.0)*x120*x121))+new_r10);
1571 evalcond[4]=(new_r01+((x119*x122)));
1572 evalcond[5]=(((x120*x122))+new_r00);
1573 evalcond[6]=((((-1.0)*new_r10*x121))+((new_r00*x122))+x120);
1574 evalcond[7]=(((new_r01*x122))+x119+(((-1.0)*new_r11*x121)));
1575 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1576 {
1577 continue;
1578 }
1579 }
1580 
1581 {
1582 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1583 vinfos[0].jointtype = 1;
1584 vinfos[0].foffset = j0;
1585 vinfos[0].indices[0] = _ij0[0];
1586 vinfos[0].indices[1] = _ij0[1];
1587 vinfos[0].maxsolutions = _nj0;
1588 vinfos[1].jointtype = 1;
1589 vinfos[1].foffset = j1;
1590 vinfos[1].indices[0] = _ij1[0];
1591 vinfos[1].indices[1] = _ij1[1];
1592 vinfos[1].maxsolutions = _nj1;
1593 vinfos[2].jointtype = 1;
1594 vinfos[2].foffset = j2;
1595 vinfos[2].indices[0] = _ij2[0];
1596 vinfos[2].indices[1] = _ij2[1];
1597 vinfos[2].maxsolutions = _nj2;
1598 vinfos[3].jointtype = 1;
1599 vinfos[3].foffset = j3;
1600 vinfos[3].indices[0] = _ij3[0];
1601 vinfos[3].indices[1] = _ij3[1];
1602 vinfos[3].maxsolutions = _nj3;
1603 vinfos[4].jointtype = 1;
1604 vinfos[4].foffset = j4;
1605 vinfos[4].indices[0] = _ij4[0];
1606 vinfos[4].indices[1] = _ij4[1];
1607 vinfos[4].maxsolutions = _nj4;
1608 vinfos[5].jointtype = 1;
1609 vinfos[5].foffset = j5;
1610 vinfos[5].indices[0] = _ij5[0];
1611 vinfos[5].indices[1] = _ij5[1];
1612 vinfos[5].maxsolutions = _nj5;
1613 std::vector<int> vfree(0);
1614 solutions.AddSolution(vinfos,vfree);
1615 }
1616 }
1617 }
1618 
1619 }
1620 
1621 }
1622 
1623 }
1624 } while(0);
1625 if( bgotonextstatement )
1626 {
1627 bool bgotonextstatement = true;
1628 do
1629 {
1630 CheckValue<IkReal> x123=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1631 if(!x123.valid){
1632 continue;
1633 }
1634 if((x123.value) < -0.00001)
1635 continue;
1636 IkReal gconst18=((-1.0)*(IKsqrt(x123.value)));
1637 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1638 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1639 {
1640 bgotonextstatement=false;
1641 {
1642 IkReal j5eval[1];
1643 new_r21=0;
1644 new_r20=0;
1645 new_r02=0;
1646 new_r12=0;
1647 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1648 continue;
1649 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))));
1650 cj3=gconst18;
1651 if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1652  continue;
1653 j3=((-1.0)*(IKacos(gconst18)));
1654 CheckValue<IkReal> x124=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1655 if(!x124.valid){
1656 continue;
1657 }
1658 if((x124.value) < -0.00001)
1659 continue;
1660 IkReal gconst18=((-1.0)*(IKsqrt(x124.value)));
1661 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1662 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1663 {
1664 {
1665 IkReal j5array[1], cj5array[1], sj5array[1];
1666 bool j5valid[1]={false};
1667 _nj5 = 1;
1668 if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1669 continue;
1670 CheckValue<IkReal> x125=IKPowWithIntegerCheck(gconst18,-1);
1671 if(!x125.valid){
1672 continue;
1673 }
1674 if( IKabs((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x125.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x125.value)))-1) <= IKFAST_SINCOS_THRESH )
1675  continue;
1676 j5array[0]=IKatan2((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x125.value)));
1677 sj5array[0]=IKsin(j5array[0]);
1678 cj5array[0]=IKcos(j5array[0]);
1679 if( j5array[0] > IKPI )
1680 {
1681  j5array[0]-=IK2PI;
1682 }
1683 else if( j5array[0] < -IKPI )
1684 { j5array[0]+=IK2PI;
1685 }
1686 j5valid[0] = true;
1687 for(int ij5 = 0; ij5 < 1; ++ij5)
1688 {
1689 if( !j5valid[ij5] )
1690 {
1691  continue;
1692 }
1693 _ij5[0] = ij5; _ij5[1] = -1;
1694 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1695 {
1696 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1697 {
1698  j5valid[iij5]=false; _ij5[1] = iij5; break;
1699 }
1700 }
1701 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1702 {
1703 IkReal evalcond[8];
1704 IkReal x126=IKcos(j5);
1705 IkReal x127=IKsin(j5);
1706 IkReal x128=((1.0)*gconst18);
1707 if((((1.0)+(((-1.0)*gconst18*x128)))) < -0.00001)
1708 continue;
1709 IkReal x129=IKsqrt(((1.0)+(((-1.0)*gconst18*x128))));
1710 IkReal x130=((1.0)*x129);
1711 evalcond[0]=x126;
1712 evalcond[1]=((-1.0)*x127);
1713 evalcond[2]=((((-1.0)*x126*x128))+new_r11);
1714 evalcond[3]=(new_r10+(((-1.0)*x127*x128)));
1715 evalcond[4]=((((-1.0)*x126*x130))+new_r01);
1716 evalcond[5]=(new_r00+(((-1.0)*x127*x130)));
1717 evalcond[6]=((((-1.0)*new_r10*x128))+(((-1.0)*new_r00*x130))+x127);
1718 evalcond[7]=((((-1.0)*new_r01*x130))+x126+(((-1.0)*new_r11*x128)));
1719 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1720 {
1721 continue;
1722 }
1723 }
1724 
1725 {
1726 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1727 vinfos[0].jointtype = 1;
1728 vinfos[0].foffset = j0;
1729 vinfos[0].indices[0] = _ij0[0];
1730 vinfos[0].indices[1] = _ij0[1];
1731 vinfos[0].maxsolutions = _nj0;
1732 vinfos[1].jointtype = 1;
1733 vinfos[1].foffset = j1;
1734 vinfos[1].indices[0] = _ij1[0];
1735 vinfos[1].indices[1] = _ij1[1];
1736 vinfos[1].maxsolutions = _nj1;
1737 vinfos[2].jointtype = 1;
1738 vinfos[2].foffset = j2;
1739 vinfos[2].indices[0] = _ij2[0];
1740 vinfos[2].indices[1] = _ij2[1];
1741 vinfos[2].maxsolutions = _nj2;
1742 vinfos[3].jointtype = 1;
1743 vinfos[3].foffset = j3;
1744 vinfos[3].indices[0] = _ij3[0];
1745 vinfos[3].indices[1] = _ij3[1];
1746 vinfos[3].maxsolutions = _nj3;
1747 vinfos[4].jointtype = 1;
1748 vinfos[4].foffset = j4;
1749 vinfos[4].indices[0] = _ij4[0];
1750 vinfos[4].indices[1] = _ij4[1];
1751 vinfos[4].maxsolutions = _nj4;
1752 vinfos[5].jointtype = 1;
1753 vinfos[5].foffset = j5;
1754 vinfos[5].indices[0] = _ij5[0];
1755 vinfos[5].indices[1] = _ij5[1];
1756 vinfos[5].maxsolutions = _nj5;
1757 std::vector<int> vfree(0);
1758 solutions.AddSolution(vinfos,vfree);
1759 }
1760 }
1761 }
1762 
1763 } else
1764 {
1765 {
1766 IkReal j5array[1], cj5array[1], sj5array[1];
1767 bool j5valid[1]={false};
1768 _nj5 = 1;
1769 CheckValue<IkReal> x131=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1770 if(!x131.valid){
1771 continue;
1772 }
1773 CheckValue<IkReal> x132 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1774 if(!x132.valid){
1775 continue;
1776 }
1777 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x131.value)))+(x132.value));
1778 sj5array[0]=IKsin(j5array[0]);
1779 cj5array[0]=IKcos(j5array[0]);
1780 if( j5array[0] > IKPI )
1781 {
1782  j5array[0]-=IK2PI;
1783 }
1784 else if( j5array[0] < -IKPI )
1785 { j5array[0]+=IK2PI;
1786 }
1787 j5valid[0] = true;
1788 for(int ij5 = 0; ij5 < 1; ++ij5)
1789 {
1790 if( !j5valid[ij5] )
1791 {
1792  continue;
1793 }
1794 _ij5[0] = ij5; _ij5[1] = -1;
1795 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1796 {
1797 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1798 {
1799  j5valid[iij5]=false; _ij5[1] = iij5; break;
1800 }
1801 }
1802 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1803 {
1804 IkReal evalcond[8];
1805 IkReal x133=IKcos(j5);
1806 IkReal x134=IKsin(j5);
1807 IkReal x135=((1.0)*gconst18);
1808 if((((1.0)+(((-1.0)*gconst18*x135)))) < -0.00001)
1809 continue;
1810 IkReal x136=IKsqrt(((1.0)+(((-1.0)*gconst18*x135))));
1811 IkReal x137=((1.0)*x136);
1812 evalcond[0]=x133;
1813 evalcond[1]=((-1.0)*x134);
1814 evalcond[2]=((((-1.0)*x133*x135))+new_r11);
1815 evalcond[3]=((((-1.0)*x134*x135))+new_r10);
1816 evalcond[4]=((((-1.0)*x133*x137))+new_r01);
1817 evalcond[5]=((((-1.0)*x134*x137))+new_r00);
1818 evalcond[6]=((((-1.0)*new_r10*x135))+(((-1.0)*new_r00*x137))+x134);
1819 evalcond[7]=((((-1.0)*new_r11*x135))+(((-1.0)*new_r01*x137))+x133);
1820 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1821 {
1822 continue;
1823 }
1824 }
1825 
1826 {
1827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1828 vinfos[0].jointtype = 1;
1829 vinfos[0].foffset = j0;
1830 vinfos[0].indices[0] = _ij0[0];
1831 vinfos[0].indices[1] = _ij0[1];
1832 vinfos[0].maxsolutions = _nj0;
1833 vinfos[1].jointtype = 1;
1834 vinfos[1].foffset = j1;
1835 vinfos[1].indices[0] = _ij1[0];
1836 vinfos[1].indices[1] = _ij1[1];
1837 vinfos[1].maxsolutions = _nj1;
1838 vinfos[2].jointtype = 1;
1839 vinfos[2].foffset = j2;
1840 vinfos[2].indices[0] = _ij2[0];
1841 vinfos[2].indices[1] = _ij2[1];
1842 vinfos[2].maxsolutions = _nj2;
1843 vinfos[3].jointtype = 1;
1844 vinfos[3].foffset = j3;
1845 vinfos[3].indices[0] = _ij3[0];
1846 vinfos[3].indices[1] = _ij3[1];
1847 vinfos[3].maxsolutions = _nj3;
1848 vinfos[4].jointtype = 1;
1849 vinfos[4].foffset = j4;
1850 vinfos[4].indices[0] = _ij4[0];
1851 vinfos[4].indices[1] = _ij4[1];
1852 vinfos[4].maxsolutions = _nj4;
1853 vinfos[5].jointtype = 1;
1854 vinfos[5].foffset = j5;
1855 vinfos[5].indices[0] = _ij5[0];
1856 vinfos[5].indices[1] = _ij5[1];
1857 vinfos[5].maxsolutions = _nj5;
1858 std::vector<int> vfree(0);
1859 solutions.AddSolution(vinfos,vfree);
1860 }
1861 }
1862 }
1863 
1864 }
1865 
1866 }
1867 
1868 }
1869 } while(0);
1870 if( bgotonextstatement )
1871 {
1872 bool bgotonextstatement = true;
1873 do
1874 {
1875 CheckValue<IkReal> x138=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1876 if(!x138.valid){
1877 continue;
1878 }
1879 if((x138.value) < -0.00001)
1880 continue;
1881 IkReal gconst19=IKsqrt(x138.value);
1882 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
1883 if( IKabs(evalcond[0]) < 0.0000050000000000 )
1884 {
1885 bgotonextstatement=false;
1886 {
1887 IkReal j5eval[1];
1888 new_r21=0;
1889 new_r20=0;
1890 new_r02=0;
1891 new_r12=0;
1892 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1893 continue;
1894 sj3=IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))));
1895 cj3=gconst19;
1896 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
1897  continue;
1898 j3=IKacos(gconst19);
1899 CheckValue<IkReal> x139=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1900 if(!x139.valid){
1901 continue;
1902 }
1903 if((x139.value) < -0.00001)
1904 continue;
1905 IkReal gconst19=IKsqrt(x139.value);
1906 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1907 if( IKabs(j5eval[0]) < 0.0000010000000000 )
1908 {
1909 {
1910 IkReal j5array[1], cj5array[1], sj5array[1];
1911 bool j5valid[1]={false};
1912 _nj5 = 1;
1913 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1914 continue;
1915 CheckValue<IkReal> x140=IKPowWithIntegerCheck(gconst19,-1);
1916 if(!x140.valid){
1917 continue;
1918 }
1919 if( IKabs((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x140.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))))+IKsqr((new_r11*(x140.value)))-1) <= IKFAST_SINCOS_THRESH )
1920  continue;
1921 j5array[0]=IKatan2((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))), (new_r11*(x140.value)));
1922 sj5array[0]=IKsin(j5array[0]);
1923 cj5array[0]=IKcos(j5array[0]);
1924 if( j5array[0] > IKPI )
1925 {
1926  j5array[0]-=IK2PI;
1927 }
1928 else if( j5array[0] < -IKPI )
1929 { j5array[0]+=IK2PI;
1930 }
1931 j5valid[0] = true;
1932 for(int ij5 = 0; ij5 < 1; ++ij5)
1933 {
1934 if( !j5valid[ij5] )
1935 {
1936  continue;
1937 }
1938 _ij5[0] = ij5; _ij5[1] = -1;
1939 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1940 {
1941 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1942 {
1943  j5valid[iij5]=false; _ij5[1] = iij5; break;
1944 }
1945 }
1946 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1947 {
1948 IkReal evalcond[8];
1949 IkReal x141=IKcos(j5);
1950 IkReal x142=IKsin(j5);
1951 IkReal x143=((1.0)*gconst19);
1952 if((((1.0)+(((-1.0)*gconst19*x143)))) < -0.00001)
1953 continue;
1954 IkReal x144=IKsqrt(((1.0)+(((-1.0)*gconst19*x143))));
1955 evalcond[0]=x141;
1956 evalcond[1]=((-1.0)*x142);
1957 evalcond[2]=((((-1.0)*x141*x143))+new_r11);
1958 evalcond[3]=((((-1.0)*x142*x143))+new_r10);
1959 evalcond[4]=(((x141*x144))+new_r01);
1960 evalcond[5]=(((x142*x144))+new_r00);
1961 evalcond[6]=(((new_r00*x144))+x142+(((-1.0)*new_r10*x143)));
1962 evalcond[7]=(((new_r01*x144))+(((-1.0)*new_r11*x143))+x141);
1963 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1964 {
1965 continue;
1966 }
1967 }
1968 
1969 {
1970 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1971 vinfos[0].jointtype = 1;
1972 vinfos[0].foffset = j0;
1973 vinfos[0].indices[0] = _ij0[0];
1974 vinfos[0].indices[1] = _ij0[1];
1975 vinfos[0].maxsolutions = _nj0;
1976 vinfos[1].jointtype = 1;
1977 vinfos[1].foffset = j1;
1978 vinfos[1].indices[0] = _ij1[0];
1979 vinfos[1].indices[1] = _ij1[1];
1980 vinfos[1].maxsolutions = _nj1;
1981 vinfos[2].jointtype = 1;
1982 vinfos[2].foffset = j2;
1983 vinfos[2].indices[0] = _ij2[0];
1984 vinfos[2].indices[1] = _ij2[1];
1985 vinfos[2].maxsolutions = _nj2;
1986 vinfos[3].jointtype = 1;
1987 vinfos[3].foffset = j3;
1988 vinfos[3].indices[0] = _ij3[0];
1989 vinfos[3].indices[1] = _ij3[1];
1990 vinfos[3].maxsolutions = _nj3;
1991 vinfos[4].jointtype = 1;
1992 vinfos[4].foffset = j4;
1993 vinfos[4].indices[0] = _ij4[0];
1994 vinfos[4].indices[1] = _ij4[1];
1995 vinfos[4].maxsolutions = _nj4;
1996 vinfos[5].jointtype = 1;
1997 vinfos[5].foffset = j5;
1998 vinfos[5].indices[0] = _ij5[0];
1999 vinfos[5].indices[1] = _ij5[1];
2000 vinfos[5].maxsolutions = _nj5;
2001 std::vector<int> vfree(0);
2002 solutions.AddSolution(vinfos,vfree);
2003 }
2004 }
2005 }
2006 
2007 } else
2008 {
2009 {
2010 IkReal j5array[1], cj5array[1], sj5array[1];
2011 bool j5valid[1]={false};
2012 _nj5 = 1;
2013 CheckValue<IkReal> x145=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2014 if(!x145.valid){
2015 continue;
2016 }
2017 CheckValue<IkReal> x146 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2018 if(!x146.valid){
2019 continue;
2020 }
2021 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x145.value)))+(x146.value));
2022 sj5array[0]=IKsin(j5array[0]);
2023 cj5array[0]=IKcos(j5array[0]);
2024 if( j5array[0] > IKPI )
2025 {
2026  j5array[0]-=IK2PI;
2027 }
2028 else if( j5array[0] < -IKPI )
2029 { j5array[0]+=IK2PI;
2030 }
2031 j5valid[0] = true;
2032 for(int ij5 = 0; ij5 < 1; ++ij5)
2033 {
2034 if( !j5valid[ij5] )
2035 {
2036  continue;
2037 }
2038 _ij5[0] = ij5; _ij5[1] = -1;
2039 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2040 {
2041 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2042 {
2043  j5valid[iij5]=false; _ij5[1] = iij5; break;
2044 }
2045 }
2046 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2047 {
2048 IkReal evalcond[8];
2049 IkReal x147=IKcos(j5);
2050 IkReal x148=IKsin(j5);
2051 IkReal x149=((1.0)*gconst19);
2052 if((((1.0)+(((-1.0)*gconst19*x149)))) < -0.00001)
2053 continue;
2054 IkReal x150=IKsqrt(((1.0)+(((-1.0)*gconst19*x149))));
2055 evalcond[0]=x147;
2056 evalcond[1]=((-1.0)*x148);
2057 evalcond[2]=((((-1.0)*x147*x149))+new_r11);
2058 evalcond[3]=((((-1.0)*x148*x149))+new_r10);
2059 evalcond[4]=(((x147*x150))+new_r01);
2060 evalcond[5]=(((x148*x150))+new_r00);
2061 evalcond[6]=(((new_r00*x150))+x148+(((-1.0)*new_r10*x149)));
2062 evalcond[7]=(((new_r01*x150))+(((-1.0)*new_r11*x149))+x147);
2063 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2064 {
2065 continue;
2066 }
2067 }
2068 
2069 {
2070 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2071 vinfos[0].jointtype = 1;
2072 vinfos[0].foffset = j0;
2073 vinfos[0].indices[0] = _ij0[0];
2074 vinfos[0].indices[1] = _ij0[1];
2075 vinfos[0].maxsolutions = _nj0;
2076 vinfos[1].jointtype = 1;
2077 vinfos[1].foffset = j1;
2078 vinfos[1].indices[0] = _ij1[0];
2079 vinfos[1].indices[1] = _ij1[1];
2080 vinfos[1].maxsolutions = _nj1;
2081 vinfos[2].jointtype = 1;
2082 vinfos[2].foffset = j2;
2083 vinfos[2].indices[0] = _ij2[0];
2084 vinfos[2].indices[1] = _ij2[1];
2085 vinfos[2].maxsolutions = _nj2;
2086 vinfos[3].jointtype = 1;
2087 vinfos[3].foffset = j3;
2088 vinfos[3].indices[0] = _ij3[0];
2089 vinfos[3].indices[1] = _ij3[1];
2090 vinfos[3].maxsolutions = _nj3;
2091 vinfos[4].jointtype = 1;
2092 vinfos[4].foffset = j4;
2093 vinfos[4].indices[0] = _ij4[0];
2094 vinfos[4].indices[1] = _ij4[1];
2095 vinfos[4].maxsolutions = _nj4;
2096 vinfos[5].jointtype = 1;
2097 vinfos[5].foffset = j5;
2098 vinfos[5].indices[0] = _ij5[0];
2099 vinfos[5].indices[1] = _ij5[1];
2100 vinfos[5].maxsolutions = _nj5;
2101 std::vector<int> vfree(0);
2102 solutions.AddSolution(vinfos,vfree);
2103 }
2104 }
2105 }
2106 
2107 }
2108 
2109 }
2110 
2111 }
2112 } while(0);
2113 if( bgotonextstatement )
2114 {
2115 bool bgotonextstatement = true;
2116 do
2117 {
2118 CheckValue<IkReal> x151=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2119 if(!x151.valid){
2120 continue;
2121 }
2122 if((x151.value) < -0.00001)
2123 continue;
2124 IkReal gconst19=IKsqrt(x151.value);
2125 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
2126 if( IKabs(evalcond[0]) < 0.0000050000000000 )
2127 {
2128 bgotonextstatement=false;
2129 {
2130 IkReal j5eval[1];
2131 new_r21=0;
2132 new_r20=0;
2133 new_r02=0;
2134 new_r12=0;
2135 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2136 continue;
2137 sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))));
2138 cj3=gconst19;
2139 if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
2140  continue;
2141 j3=((-1.0)*(IKacos(gconst19)));
2142 CheckValue<IkReal> x152=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2143 if(!x152.valid){
2144 continue;
2145 }
2146 if((x152.value) < -0.00001)
2147 continue;
2148 IkReal gconst19=IKsqrt(x152.value);
2149 j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2150 if( IKabs(j5eval[0]) < 0.0000010000000000 )
2151 {
2152 {
2153 IkReal j5array[1], cj5array[1], sj5array[1];
2154 bool j5valid[1]={false};
2155 _nj5 = 1;
2156 if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2157 continue;
2158 CheckValue<IkReal> x153=IKPowWithIntegerCheck(gconst19,-1);
2159 if(!x153.valid){
2160 continue;
2161 }
2162 if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x153.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))))+IKsqr((new_r11*(x153.value)))-1) <= IKFAST_SINCOS_THRESH )
2163  continue;
2164 j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))), (new_r11*(x153.value)));
2165 sj5array[0]=IKsin(j5array[0]);
2166 cj5array[0]=IKcos(j5array[0]);
2167 if( j5array[0] > IKPI )
2168 {
2169  j5array[0]-=IK2PI;
2170 }
2171 else if( j5array[0] < -IKPI )
2172 { j5array[0]+=IK2PI;
2173 }
2174 j5valid[0] = true;
2175 for(int ij5 = 0; ij5 < 1; ++ij5)
2176 {
2177 if( !j5valid[ij5] )
2178 {
2179  continue;
2180 }
2181 _ij5[0] = ij5; _ij5[1] = -1;
2182 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2183 {
2184 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2185 {
2186  j5valid[iij5]=false; _ij5[1] = iij5; break;
2187 }
2188 }
2189 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2190 {
2191 IkReal evalcond[8];
2192 IkReal x154=IKcos(j5);
2193 IkReal x155=IKsin(j5);
2194 IkReal x156=((1.0)*gconst19);
2195 if((((1.0)+(((-1.0)*gconst19*x156)))) < -0.00001)
2196 continue;
2197 IkReal x157=IKsqrt(((1.0)+(((-1.0)*gconst19*x156))));
2198 IkReal x158=((1.0)*x157);
2199 evalcond[0]=x154;
2200 evalcond[1]=((-1.0)*x155);
2201 evalcond[2]=((((-1.0)*x154*x156))+new_r11);
2202 evalcond[3]=((((-1.0)*x155*x156))+new_r10);
2203 evalcond[4]=((((-1.0)*x154*x158))+new_r01);
2204 evalcond[5]=((((-1.0)*x155*x158))+new_r00);
2205 evalcond[6]=(x155+(((-1.0)*new_r10*x156))+(((-1.0)*new_r00*x158)));
2206 evalcond[7]=(x154+(((-1.0)*new_r11*x156))+(((-1.0)*new_r01*x158)));
2207 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2208 {
2209 continue;
2210 }
2211 }
2212 
2213 {
2214 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2215 vinfos[0].jointtype = 1;
2216 vinfos[0].foffset = j0;
2217 vinfos[0].indices[0] = _ij0[0];
2218 vinfos[0].indices[1] = _ij0[1];
2219 vinfos[0].maxsolutions = _nj0;
2220 vinfos[1].jointtype = 1;
2221 vinfos[1].foffset = j1;
2222 vinfos[1].indices[0] = _ij1[0];
2223 vinfos[1].indices[1] = _ij1[1];
2224 vinfos[1].maxsolutions = _nj1;
2225 vinfos[2].jointtype = 1;
2226 vinfos[2].foffset = j2;
2227 vinfos[2].indices[0] = _ij2[0];
2228 vinfos[2].indices[1] = _ij2[1];
2229 vinfos[2].maxsolutions = _nj2;
2230 vinfos[3].jointtype = 1;
2231 vinfos[3].foffset = j3;
2232 vinfos[3].indices[0] = _ij3[0];
2233 vinfos[3].indices[1] = _ij3[1];
2234 vinfos[3].maxsolutions = _nj3;
2235 vinfos[4].jointtype = 1;
2236 vinfos[4].foffset = j4;
2237 vinfos[4].indices[0] = _ij4[0];
2238 vinfos[4].indices[1] = _ij4[1];
2239 vinfos[4].maxsolutions = _nj4;
2240 vinfos[5].jointtype = 1;
2241 vinfos[5].foffset = j5;
2242 vinfos[5].indices[0] = _ij5[0];
2243 vinfos[5].indices[1] = _ij5[1];
2244 vinfos[5].maxsolutions = _nj5;
2245 std::vector<int> vfree(0);
2246 solutions.AddSolution(vinfos,vfree);
2247 }
2248 }
2249 }
2250 
2251 } else
2252 {
2253 {
2254 IkReal j5array[1], cj5array[1], sj5array[1];
2255 bool j5valid[1]={false};
2256 _nj5 = 1;
2257 CheckValue<IkReal> x159=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2258 if(!x159.valid){
2259 continue;
2260 }
2261 CheckValue<IkReal> x160 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2262 if(!x160.valid){
2263 continue;
2264 }
2265 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x159.value)))+(x160.value));
2266 sj5array[0]=IKsin(j5array[0]);
2267 cj5array[0]=IKcos(j5array[0]);
2268 if( j5array[0] > IKPI )
2269 {
2270  j5array[0]-=IK2PI;
2271 }
2272 else if( j5array[0] < -IKPI )
2273 { j5array[0]+=IK2PI;
2274 }
2275 j5valid[0] = true;
2276 for(int ij5 = 0; ij5 < 1; ++ij5)
2277 {
2278 if( !j5valid[ij5] )
2279 {
2280  continue;
2281 }
2282 _ij5[0] = ij5; _ij5[1] = -1;
2283 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2284 {
2285 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2286 {
2287  j5valid[iij5]=false; _ij5[1] = iij5; break;
2288 }
2289 }
2290 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2291 {
2292 IkReal evalcond[8];
2293 IkReal x161=IKcos(j5);
2294 IkReal x162=IKsin(j5);
2295 IkReal x163=((1.0)*gconst19);
2296 if((((1.0)+(((-1.0)*gconst19*x163)))) < -0.00001)
2297 continue;
2298 IkReal x164=IKsqrt(((1.0)+(((-1.0)*gconst19*x163))));
2299 IkReal x165=((1.0)*x164);
2300 evalcond[0]=x161;
2301 evalcond[1]=((-1.0)*x162);
2302 evalcond[2]=((((-1.0)*x161*x163))+new_r11);
2303 evalcond[3]=((((-1.0)*x162*x163))+new_r10);
2304 evalcond[4]=((((-1.0)*x161*x165))+new_r01);
2305 evalcond[5]=((((-1.0)*x162*x165))+new_r00);
2306 evalcond[6]=((((-1.0)*new_r00*x165))+(((-1.0)*new_r10*x163))+x162);
2307 evalcond[7]=((((-1.0)*new_r11*x163))+x161+(((-1.0)*new_r01*x165)));
2308 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2309 {
2310 continue;
2311 }
2312 }
2313 
2314 {
2315 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2316 vinfos[0].jointtype = 1;
2317 vinfos[0].foffset = j0;
2318 vinfos[0].indices[0] = _ij0[0];
2319 vinfos[0].indices[1] = _ij0[1];
2320 vinfos[0].maxsolutions = _nj0;
2321 vinfos[1].jointtype = 1;
2322 vinfos[1].foffset = j1;
2323 vinfos[1].indices[0] = _ij1[0];
2324 vinfos[1].indices[1] = _ij1[1];
2325 vinfos[1].maxsolutions = _nj1;
2326 vinfos[2].jointtype = 1;
2327 vinfos[2].foffset = j2;
2328 vinfos[2].indices[0] = _ij2[0];
2329 vinfos[2].indices[1] = _ij2[1];
2330 vinfos[2].maxsolutions = _nj2;
2331 vinfos[3].jointtype = 1;
2332 vinfos[3].foffset = j3;
2333 vinfos[3].indices[0] = _ij3[0];
2334 vinfos[3].indices[1] = _ij3[1];
2335 vinfos[3].maxsolutions = _nj3;
2336 vinfos[4].jointtype = 1;
2337 vinfos[4].foffset = j4;
2338 vinfos[4].indices[0] = _ij4[0];
2339 vinfos[4].indices[1] = _ij4[1];
2340 vinfos[4].maxsolutions = _nj4;
2341 vinfos[5].jointtype = 1;
2342 vinfos[5].foffset = j5;
2343 vinfos[5].indices[0] = _ij5[0];
2344 vinfos[5].indices[1] = _ij5[1];
2345 vinfos[5].maxsolutions = _nj5;
2346 std::vector<int> vfree(0);
2347 solutions.AddSolution(vinfos,vfree);
2348 }
2349 }
2350 }
2351 
2352 }
2353 
2354 }
2355 
2356 }
2357 } while(0);
2358 if( bgotonextstatement )
2359 {
2360 bool bgotonextstatement = true;
2361 do
2362 {
2363 if( 1 )
2364 {
2365 bgotonextstatement=false;
2366 continue; // branch miss [j5]
2367 
2368 }
2369 } while(0);
2370 if( bgotonextstatement )
2371 {
2372 }
2373 }
2374 }
2375 }
2376 }
2377 }
2378 }
2379 }
2380 
2381 } else
2382 {
2383 {
2384 IkReal j5array[1], cj5array[1], sj5array[1];
2385 bool j5valid[1]={false};
2386 _nj5 = 1;
2387 IkReal x166=(new_r00*sj3);
2388 CheckValue<IkReal> x167=IKPowWithIntegerCheck(sj3,-1);
2389 if(!x167.valid){
2390 continue;
2391 }
2392 if( IKabs(((((-1.0)*x166))+((cj3*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x166))+((cj3*new_r10))))+IKsqr(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2393  continue;
2394 j5array[0]=IKatan2(((((-1.0)*x166))+((cj3*new_r10))), ((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2395 sj5array[0]=IKsin(j5array[0]);
2396 cj5array[0]=IKcos(j5array[0]);
2397 if( j5array[0] > IKPI )
2398 {
2399  j5array[0]-=IK2PI;
2400 }
2401 else if( j5array[0] < -IKPI )
2402 { j5array[0]+=IK2PI;
2403 }
2404 j5valid[0] = true;
2405 for(int ij5 = 0; ij5 < 1; ++ij5)
2406 {
2407 if( !j5valid[ij5] )
2408 {
2409  continue;
2410 }
2411 _ij5[0] = ij5; _ij5[1] = -1;
2412 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2413 {
2414 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2415 {
2416  j5valid[iij5]=false; _ij5[1] = iij5; break;
2417 }
2418 }
2419 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2420 {
2421 IkReal evalcond[10];
2422 IkReal x168=IKsin(j5);
2423 IkReal x169=IKcos(j5);
2424 IkReal x170=((1.0)*new_r11);
2425 IkReal x171=(new_r22*sj3);
2426 IkReal x172=((1.0)*new_r10);
2427 IkReal x173=((1.0)*cj3*new_r22);
2428 IkReal x174=(sj3*x168);
2429 IkReal x175=(new_r22*x168);
2430 IkReal x176=((1.0)*x169);
2431 IkReal x177=((1.0)*x168);
2432 evalcond[0]=(((new_r00*sj3))+x168+(((-1.0)*cj3*x172)));
2433 evalcond[1]=(((new_r01*sj3))+x169+(((-1.0)*cj3*x170)));
2434 evalcond[2]=(((new_r11*sj3))+x175+((cj3*new_r01)));
2435 evalcond[3]=(((sj3*x169))+((cj3*x175))+new_r01);
2436 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x176))+((cj3*new_r00)));
2437 evalcond[5]=(x174+new_r00+(((-1.0)*x169*x173)));
2438 evalcond[6]=(((x168*x171))+(((-1.0)*cj3*x176))+new_r11);
2439 evalcond[7]=(x169+(((-1.0)*x171*x172))+(((-1.0)*new_r00*x173)));
2440 evalcond[8]=((((-1.0)*cj3*x177))+(((-1.0)*x171*x176))+new_r10);
2441 evalcond[9]=((((-1.0)*x177))+(((-1.0)*x170*x171))+(((-1.0)*new_r01*x173)));
2442 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2443 {
2444 continue;
2445 }
2446 }
2447 
2448 {
2449 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2450 vinfos[0].jointtype = 1;
2451 vinfos[0].foffset = j0;
2452 vinfos[0].indices[0] = _ij0[0];
2453 vinfos[0].indices[1] = _ij0[1];
2454 vinfos[0].maxsolutions = _nj0;
2455 vinfos[1].jointtype = 1;
2456 vinfos[1].foffset = j1;
2457 vinfos[1].indices[0] = _ij1[0];
2458 vinfos[1].indices[1] = _ij1[1];
2459 vinfos[1].maxsolutions = _nj1;
2460 vinfos[2].jointtype = 1;
2461 vinfos[2].foffset = j2;
2462 vinfos[2].indices[0] = _ij2[0];
2463 vinfos[2].indices[1] = _ij2[1];
2464 vinfos[2].maxsolutions = _nj2;
2465 vinfos[3].jointtype = 1;
2466 vinfos[3].foffset = j3;
2467 vinfos[3].indices[0] = _ij3[0];
2468 vinfos[3].indices[1] = _ij3[1];
2469 vinfos[3].maxsolutions = _nj3;
2470 vinfos[4].jointtype = 1;
2471 vinfos[4].foffset = j4;
2472 vinfos[4].indices[0] = _ij4[0];
2473 vinfos[4].indices[1] = _ij4[1];
2474 vinfos[4].maxsolutions = _nj4;
2475 vinfos[5].jointtype = 1;
2476 vinfos[5].foffset = j5;
2477 vinfos[5].indices[0] = _ij5[0];
2478 vinfos[5].indices[1] = _ij5[1];
2479 vinfos[5].maxsolutions = _nj5;
2480 std::vector<int> vfree(0);
2481 solutions.AddSolution(vinfos,vfree);
2482 }
2483 }
2484 }
2485 
2486 }
2487 
2488 }
2489 
2490 } else
2491 {
2492 {
2493 IkReal j5array[1], cj5array[1], sj5array[1];
2494 bool j5valid[1]={false};
2495 _nj5 = 1;
2496 IkReal x178=((1.0)*new_r01);
2497 CheckValue<IkReal> x179=IKPowWithIntegerCheck(new_r22,-1);
2498 if(!x179.valid){
2499 continue;
2500 }
2501 if( IKabs(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r11))+(((-1.0)*sj3*x178)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))))+IKsqr((((cj3*new_r11))+(((-1.0)*sj3*x178))))-1) <= IKFAST_SINCOS_THRESH )
2502  continue;
2503 j5array[0]=IKatan2(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))), (((cj3*new_r11))+(((-1.0)*sj3*x178))));
2504 sj5array[0]=IKsin(j5array[0]);
2505 cj5array[0]=IKcos(j5array[0]);
2506 if( j5array[0] > IKPI )
2507 {
2508  j5array[0]-=IK2PI;
2509 }
2510 else if( j5array[0] < -IKPI )
2511 { j5array[0]+=IK2PI;
2512 }
2513 j5valid[0] = true;
2514 for(int ij5 = 0; ij5 < 1; ++ij5)
2515 {
2516 if( !j5valid[ij5] )
2517 {
2518  continue;
2519 }
2520 _ij5[0] = ij5; _ij5[1] = -1;
2521 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2522 {
2523 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2524 {
2525  j5valid[iij5]=false; _ij5[1] = iij5; break;
2526 }
2527 }
2528 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2529 {
2530 IkReal evalcond[10];
2531 IkReal x180=IKsin(j5);
2532 IkReal x181=IKcos(j5);
2533 IkReal x182=((1.0)*new_r11);
2534 IkReal x183=(new_r22*sj3);
2535 IkReal x184=((1.0)*new_r10);
2536 IkReal x185=((1.0)*cj3*new_r22);
2537 IkReal x186=(sj3*x180);
2538 IkReal x187=(new_r22*x180);
2539 IkReal x188=((1.0)*x181);
2540 IkReal x189=((1.0)*x180);
2541 evalcond[0]=(((new_r00*sj3))+x180+(((-1.0)*cj3*x184)));
2542 evalcond[1]=(((new_r01*sj3))+x181+(((-1.0)*cj3*x182)));
2543 evalcond[2]=(((new_r11*sj3))+x187+((cj3*new_r01)));
2544 evalcond[3]=(((sj3*x181))+((cj3*x187))+new_r01);
2545 evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x188))+((cj3*new_r00)));
2546 evalcond[5]=(x186+new_r00+(((-1.0)*x181*x185)));
2547 evalcond[6]=(((x180*x183))+new_r11+(((-1.0)*cj3*x188)));
2548 evalcond[7]=(x181+(((-1.0)*new_r00*x185))+(((-1.0)*x183*x184)));
2549 evalcond[8]=(new_r10+(((-1.0)*x183*x188))+(((-1.0)*cj3*x189)));
2550 evalcond[9]=((((-1.0)*x189))+(((-1.0)*new_r01*x185))+(((-1.0)*x182*x183)));
2551 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2552 {
2553 continue;
2554 }
2555 }
2556 
2557 {
2558 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2559 vinfos[0].jointtype = 1;
2560 vinfos[0].foffset = j0;
2561 vinfos[0].indices[0] = _ij0[0];
2562 vinfos[0].indices[1] = _ij0[1];
2563 vinfos[0].maxsolutions = _nj0;
2564 vinfos[1].jointtype = 1;
2565 vinfos[1].foffset = j1;
2566 vinfos[1].indices[0] = _ij1[0];
2567 vinfos[1].indices[1] = _ij1[1];
2568 vinfos[1].maxsolutions = _nj1;
2569 vinfos[2].jointtype = 1;
2570 vinfos[2].foffset = j2;
2571 vinfos[2].indices[0] = _ij2[0];
2572 vinfos[2].indices[1] = _ij2[1];
2573 vinfos[2].maxsolutions = _nj2;
2574 vinfos[3].jointtype = 1;
2575 vinfos[3].foffset = j3;
2576 vinfos[3].indices[0] = _ij3[0];
2577 vinfos[3].indices[1] = _ij3[1];
2578 vinfos[3].maxsolutions = _nj3;
2579 vinfos[4].jointtype = 1;
2580 vinfos[4].foffset = j4;
2581 vinfos[4].indices[0] = _ij4[0];
2582 vinfos[4].indices[1] = _ij4[1];
2583 vinfos[4].maxsolutions = _nj4;
2584 vinfos[5].jointtype = 1;
2585 vinfos[5].foffset = j5;
2586 vinfos[5].indices[0] = _ij5[0];
2587 vinfos[5].indices[1] = _ij5[1];
2588 vinfos[5].maxsolutions = _nj5;
2589 std::vector<int> vfree(0);
2590 solutions.AddSolution(vinfos,vfree);
2591 }
2592 }
2593 }
2594 
2595 }
2596 
2597 }
2598 
2599 } else
2600 {
2601 {
2602 IkReal j5array[1], cj5array[1], sj5array[1];
2603 bool j5valid[1]={false};
2604 _nj5 = 1;
2605 IkReal x190=cj3*cj3;
2606 IkReal x191=(cj3*new_r22);
2607 CheckValue<IkReal> x192=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x190*(new_r22*new_r22)))+x190)),-1);
2608 if(!x192.valid){
2609 continue;
2610 }
2611 CheckValue<IkReal> x193 = IKatan2WithCheck(IkReal((((new_r01*x191))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x191)))),IKFAST_ATAN2_MAGTHRESH);
2612 if(!x193.valid){
2613 continue;
2614 }
2615 j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.value)))+(x193.value));
2616 sj5array[0]=IKsin(j5array[0]);
2617 cj5array[0]=IKcos(j5array[0]);
2618 if( j5array[0] > IKPI )
2619 {
2620  j5array[0]-=IK2PI;
2621 }
2622 else if( j5array[0] < -IKPI )
2623 { j5array[0]+=IK2PI;
2624 }
2625 j5valid[0] = true;
2626 for(int ij5 = 0; ij5 < 1; ++ij5)
2627 {
2628 if( !j5valid[ij5] )
2629 {
2630  continue;
2631 }
2632 _ij5[0] = ij5; _ij5[1] = -1;
2633 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2634 {
2635 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2636 {
2637  j5valid[iij5]=false; _ij5[1] = iij5; break;
2638 }
2639 }
2640 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2641 {
2642 IkReal evalcond[10];
2643 IkReal x194=IKsin(j5);
2644 IkReal x195=IKcos(j5);
2645 IkReal x196=((1.0)*new_r11);
2646 IkReal x197=(new_r22*sj3);
2647 IkReal x198=((1.0)*new_r10);
2648 IkReal x199=((1.0)*cj3*new_r22);
2649 IkReal x200=(sj3*x194);
2650 IkReal x201=(new_r22*x194);
2651 IkReal x202=((1.0)*x195);
2652 IkReal x203=((1.0)*x194);
2653 evalcond[0]=(((new_r00*sj3))+x194+(((-1.0)*cj3*x198)));
2654 evalcond[1]=(((new_r01*sj3))+x195+(((-1.0)*cj3*x196)));
2655 evalcond[2]=(((new_r11*sj3))+x201+((cj3*new_r01)));
2656 evalcond[3]=(((sj3*x195))+((cj3*x201))+new_r01);
2657 evalcond[4]=((((-1.0)*new_r22*x202))+((new_r10*sj3))+((cj3*new_r00)));
2658 evalcond[5]=((((-1.0)*x195*x199))+x200+new_r00);
2659 evalcond[6]=(((x194*x197))+new_r11+(((-1.0)*cj3*x202)));
2660 evalcond[7]=((((-1.0)*x197*x198))+x195+(((-1.0)*new_r00*x199)));
2661 evalcond[8]=((((-1.0)*x197*x202))+new_r10+(((-1.0)*cj3*x203)));
2662 evalcond[9]=((((-1.0)*x196*x197))+(((-1.0)*x203))+(((-1.0)*new_r01*x199)));
2663 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
2664 {
2665 continue;
2666 }
2667 }
2668 
2669 {
2670 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2671 vinfos[0].jointtype = 1;
2672 vinfos[0].foffset = j0;
2673 vinfos[0].indices[0] = _ij0[0];
2674 vinfos[0].indices[1] = _ij0[1];
2675 vinfos[0].maxsolutions = _nj0;
2676 vinfos[1].jointtype = 1;
2677 vinfos[1].foffset = j1;
2678 vinfos[1].indices[0] = _ij1[0];
2679 vinfos[1].indices[1] = _ij1[1];
2680 vinfos[1].maxsolutions = _nj1;
2681 vinfos[2].jointtype = 1;
2682 vinfos[2].foffset = j2;
2683 vinfos[2].indices[0] = _ij2[0];
2684 vinfos[2].indices[1] = _ij2[1];
2685 vinfos[2].maxsolutions = _nj2;
2686 vinfos[3].jointtype = 1;
2687 vinfos[3].foffset = j3;
2688 vinfos[3].indices[0] = _ij3[0];
2689 vinfos[3].indices[1] = _ij3[1];
2690 vinfos[3].maxsolutions = _nj3;
2691 vinfos[4].jointtype = 1;
2692 vinfos[4].foffset = j4;
2693 vinfos[4].indices[0] = _ij4[0];
2694 vinfos[4].indices[1] = _ij4[1];
2695 vinfos[4].maxsolutions = _nj4;
2696 vinfos[5].jointtype = 1;
2697 vinfos[5].foffset = j5;
2698 vinfos[5].indices[0] = _ij5[0];
2699 vinfos[5].indices[1] = _ij5[1];
2700 vinfos[5].maxsolutions = _nj5;
2701 std::vector<int> vfree(0);
2702 solutions.AddSolution(vinfos,vfree);
2703 }
2704 }
2705 }
2706 
2707 }
2708 
2709 }
2710  }
2711 
2712 }
2713 
2714 }
2715 
2716 }
2717 } while(0);
2718 if( bgotonextstatement )
2719 {
2720 bool bgotonextstatement = true;
2721 do
2722 {
2723 if( 1 )
2724 {
2725 bgotonextstatement=false;
2726 continue; // branch miss [j3, j5]
2727 
2728 }
2729 } while(0);
2730 if( bgotonextstatement )
2731 {
2732 }
2733 }
2734 }
2735 }
2736 }
2737 
2738 } else
2739 {
2740 {
2741 IkReal j3array[1], cj3array[1], sj3array[1];
2742 bool j3valid[1]={false};
2743 _nj3 = 1;
2744 CheckValue<IkReal> x205=IKPowWithIntegerCheck(sj4,-1);
2745 if(!x205.valid){
2746 continue;
2747 }
2748 IkReal x204=x205.value;
2749 CheckValue<IkReal> x206=IKPowWithIntegerCheck(new_r12,-1);
2750 if(!x206.valid){
2751 continue;
2752 }
2753 if( IKabs((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x204)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x204))-1) <= IKFAST_SINCOS_THRESH )
2754  continue;
2755 j3array[0]=IKatan2((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x204));
2756 sj3array[0]=IKsin(j3array[0]);
2757 cj3array[0]=IKcos(j3array[0]);
2758 if( j3array[0] > IKPI )
2759 {
2760  j3array[0]-=IK2PI;
2761 }
2762 else if( j3array[0] < -IKPI )
2763 { j3array[0]+=IK2PI;
2764 }
2765 j3valid[0] = true;
2766 for(int ij3 = 0; ij3 < 1; ++ij3)
2767 {
2768 if( !j3valid[ij3] )
2769 {
2770  continue;
2771 }
2772 _ij3[0] = ij3; _ij3[1] = -1;
2773 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2774 {
2775 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2776 {
2777  j3valid[iij3]=false; _ij3[1] = iij3; break;
2778 }
2779 }
2780 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2781 {
2782 IkReal evalcond[8];
2783 IkReal x207=IKcos(j3);
2784 IkReal x208=IKsin(j3);
2785 IkReal x209=((1.0)*cj4);
2786 IkReal x210=(sj4*x208);
2787 IkReal x211=(new_r12*x208);
2788 IkReal x212=(sj4*x207);
2789 IkReal x213=(new_r02*x207);
2790 evalcond[0]=(x212+new_r02);
2791 evalcond[1]=(x210+new_r12);
2792 evalcond[2]=((((-1.0)*new_r02*x208))+((new_r12*x207)));
2793 evalcond[3]=(sj4+x211+x213);
2794 evalcond[4]=((((-1.0)*new_r20*x209))+((new_r00*x212))+((new_r10*x210)));
2795 evalcond[5]=((((-1.0)*new_r21*x209))+((new_r01*x212))+((new_r11*x210)));
2796 evalcond[6]=((1.0)+(((-1.0)*new_r22*x209))+((new_r02*x212))+((new_r12*x210)));
2797 evalcond[7]=((((-1.0)*new_r22*sj4))+(((-1.0)*x209*x213))+(((-1.0)*x209*x211)));
2798 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2799 {
2800 continue;
2801 }
2802 }
2803 
2804 {
2805 IkReal j5eval[3];
2806 j5eval[0]=sj4;
2807 j5eval[1]=IKsign(sj4);
2808 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2809 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2810 {
2811 {
2812 IkReal j5eval[2];
2813 j5eval[0]=sj4;
2814 j5eval[1]=sj3;
2815 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2816 {
2817 {
2818 IkReal j5eval[2];
2819 j5eval[0]=sj4;
2820 j5eval[1]=cj3;
2821 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2822 {
2823 {
2824 IkReal evalcond[5];
2825 bool bgotonextstatement = true;
2826 do
2827 {
2828 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2829 evalcond[1]=new_r02;
2830 evalcond[2]=new_r12;
2831 evalcond[3]=new_r21;
2832 evalcond[4]=new_r20;
2833 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 )
2834 {
2835 bgotonextstatement=false;
2836 {
2837 IkReal j5array[1], cj5array[1], sj5array[1];
2838 bool j5valid[1]={false};
2839 _nj5 = 1;
2840 IkReal x214=((1.0)*new_r01);
2841 if( IKabs(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x214))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x214))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2842  continue;
2843 j5array[0]=IKatan2(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x214))+((cj3*new_r00))));
2844 sj5array[0]=IKsin(j5array[0]);
2845 cj5array[0]=IKcos(j5array[0]);
2846 if( j5array[0] > IKPI )
2847 {
2848  j5array[0]-=IK2PI;
2849 }
2850 else if( j5array[0] < -IKPI )
2851 { j5array[0]+=IK2PI;
2852 }
2853 j5valid[0] = true;
2854 for(int ij5 = 0; ij5 < 1; ++ij5)
2855 {
2856 if( !j5valid[ij5] )
2857 {
2858  continue;
2859 }
2860 _ij5[0] = ij5; _ij5[1] = -1;
2861 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2862 {
2863 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2864 {
2865  j5valid[iij5]=false; _ij5[1] = iij5; break;
2866 }
2867 }
2868 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2869 {
2870 IkReal evalcond[8];
2871 IkReal x215=IKsin(j5);
2872 IkReal x216=IKcos(j5);
2873 IkReal x217=((1.0)*cj3);
2874 IkReal x218=(sj3*x215);
2875 IkReal x219=((1.0)*x216);
2876 IkReal x220=(x216*x217);
2877 evalcond[0]=(((new_r11*sj3))+x215+((cj3*new_r01)));
2878 evalcond[1]=(((new_r00*sj3))+(((-1.0)*new_r10*x217))+x215);
2879 evalcond[2]=((((-1.0)*new_r11*x217))+((new_r01*sj3))+x216);
2880 evalcond[3]=(((sj3*x216))+((cj3*x215))+new_r01);
2881 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x219)));
2882 evalcond[5]=(x218+new_r00+(((-1.0)*x220)));
2883 evalcond[6]=(x218+new_r11+(((-1.0)*x220)));
2884 evalcond[7]=((((-1.0)*sj3*x219))+new_r10+(((-1.0)*x215*x217)));
2885 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2886 {
2887 continue;
2888 }
2889 }
2890 
2891 {
2892 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2893 vinfos[0].jointtype = 1;
2894 vinfos[0].foffset = j0;
2895 vinfos[0].indices[0] = _ij0[0];
2896 vinfos[0].indices[1] = _ij0[1];
2897 vinfos[0].maxsolutions = _nj0;
2898 vinfos[1].jointtype = 1;
2899 vinfos[1].foffset = j1;
2900 vinfos[1].indices[0] = _ij1[0];
2901 vinfos[1].indices[1] = _ij1[1];
2902 vinfos[1].maxsolutions = _nj1;
2903 vinfos[2].jointtype = 1;
2904 vinfos[2].foffset = j2;
2905 vinfos[2].indices[0] = _ij2[0];
2906 vinfos[2].indices[1] = _ij2[1];
2907 vinfos[2].maxsolutions = _nj2;
2908 vinfos[3].jointtype = 1;
2909 vinfos[3].foffset = j3;
2910 vinfos[3].indices[0] = _ij3[0];
2911 vinfos[3].indices[1] = _ij3[1];
2912 vinfos[3].maxsolutions = _nj3;
2913 vinfos[4].jointtype = 1;
2914 vinfos[4].foffset = j4;
2915 vinfos[4].indices[0] = _ij4[0];
2916 vinfos[4].indices[1] = _ij4[1];
2917 vinfos[4].maxsolutions = _nj4;
2918 vinfos[5].jointtype = 1;
2919 vinfos[5].foffset = j5;
2920 vinfos[5].indices[0] = _ij5[0];
2921 vinfos[5].indices[1] = _ij5[1];
2922 vinfos[5].maxsolutions = _nj5;
2923 std::vector<int> vfree(0);
2924 solutions.AddSolution(vinfos,vfree);
2925 }
2926 }
2927 }
2928 
2929 }
2930 } while(0);
2931 if( bgotonextstatement )
2932 {
2933 bool bgotonextstatement = true;
2934 do
2935 {
2936 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2937 evalcond[1]=new_r02;
2938 evalcond[2]=new_r12;
2939 evalcond[3]=new_r21;
2940 evalcond[4]=new_r20;
2941 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 )
2942 {
2943 bgotonextstatement=false;
2944 {
2945 IkReal j5array[1], cj5array[1], sj5array[1];
2946 bool j5valid[1]={false};
2947 _nj5 = 1;
2948 IkReal x221=((1.0)*sj3);
2949 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x221)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x221))))+IKsqr(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2950  continue;
2951 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x221))), ((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))));
2952 sj5array[0]=IKsin(j5array[0]);
2953 cj5array[0]=IKcos(j5array[0]);
2954 if( j5array[0] > IKPI )
2955 {
2956  j5array[0]-=IK2PI;
2957 }
2958 else if( j5array[0] < -IKPI )
2959 { j5array[0]+=IK2PI;
2960 }
2961 j5valid[0] = true;
2962 for(int ij5 = 0; ij5 < 1; ++ij5)
2963 {
2964 if( !j5valid[ij5] )
2965 {
2966  continue;
2967 }
2968 _ij5[0] = ij5; _ij5[1] = -1;
2969 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2970 {
2971 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2972 {
2973  j5valid[iij5]=false; _ij5[1] = iij5; break;
2974 }
2975 }
2976 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2977 {
2978 IkReal evalcond[8];
2979 IkReal x222=IKcos(j5);
2980 IkReal x223=IKsin(j5);
2981 IkReal x224=((1.0)*cj3);
2982 IkReal x225=(sj3*x222);
2983 IkReal x226=((1.0)*x223);
2984 IkReal x227=(x223*x224);
2985 evalcond[0]=(((new_r10*sj3))+x222+((cj3*new_r00)));
2986 evalcond[1]=((((-1.0)*new_r10*x224))+((new_r00*sj3))+x223);
2987 evalcond[2]=((((-1.0)*new_r11*x224))+((new_r01*sj3))+x222);
2988 evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x226)));
2989 evalcond[4]=(((cj3*x222))+((sj3*x223))+new_r00);
2990 evalcond[5]=(x225+new_r01+(((-1.0)*x227)));
2991 evalcond[6]=(x225+new_r10+(((-1.0)*x227)));
2992 evalcond[7]=((((-1.0)*x222*x224))+(((-1.0)*sj3*x226))+new_r11);
2993 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2994 {
2995 continue;
2996 }
2997 }
2998 
2999 {
3000 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3001 vinfos[0].jointtype = 1;
3002 vinfos[0].foffset = j0;
3003 vinfos[0].indices[0] = _ij0[0];
3004 vinfos[0].indices[1] = _ij0[1];
3005 vinfos[0].maxsolutions = _nj0;
3006 vinfos[1].jointtype = 1;
3007 vinfos[1].foffset = j1;
3008 vinfos[1].indices[0] = _ij1[0];
3009 vinfos[1].indices[1] = _ij1[1];
3010 vinfos[1].maxsolutions = _nj1;
3011 vinfos[2].jointtype = 1;
3012 vinfos[2].foffset = j2;
3013 vinfos[2].indices[0] = _ij2[0];
3014 vinfos[2].indices[1] = _ij2[1];
3015 vinfos[2].maxsolutions = _nj2;
3016 vinfos[3].jointtype = 1;
3017 vinfos[3].foffset = j3;
3018 vinfos[3].indices[0] = _ij3[0];
3019 vinfos[3].indices[1] = _ij3[1];
3020 vinfos[3].maxsolutions = _nj3;
3021 vinfos[4].jointtype = 1;
3022 vinfos[4].foffset = j4;
3023 vinfos[4].indices[0] = _ij4[0];
3024 vinfos[4].indices[1] = _ij4[1];
3025 vinfos[4].maxsolutions = _nj4;
3026 vinfos[5].jointtype = 1;
3027 vinfos[5].foffset = j5;
3028 vinfos[5].indices[0] = _ij5[0];
3029 vinfos[5].indices[1] = _ij5[1];
3030 vinfos[5].maxsolutions = _nj5;
3031 std::vector<int> vfree(0);
3032 solutions.AddSolution(vinfos,vfree);
3033 }
3034 }
3035 }
3036 
3037 }
3038 } while(0);
3039 if( bgotonextstatement )
3040 {
3041 bool bgotonextstatement = true;
3042 do
3043 {
3044 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3045 evalcond[1]=new_r02;
3046 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3047 {
3048 bgotonextstatement=false;
3049 {
3050 IkReal j5array[1], cj5array[1], sj5array[1];
3051 bool j5valid[1]={false};
3052 _nj5 = 1;
3053 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 )
3054  continue;
3055 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3056 sj5array[0]=IKsin(j5array[0]);
3057 cj5array[0]=IKcos(j5array[0]);
3058 if( j5array[0] > IKPI )
3059 {
3060  j5array[0]-=IK2PI;
3061 }
3062 else if( j5array[0] < -IKPI )
3063 { j5array[0]+=IK2PI;
3064 }
3065 j5valid[0] = true;
3066 for(int ij5 = 0; ij5 < 1; ++ij5)
3067 {
3068 if( !j5valid[ij5] )
3069 {
3070  continue;
3071 }
3072 _ij5[0] = ij5; _ij5[1] = -1;
3073 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3074 {
3075 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3076 {
3077  j5valid[iij5]=false; _ij5[1] = iij5; break;
3078 }
3079 }
3080 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3081 {
3082 IkReal evalcond[8];
3083 IkReal x228=IKsin(j5);
3084 IkReal x229=IKcos(j5);
3085 IkReal x230=((1.0)*cj4);
3086 IkReal x231=((1.0)*sj4);
3087 evalcond[0]=(x228+new_r00);
3088 evalcond[1]=(x229+new_r01);
3089 evalcond[2]=(((sj4*x228))+new_r21);
3090 evalcond[3]=(((cj4*x228))+new_r11);
3091 evalcond[4]=(new_r20+(((-1.0)*x229*x231)));
3092 evalcond[5]=(new_r10+(((-1.0)*x229*x230)));
3093 evalcond[6]=((((-1.0)*new_r20*x231))+x229+(((-1.0)*new_r10*x230)));
3094 evalcond[7]=((((-1.0)*new_r21*x231))+(((-1.0)*new_r11*x230))+(((-1.0)*x228)));
3095 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3096 {
3097 continue;
3098 }
3099 }
3100 
3101 {
3102 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3103 vinfos[0].jointtype = 1;
3104 vinfos[0].foffset = j0;
3105 vinfos[0].indices[0] = _ij0[0];
3106 vinfos[0].indices[1] = _ij0[1];
3107 vinfos[0].maxsolutions = _nj0;
3108 vinfos[1].jointtype = 1;
3109 vinfos[1].foffset = j1;
3110 vinfos[1].indices[0] = _ij1[0];
3111 vinfos[1].indices[1] = _ij1[1];
3112 vinfos[1].maxsolutions = _nj1;
3113 vinfos[2].jointtype = 1;
3114 vinfos[2].foffset = j2;
3115 vinfos[2].indices[0] = _ij2[0];
3116 vinfos[2].indices[1] = _ij2[1];
3117 vinfos[2].maxsolutions = _nj2;
3118 vinfos[3].jointtype = 1;
3119 vinfos[3].foffset = j3;
3120 vinfos[3].indices[0] = _ij3[0];
3121 vinfos[3].indices[1] = _ij3[1];
3122 vinfos[3].maxsolutions = _nj3;
3123 vinfos[4].jointtype = 1;
3124 vinfos[4].foffset = j4;
3125 vinfos[4].indices[0] = _ij4[0];
3126 vinfos[4].indices[1] = _ij4[1];
3127 vinfos[4].maxsolutions = _nj4;
3128 vinfos[5].jointtype = 1;
3129 vinfos[5].foffset = j5;
3130 vinfos[5].indices[0] = _ij5[0];
3131 vinfos[5].indices[1] = _ij5[1];
3132 vinfos[5].maxsolutions = _nj5;
3133 std::vector<int> vfree(0);
3134 solutions.AddSolution(vinfos,vfree);
3135 }
3136 }
3137 }
3138 
3139 }
3140 } while(0);
3141 if( bgotonextstatement )
3142 {
3143 bool bgotonextstatement = true;
3144 do
3145 {
3146 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3147 evalcond[1]=new_r02;
3148 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3149 {
3150 bgotonextstatement=false;
3151 {
3152 IkReal j5array[1], cj5array[1], sj5array[1];
3153 bool j5valid[1]={false};
3154 _nj5 = 1;
3155 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3156  continue;
3157 j5array[0]=IKatan2(new_r00, new_r01);
3158 sj5array[0]=IKsin(j5array[0]);
3159 cj5array[0]=IKcos(j5array[0]);
3160 if( j5array[0] > IKPI )
3161 {
3162  j5array[0]-=IK2PI;
3163 }
3164 else if( j5array[0] < -IKPI )
3165 { j5array[0]+=IK2PI;
3166 }
3167 j5valid[0] = true;
3168 for(int ij5 = 0; ij5 < 1; ++ij5)
3169 {
3170 if( !j5valid[ij5] )
3171 {
3172  continue;
3173 }
3174 _ij5[0] = ij5; _ij5[1] = -1;
3175 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3176 {
3177 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3178 {
3179  j5valid[iij5]=false; _ij5[1] = iij5; break;
3180 }
3181 }
3182 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3183 {
3184 IkReal evalcond[8];
3185 IkReal x232=IKsin(j5);
3186 IkReal x233=IKcos(j5);
3187 IkReal x234=((1.0)*sj4);
3188 IkReal x235=((1.0)*x233);
3189 evalcond[0]=(((sj4*x232))+new_r21);
3190 evalcond[1]=(x232+(((-1.0)*new_r00)));
3191 evalcond[2]=(x233+(((-1.0)*new_r01)));
3192 evalcond[3]=((((-1.0)*x233*x234))+new_r20);
3193 evalcond[4]=(((cj4*x232))+(((-1.0)*new_r11)));
3194 evalcond[5]=((((-1.0)*cj4*x235))+(((-1.0)*new_r10)));
3195 evalcond[6]=((((-1.0)*new_r20*x234))+((cj4*new_r10))+x233);
3196 evalcond[7]=((((-1.0)*new_r21*x234))+((cj4*new_r11))+(((-1.0)*x232)));
3197 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3198 {
3199 continue;
3200 }
3201 }
3202 
3203 {
3204 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3205 vinfos[0].jointtype = 1;
3206 vinfos[0].foffset = j0;
3207 vinfos[0].indices[0] = _ij0[0];
3208 vinfos[0].indices[1] = _ij0[1];
3209 vinfos[0].maxsolutions = _nj0;
3210 vinfos[1].jointtype = 1;
3211 vinfos[1].foffset = j1;
3212 vinfos[1].indices[0] = _ij1[0];
3213 vinfos[1].indices[1] = _ij1[1];
3214 vinfos[1].maxsolutions = _nj1;
3215 vinfos[2].jointtype = 1;
3216 vinfos[2].foffset = j2;
3217 vinfos[2].indices[0] = _ij2[0];
3218 vinfos[2].indices[1] = _ij2[1];
3219 vinfos[2].maxsolutions = _nj2;
3220 vinfos[3].jointtype = 1;
3221 vinfos[3].foffset = j3;
3222 vinfos[3].indices[0] = _ij3[0];
3223 vinfos[3].indices[1] = _ij3[1];
3224 vinfos[3].maxsolutions = _nj3;
3225 vinfos[4].jointtype = 1;
3226 vinfos[4].foffset = j4;
3227 vinfos[4].indices[0] = _ij4[0];
3228 vinfos[4].indices[1] = _ij4[1];
3229 vinfos[4].maxsolutions = _nj4;
3230 vinfos[5].jointtype = 1;
3231 vinfos[5].foffset = j5;
3232 vinfos[5].indices[0] = _ij5[0];
3233 vinfos[5].indices[1] = _ij5[1];
3234 vinfos[5].maxsolutions = _nj5;
3235 std::vector<int> vfree(0);
3236 solutions.AddSolution(vinfos,vfree);
3237 }
3238 }
3239 }
3240 
3241 }
3242 } while(0);
3243 if( bgotonextstatement )
3244 {
3245 bool bgotonextstatement = true;
3246 do
3247 {
3248 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3249 evalcond[1]=new_r12;
3250 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3251 {
3252 bgotonextstatement=false;
3253 {
3254 IkReal j5array[1], cj5array[1], sj5array[1];
3255 bool j5valid[1]={false};
3256 _nj5 = 1;
3257 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3258  continue;
3259 j5array[0]=IKatan2(new_r10, new_r11);
3260 sj5array[0]=IKsin(j5array[0]);
3261 cj5array[0]=IKcos(j5array[0]);
3262 if( j5array[0] > IKPI )
3263 {
3264  j5array[0]-=IK2PI;
3265 }
3266 else if( j5array[0] < -IKPI )
3267 { j5array[0]+=IK2PI;
3268 }
3269 j5valid[0] = true;
3270 for(int ij5 = 0; ij5 < 1; ++ij5)
3271 {
3272 if( !j5valid[ij5] )
3273 {
3274  continue;
3275 }
3276 _ij5[0] = ij5; _ij5[1] = -1;
3277 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3278 {
3279 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3280 {
3281  j5valid[iij5]=false; _ij5[1] = iij5; break;
3282 }
3283 }
3284 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3285 {
3286 IkReal evalcond[8];
3287 IkReal x236=IKcos(j5);
3288 IkReal x237=IKsin(j5);
3289 IkReal x238=((1.0)*cj4);
3290 IkReal x239=((1.0)*sj4);
3291 IkReal x240=((1.0)*x237);
3292 evalcond[0]=(((new_r02*x236))+new_r20);
3293 evalcond[1]=(x237+(((-1.0)*new_r10)));
3294 evalcond[2]=(x236+(((-1.0)*new_r11)));
3295 evalcond[3]=(((cj4*x237))+new_r01);
3296 evalcond[4]=((((-1.0)*new_r02*x240))+new_r21);
3297 evalcond[5]=((((-1.0)*x236*x238))+new_r00);
3298 evalcond[6]=((((-1.0)*new_r20*x239))+x236+(((-1.0)*new_r00*x238)));
3299 evalcond[7]=((((-1.0)*new_r21*x239))+(((-1.0)*x240))+(((-1.0)*new_r01*x238)));
3300 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3301 {
3302 continue;
3303 }
3304 }
3305 
3306 {
3307 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3308 vinfos[0].jointtype = 1;
3309 vinfos[0].foffset = j0;
3310 vinfos[0].indices[0] = _ij0[0];
3311 vinfos[0].indices[1] = _ij0[1];
3312 vinfos[0].maxsolutions = _nj0;
3313 vinfos[1].jointtype = 1;
3314 vinfos[1].foffset = j1;
3315 vinfos[1].indices[0] = _ij1[0];
3316 vinfos[1].indices[1] = _ij1[1];
3317 vinfos[1].maxsolutions = _nj1;
3318 vinfos[2].jointtype = 1;
3319 vinfos[2].foffset = j2;
3320 vinfos[2].indices[0] = _ij2[0];
3321 vinfos[2].indices[1] = _ij2[1];
3322 vinfos[2].maxsolutions = _nj2;
3323 vinfos[3].jointtype = 1;
3324 vinfos[3].foffset = j3;
3325 vinfos[3].indices[0] = _ij3[0];
3326 vinfos[3].indices[1] = _ij3[1];
3327 vinfos[3].maxsolutions = _nj3;
3328 vinfos[4].jointtype = 1;
3329 vinfos[4].foffset = j4;
3330 vinfos[4].indices[0] = _ij4[0];
3331 vinfos[4].indices[1] = _ij4[1];
3332 vinfos[4].maxsolutions = _nj4;
3333 vinfos[5].jointtype = 1;
3334 vinfos[5].foffset = j5;
3335 vinfos[5].indices[0] = _ij5[0];
3336 vinfos[5].indices[1] = _ij5[1];
3337 vinfos[5].maxsolutions = _nj5;
3338 std::vector<int> vfree(0);
3339 solutions.AddSolution(vinfos,vfree);
3340 }
3341 }
3342 }
3343 
3344 }
3345 } while(0);
3346 if( bgotonextstatement )
3347 {
3348 bool bgotonextstatement = true;
3349 do
3350 {
3351 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3352 evalcond[1]=new_r12;
3353 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3354 {
3355 bgotonextstatement=false;
3356 {
3357 IkReal j5array[1], cj5array[1], sj5array[1];
3358 bool j5valid[1]={false};
3359 _nj5 = 1;
3360 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 )
3361  continue;
3362 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3363 sj5array[0]=IKsin(j5array[0]);
3364 cj5array[0]=IKcos(j5array[0]);
3365 if( j5array[0] > IKPI )
3366 {
3367  j5array[0]-=IK2PI;
3368 }
3369 else if( j5array[0] < -IKPI )
3370 { j5array[0]+=IK2PI;
3371 }
3372 j5valid[0] = true;
3373 for(int ij5 = 0; ij5 < 1; ++ij5)
3374 {
3375 if( !j5valid[ij5] )
3376 {
3377  continue;
3378 }
3379 _ij5[0] = ij5; _ij5[1] = -1;
3380 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3381 {
3382 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3383 {
3384  j5valid[iij5]=false; _ij5[1] = iij5; break;
3385 }
3386 }
3387 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3388 {
3389 IkReal evalcond[8];
3390 IkReal x241=IKsin(j5);
3391 IkReal x242=IKcos(j5);
3392 IkReal x243=((1.0)*sj4);
3393 IkReal x244=((1.0)*x242);
3394 evalcond[0]=(x241+new_r10);
3395 evalcond[1]=(x242+new_r11);
3396 evalcond[2]=(new_r21+((new_r02*x241)));
3397 evalcond[3]=((((-1.0)*new_r02*x244))+new_r20);
3398 evalcond[4]=(((cj4*x241))+(((-1.0)*new_r01)));
3399 evalcond[5]=((((-1.0)*cj4*x244))+(((-1.0)*new_r00)));
3400 evalcond[6]=(((cj4*new_r00))+x242+(((-1.0)*new_r20*x243)));
3401 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x241))+(((-1.0)*new_r21*x243)));
3402 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3403 {
3404 continue;
3405 }
3406 }
3407 
3408 {
3409 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3410 vinfos[0].jointtype = 1;
3411 vinfos[0].foffset = j0;
3412 vinfos[0].indices[0] = _ij0[0];
3413 vinfos[0].indices[1] = _ij0[1];
3414 vinfos[0].maxsolutions = _nj0;
3415 vinfos[1].jointtype = 1;
3416 vinfos[1].foffset = j1;
3417 vinfos[1].indices[0] = _ij1[0];
3418 vinfos[1].indices[1] = _ij1[1];
3419 vinfos[1].maxsolutions = _nj1;
3420 vinfos[2].jointtype = 1;
3421 vinfos[2].foffset = j2;
3422 vinfos[2].indices[0] = _ij2[0];
3423 vinfos[2].indices[1] = _ij2[1];
3424 vinfos[2].maxsolutions = _nj2;
3425 vinfos[3].jointtype = 1;
3426 vinfos[3].foffset = j3;
3427 vinfos[3].indices[0] = _ij3[0];
3428 vinfos[3].indices[1] = _ij3[1];
3429 vinfos[3].maxsolutions = _nj3;
3430 vinfos[4].jointtype = 1;
3431 vinfos[4].foffset = j4;
3432 vinfos[4].indices[0] = _ij4[0];
3433 vinfos[4].indices[1] = _ij4[1];
3434 vinfos[4].maxsolutions = _nj4;
3435 vinfos[5].jointtype = 1;
3436 vinfos[5].foffset = j5;
3437 vinfos[5].indices[0] = _ij5[0];
3438 vinfos[5].indices[1] = _ij5[1];
3439 vinfos[5].maxsolutions = _nj5;
3440 std::vector<int> vfree(0);
3441 solutions.AddSolution(vinfos,vfree);
3442 }
3443 }
3444 }
3445 
3446 }
3447 } while(0);
3448 if( bgotonextstatement )
3449 {
3450 bool bgotonextstatement = true;
3451 do
3452 {
3453 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3454 if( IKabs(evalcond[0]) < 0.0000050000000000 )
3455 {
3456 bgotonextstatement=false;
3457 {
3458 IkReal j5eval[1];
3459 new_r21=0;
3460 new_r20=0;
3461 new_r02=0;
3462 new_r12=0;
3463 j5eval[0]=1.0;
3464 if( IKabs(j5eval[0]) < 0.0000000100000000 )
3465 {
3466 continue; // no branches [j5]
3467 
3468 } else
3469 {
3470 IkReal op[2+1], zeror[2];
3471 int numroots;
3472 op[0]=-1.0;
3473 op[1]=0;
3474 op[2]=1.0;
3475 polyroots2(op,zeror,numroots);
3476 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3477 int numsolutions = 0;
3478 for(int ij5 = 0; ij5 < numroots; ++ij5)
3479 {
3480 IkReal htj5 = zeror[ij5];
3481 tempj5array[0]=((2.0)*(atan(htj5)));
3482 for(int kj5 = 0; kj5 < 1; ++kj5)
3483 {
3484 j5array[numsolutions] = tempj5array[kj5];
3485 if( j5array[numsolutions] > IKPI )
3486 {
3487  j5array[numsolutions]-=IK2PI;
3488 }
3489 else if( j5array[numsolutions] < -IKPI )
3490 {
3491  j5array[numsolutions]+=IK2PI;
3492 }
3493 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3494 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3495 numsolutions++;
3496 }
3497 }
3498 bool j5valid[2]={true,true};
3499 _nj5 = 2;
3500 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3501  {
3502 if( !j5valid[ij5] )
3503 {
3504  continue;
3505 }
3506  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3507 htj5 = IKtan(j5/2);
3508 
3509 _ij5[0] = ij5; _ij5[1] = -1;
3510 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3511 {
3512 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3513 {
3514  j5valid[iij5]=false; _ij5[1] = iij5; break;
3515 }
3516 }
3517 {
3518 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3519 vinfos[0].jointtype = 1;
3520 vinfos[0].foffset = j0;
3521 vinfos[0].indices[0] = _ij0[0];
3522 vinfos[0].indices[1] = _ij0[1];
3523 vinfos[0].maxsolutions = _nj0;
3524 vinfos[1].jointtype = 1;
3525 vinfos[1].foffset = j1;
3526 vinfos[1].indices[0] = _ij1[0];
3527 vinfos[1].indices[1] = _ij1[1];
3528 vinfos[1].maxsolutions = _nj1;
3529 vinfos[2].jointtype = 1;
3530 vinfos[2].foffset = j2;
3531 vinfos[2].indices[0] = _ij2[0];
3532 vinfos[2].indices[1] = _ij2[1];
3533 vinfos[2].maxsolutions = _nj2;
3534 vinfos[3].jointtype = 1;
3535 vinfos[3].foffset = j3;
3536 vinfos[3].indices[0] = _ij3[0];
3537 vinfos[3].indices[1] = _ij3[1];
3538 vinfos[3].maxsolutions = _nj3;
3539 vinfos[4].jointtype = 1;
3540 vinfos[4].foffset = j4;
3541 vinfos[4].indices[0] = _ij4[0];
3542 vinfos[4].indices[1] = _ij4[1];
3543 vinfos[4].maxsolutions = _nj4;
3544 vinfos[5].jointtype = 1;
3545 vinfos[5].foffset = j5;
3546 vinfos[5].indices[0] = _ij5[0];
3547 vinfos[5].indices[1] = _ij5[1];
3548 vinfos[5].maxsolutions = _nj5;
3549 std::vector<int> vfree(0);
3550 solutions.AddSolution(vinfos,vfree);
3551 }
3552  }
3553 
3554 }
3555 
3556 }
3557 
3558 }
3559 } while(0);
3560 if( bgotonextstatement )
3561 {
3562 bool bgotonextstatement = true;
3563 do
3564 {
3565 if( 1 )
3566 {
3567 bgotonextstatement=false;
3568 continue; // branch miss [j5]
3569 
3570 }
3571 } while(0);
3572 if( bgotonextstatement )
3573 {
3574 }
3575 }
3576 }
3577 }
3578 }
3579 }
3580 }
3581 }
3582 }
3583 
3584 } else
3585 {
3586 {
3587 IkReal j5array[1], cj5array[1], sj5array[1];
3588 bool j5valid[1]={false};
3589 _nj5 = 1;
3590 CheckValue<IkReal> x246=IKPowWithIntegerCheck(sj4,-1);
3591 if(!x246.valid){
3592 continue;
3593 }
3594 IkReal x245=x246.value;
3595 CheckValue<IkReal> x247=IKPowWithIntegerCheck(cj3,-1);
3596 if(!x247.valid){
3597 continue;
3598 }
3599 if( IKabs(((-1.0)*new_r21*x245)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x245))+IKsqr((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3600  continue;
3601 j5array[0]=IKatan2(((-1.0)*new_r21*x245), (x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3602 sj5array[0]=IKsin(j5array[0]);
3603 cj5array[0]=IKcos(j5array[0]);
3604 if( j5array[0] > IKPI )
3605 {
3606  j5array[0]-=IK2PI;
3607 }
3608 else if( j5array[0] < -IKPI )
3609 { j5array[0]+=IK2PI;
3610 }
3611 j5valid[0] = true;
3612 for(int ij5 = 0; ij5 < 1; ++ij5)
3613 {
3614 if( !j5valid[ij5] )
3615 {
3616  continue;
3617 }
3618 _ij5[0] = ij5; _ij5[1] = -1;
3619 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3620 {
3621 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3622 {
3623  j5valid[iij5]=false; _ij5[1] = iij5; break;
3624 }
3625 }
3626 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3627 {
3628 IkReal evalcond[12];
3629 IkReal x248=IKsin(j5);
3630 IkReal x249=IKcos(j5);
3631 IkReal x250=(cj3*new_r00);
3632 IkReal x251=(cj3*cj4);
3633 IkReal x252=((1.0)*cj3);
3634 IkReal x253=(new_r11*sj3);
3635 IkReal x254=((1.0)*cj4);
3636 IkReal x255=(new_r10*sj3);
3637 IkReal x256=((1.0)*sj4);
3638 IkReal x257=(sj3*x248);
3639 IkReal x258=((1.0)*x249);
3640 IkReal x259=(sj3*x249);
3641 evalcond[0]=(new_r21+((sj4*x248)));
3642 evalcond[1]=((((-1.0)*x249*x256))+new_r20);
3643 evalcond[2]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x248);
3644 evalcond[3]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x249);
3645 evalcond[4]=(((cj4*x248))+x253+((cj3*new_r01)));
3646 evalcond[5]=(((x248*x251))+x259+new_r01);
3647 evalcond[6]=(x255+x250+(((-1.0)*x249*x254)));
3648 evalcond[7]=((((-1.0)*x251*x258))+x257+new_r00);
3649 evalcond[8]=((((-1.0)*x249*x252))+new_r11+((cj4*x257)));
3650 evalcond[9]=((((-1.0)*x254*x259))+(((-1.0)*x248*x252))+new_r10);
3651 evalcond[10]=((((-1.0)*x250*x254))+(((-1.0)*x254*x255))+x249+(((-1.0)*new_r20*x256)));
3652 evalcond[11]=((((-1.0)*new_r21*x256))+(((-1.0)*x248))+(((-1.0)*x253*x254))+(((-1.0)*new_r01*x251)));
3653 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3654 {
3655 continue;
3656 }
3657 }
3658 
3659 {
3660 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3661 vinfos[0].jointtype = 1;
3662 vinfos[0].foffset = j0;
3663 vinfos[0].indices[0] = _ij0[0];
3664 vinfos[0].indices[1] = _ij0[1];
3665 vinfos[0].maxsolutions = _nj0;
3666 vinfos[1].jointtype = 1;
3667 vinfos[1].foffset = j1;
3668 vinfos[1].indices[0] = _ij1[0];
3669 vinfos[1].indices[1] = _ij1[1];
3670 vinfos[1].maxsolutions = _nj1;
3671 vinfos[2].jointtype = 1;
3672 vinfos[2].foffset = j2;
3673 vinfos[2].indices[0] = _ij2[0];
3674 vinfos[2].indices[1] = _ij2[1];
3675 vinfos[2].maxsolutions = _nj2;
3676 vinfos[3].jointtype = 1;
3677 vinfos[3].foffset = j3;
3678 vinfos[3].indices[0] = _ij3[0];
3679 vinfos[3].indices[1] = _ij3[1];
3680 vinfos[3].maxsolutions = _nj3;
3681 vinfos[4].jointtype = 1;
3682 vinfos[4].foffset = j4;
3683 vinfos[4].indices[0] = _ij4[0];
3684 vinfos[4].indices[1] = _ij4[1];
3685 vinfos[4].maxsolutions = _nj4;
3686 vinfos[5].jointtype = 1;
3687 vinfos[5].foffset = j5;
3688 vinfos[5].indices[0] = _ij5[0];
3689 vinfos[5].indices[1] = _ij5[1];
3690 vinfos[5].maxsolutions = _nj5;
3691 std::vector<int> vfree(0);
3692 solutions.AddSolution(vinfos,vfree);
3693 }
3694 }
3695 }
3696 
3697 }
3698 
3699 }
3700 
3701 } else
3702 {
3703 {
3704 IkReal j5array[1], cj5array[1], sj5array[1];
3705 bool j5valid[1]={false};
3706 _nj5 = 1;
3707 CheckValue<IkReal> x261=IKPowWithIntegerCheck(sj4,-1);
3708 if(!x261.valid){
3709 continue;
3710 }
3711 IkReal x260=x261.value;
3712 CheckValue<IkReal> x262=IKPowWithIntegerCheck(sj3,-1);
3713 if(!x262.valid){
3714 continue;
3715 }
3716 if( IKabs(((-1.0)*new_r21*x260)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x260))+IKsqr((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3717  continue;
3718 j5array[0]=IKatan2(((-1.0)*new_r21*x260), (x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3719 sj5array[0]=IKsin(j5array[0]);
3720 cj5array[0]=IKcos(j5array[0]);
3721 if( j5array[0] > IKPI )
3722 {
3723  j5array[0]-=IK2PI;
3724 }
3725 else if( j5array[0] < -IKPI )
3726 { j5array[0]+=IK2PI;
3727 }
3728 j5valid[0] = true;
3729 for(int ij5 = 0; ij5 < 1; ++ij5)
3730 {
3731 if( !j5valid[ij5] )
3732 {
3733  continue;
3734 }
3735 _ij5[0] = ij5; _ij5[1] = -1;
3736 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3737 {
3738 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3739 {
3740  j5valid[iij5]=false; _ij5[1] = iij5; break;
3741 }
3742 }
3743 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3744 {
3745 IkReal evalcond[12];
3746 IkReal x263=IKsin(j5);
3747 IkReal x264=IKcos(j5);
3748 IkReal x265=(cj3*new_r00);
3749 IkReal x266=(cj3*cj4);
3750 IkReal x267=((1.0)*cj3);
3751 IkReal x268=(new_r11*sj3);
3752 IkReal x269=((1.0)*cj4);
3753 IkReal x270=(new_r10*sj3);
3754 IkReal x271=((1.0)*sj4);
3755 IkReal x272=(sj3*x263);
3756 IkReal x273=((1.0)*x264);
3757 IkReal x274=(sj3*x264);
3758 evalcond[0]=(new_r21+((sj4*x263)));
3759 evalcond[1]=(new_r20+(((-1.0)*x264*x271)));
3760 evalcond[2]=(((new_r00*sj3))+x263+(((-1.0)*new_r10*x267)));
3761 evalcond[3]=(((new_r01*sj3))+x264+(((-1.0)*new_r11*x267)));
3762 evalcond[4]=(((cj4*x263))+x268+((cj3*new_r01)));
3763 evalcond[5]=(((x263*x266))+x274+new_r01);
3764 evalcond[6]=(x265+x270+(((-1.0)*x264*x269)));
3765 evalcond[7]=(x272+(((-1.0)*x266*x273))+new_r00);
3766 evalcond[8]=(((cj4*x272))+new_r11+(((-1.0)*x264*x267)));
3767 evalcond[9]=((((-1.0)*x263*x267))+(((-1.0)*x269*x274))+new_r10);
3768 evalcond[10]=((((-1.0)*x269*x270))+x264+(((-1.0)*new_r20*x271))+(((-1.0)*x265*x269)));
3769 evalcond[11]=((((-1.0)*x263))+(((-1.0)*new_r01*x266))+(((-1.0)*new_r21*x271))+(((-1.0)*x268*x269)));
3770 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3771 {
3772 continue;
3773 }
3774 }
3775 
3776 {
3777 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3778 vinfos[0].jointtype = 1;
3779 vinfos[0].foffset = j0;
3780 vinfos[0].indices[0] = _ij0[0];
3781 vinfos[0].indices[1] = _ij0[1];
3782 vinfos[0].maxsolutions = _nj0;
3783 vinfos[1].jointtype = 1;
3784 vinfos[1].foffset = j1;
3785 vinfos[1].indices[0] = _ij1[0];
3786 vinfos[1].indices[1] = _ij1[1];
3787 vinfos[1].maxsolutions = _nj1;
3788 vinfos[2].jointtype = 1;
3789 vinfos[2].foffset = j2;
3790 vinfos[2].indices[0] = _ij2[0];
3791 vinfos[2].indices[1] = _ij2[1];
3792 vinfos[2].maxsolutions = _nj2;
3793 vinfos[3].jointtype = 1;
3794 vinfos[3].foffset = j3;
3795 vinfos[3].indices[0] = _ij3[0];
3796 vinfos[3].indices[1] = _ij3[1];
3797 vinfos[3].maxsolutions = _nj3;
3798 vinfos[4].jointtype = 1;
3799 vinfos[4].foffset = j4;
3800 vinfos[4].indices[0] = _ij4[0];
3801 vinfos[4].indices[1] = _ij4[1];
3802 vinfos[4].maxsolutions = _nj4;
3803 vinfos[5].jointtype = 1;
3804 vinfos[5].foffset = j5;
3805 vinfos[5].indices[0] = _ij5[0];
3806 vinfos[5].indices[1] = _ij5[1];
3807 vinfos[5].maxsolutions = _nj5;
3808 std::vector<int> vfree(0);
3809 solutions.AddSolution(vinfos,vfree);
3810 }
3811 }
3812 }
3813 
3814 }
3815 
3816 }
3817 
3818 } else
3819 {
3820 {
3821 IkReal j5array[1], cj5array[1], sj5array[1];
3822 bool j5valid[1]={false};
3823 _nj5 = 1;
3824 CheckValue<IkReal> x275 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3825 if(!x275.valid){
3826 continue;
3827 }
3828 CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(sj4),-1);
3829 if(!x276.valid){
3830 continue;
3831 }
3832 j5array[0]=((-1.5707963267949)+(x275.value)+(((1.5707963267949)*(x276.value))));
3833 sj5array[0]=IKsin(j5array[0]);
3834 cj5array[0]=IKcos(j5array[0]);
3835 if( j5array[0] > IKPI )
3836 {
3837  j5array[0]-=IK2PI;
3838 }
3839 else if( j5array[0] < -IKPI )
3840 { j5array[0]+=IK2PI;
3841 }
3842 j5valid[0] = true;
3843 for(int ij5 = 0; ij5 < 1; ++ij5)
3844 {
3845 if( !j5valid[ij5] )
3846 {
3847  continue;
3848 }
3849 _ij5[0] = ij5; _ij5[1] = -1;
3850 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3851 {
3852 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3853 {
3854  j5valid[iij5]=false; _ij5[1] = iij5; break;
3855 }
3856 }
3857 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3858 {
3859 IkReal evalcond[12];
3860 IkReal x277=IKsin(j5);
3861 IkReal x278=IKcos(j5);
3862 IkReal x279=(cj3*new_r00);
3863 IkReal x280=(cj3*cj4);
3864 IkReal x281=((1.0)*cj3);
3865 IkReal x282=(new_r11*sj3);
3866 IkReal x283=((1.0)*cj4);
3867 IkReal x284=(new_r10*sj3);
3868 IkReal x285=((1.0)*sj4);
3869 IkReal x286=(sj3*x277);
3870 IkReal x287=((1.0)*x278);
3871 IkReal x288=(sj3*x278);
3872 evalcond[0]=(new_r21+((sj4*x277)));
3873 evalcond[1]=(new_r20+(((-1.0)*x278*x285)));
3874 evalcond[2]=(((new_r00*sj3))+x277+(((-1.0)*new_r10*x281)));
3875 evalcond[3]=(((new_r01*sj3))+x278+(((-1.0)*new_r11*x281)));
3876 evalcond[4]=(((cj4*x277))+x282+((cj3*new_r01)));
3877 evalcond[5]=(x288+new_r01+((x277*x280)));
3878 evalcond[6]=(x279+x284+(((-1.0)*x278*x283)));
3879 evalcond[7]=((((-1.0)*x280*x287))+x286+new_r00);
3880 evalcond[8]=(new_r11+((cj4*x286))+(((-1.0)*x278*x281)));
3881 evalcond[9]=((((-1.0)*x277*x281))+new_r10+(((-1.0)*x283*x288)));
3882 evalcond[10]=(x278+(((-1.0)*new_r20*x285))+(((-1.0)*x279*x283))+(((-1.0)*x283*x284)));
3883 evalcond[11]=((((-1.0)*x277))+(((-1.0)*x282*x283))+(((-1.0)*new_r01*x280))+(((-1.0)*new_r21*x285)));
3884 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
3885 {
3886 continue;
3887 }
3888 }
3889 
3890 {
3891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3892 vinfos[0].jointtype = 1;
3893 vinfos[0].foffset = j0;
3894 vinfos[0].indices[0] = _ij0[0];
3895 vinfos[0].indices[1] = _ij0[1];
3896 vinfos[0].maxsolutions = _nj0;
3897 vinfos[1].jointtype = 1;
3898 vinfos[1].foffset = j1;
3899 vinfos[1].indices[0] = _ij1[0];
3900 vinfos[1].indices[1] = _ij1[1];
3901 vinfos[1].maxsolutions = _nj1;
3902 vinfos[2].jointtype = 1;
3903 vinfos[2].foffset = j2;
3904 vinfos[2].indices[0] = _ij2[0];
3905 vinfos[2].indices[1] = _ij2[1];
3906 vinfos[2].maxsolutions = _nj2;
3907 vinfos[3].jointtype = 1;
3908 vinfos[3].foffset = j3;
3909 vinfos[3].indices[0] = _ij3[0];
3910 vinfos[3].indices[1] = _ij3[1];
3911 vinfos[3].maxsolutions = _nj3;
3912 vinfos[4].jointtype = 1;
3913 vinfos[4].foffset = j4;
3914 vinfos[4].indices[0] = _ij4[0];
3915 vinfos[4].indices[1] = _ij4[1];
3916 vinfos[4].maxsolutions = _nj4;
3917 vinfos[5].jointtype = 1;
3918 vinfos[5].foffset = j5;
3919 vinfos[5].indices[0] = _ij5[0];
3920 vinfos[5].indices[1] = _ij5[1];
3921 vinfos[5].maxsolutions = _nj5;
3922 std::vector<int> vfree(0);
3923 solutions.AddSolution(vinfos,vfree);
3924 }
3925 }
3926 }
3927 
3928 }
3929 
3930 }
3931 }
3932 }
3933 
3934 }
3935 
3936 }
3937 
3938 } else
3939 {
3940 {
3941 IkReal j3array[1], cj3array[1], sj3array[1];
3942 bool j3valid[1]={false};
3943 _nj3 = 1;
3944 CheckValue<IkReal> x289=IKPowWithIntegerCheck(IKsign(sj4),-1);
3945 if(!x289.valid){
3946 continue;
3947 }
3948 CheckValue<IkReal> x290 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3949 if(!x290.valid){
3950 continue;
3951 }
3952 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x289.value)))+(x290.value));
3953 sj3array[0]=IKsin(j3array[0]);
3954 cj3array[0]=IKcos(j3array[0]);
3955 if( j3array[0] > IKPI )
3956 {
3957  j3array[0]-=IK2PI;
3958 }
3959 else if( j3array[0] < -IKPI )
3960 { j3array[0]+=IK2PI;
3961 }
3962 j3valid[0] = true;
3963 for(int ij3 = 0; ij3 < 1; ++ij3)
3964 {
3965 if( !j3valid[ij3] )
3966 {
3967  continue;
3968 }
3969 _ij3[0] = ij3; _ij3[1] = -1;
3970 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3971 {
3972 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3973 {
3974  j3valid[iij3]=false; _ij3[1] = iij3; break;
3975 }
3976 }
3977 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3978 {
3979 IkReal evalcond[8];
3980 IkReal x291=IKcos(j3);
3981 IkReal x292=IKsin(j3);
3982 IkReal x293=((1.0)*cj4);
3983 IkReal x294=(sj4*x292);
3984 IkReal x295=(new_r12*x292);
3985 IkReal x296=(sj4*x291);
3986 IkReal x297=(new_r02*x291);
3987 evalcond[0]=(x296+new_r02);
3988 evalcond[1]=(x294+new_r12);
3989 evalcond[2]=(((new_r12*x291))+(((-1.0)*new_r02*x292)));
3990 evalcond[3]=(sj4+x295+x297);
3991 evalcond[4]=((((-1.0)*new_r20*x293))+((new_r10*x294))+((new_r00*x296)));
3992 evalcond[5]=((((-1.0)*new_r21*x293))+((new_r11*x294))+((new_r01*x296)));
3993 evalcond[6]=((1.0)+((new_r02*x296))+((new_r12*x294))+(((-1.0)*new_r22*x293)));
3994 evalcond[7]=((((-1.0)*x293*x297))+(((-1.0)*x293*x295))+(((-1.0)*new_r22*sj4)));
3995 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3996 {
3997 continue;
3998 }
3999 }
4000 
4001 {
4002 IkReal j5eval[3];
4003 j5eval[0]=sj4;
4004 j5eval[1]=IKsign(sj4);
4005 j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4006 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4007 {
4008 {
4009 IkReal j5eval[2];
4010 j5eval[0]=sj4;
4011 j5eval[1]=sj3;
4012 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4013 {
4014 {
4015 IkReal j5eval[2];
4016 j5eval[0]=sj4;
4017 j5eval[1]=cj3;
4018 if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4019 {
4020 {
4021 IkReal evalcond[5];
4022 bool bgotonextstatement = true;
4023 do
4024 {
4025 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4026 evalcond[1]=new_r02;
4027 evalcond[2]=new_r12;
4028 evalcond[3]=new_r21;
4029 evalcond[4]=new_r20;
4030 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 )
4031 {
4032 bgotonextstatement=false;
4033 {
4034 IkReal j5array[1], cj5array[1], sj5array[1];
4035 bool j5valid[1]={false};
4036 _nj5 = 1;
4037 IkReal x298=((1.0)*new_r01);
4038 if( IKabs(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x298))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x298))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
4039  continue;
4040 j5array[0]=IKatan2(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x298))+((cj3*new_r00))));
4041 sj5array[0]=IKsin(j5array[0]);
4042 cj5array[0]=IKcos(j5array[0]);
4043 if( j5array[0] > IKPI )
4044 {
4045  j5array[0]-=IK2PI;
4046 }
4047 else if( j5array[0] < -IKPI )
4048 { j5array[0]+=IK2PI;
4049 }
4050 j5valid[0] = true;
4051 for(int ij5 = 0; ij5 < 1; ++ij5)
4052 {
4053 if( !j5valid[ij5] )
4054 {
4055  continue;
4056 }
4057 _ij5[0] = ij5; _ij5[1] = -1;
4058 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4059 {
4060 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4061 {
4062  j5valid[iij5]=false; _ij5[1] = iij5; break;
4063 }
4064 }
4065 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4066 {
4067 IkReal evalcond[8];
4068 IkReal x299=IKsin(j5);
4069 IkReal x300=IKcos(j5);
4070 IkReal x301=((1.0)*cj3);
4071 IkReal x302=(sj3*x299);
4072 IkReal x303=((1.0)*x300);
4073 IkReal x304=(x300*x301);
4074 evalcond[0]=(((new_r11*sj3))+x299+((cj3*new_r01)));
4075 evalcond[1]=(((new_r00*sj3))+x299+(((-1.0)*new_r10*x301)));
4076 evalcond[2]=(((new_r01*sj3))+x300+(((-1.0)*new_r11*x301)));
4077 evalcond[3]=(((cj3*x299))+((sj3*x300))+new_r01);
4078 evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x303)));
4079 evalcond[5]=(x302+new_r00+(((-1.0)*x304)));
4080 evalcond[6]=(x302+new_r11+(((-1.0)*x304)));
4081 evalcond[7]=((((-1.0)*sj3*x303))+(((-1.0)*x299*x301))+new_r10);
4082 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4083 {
4084 continue;
4085 }
4086 }
4087 
4088 {
4089 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4090 vinfos[0].jointtype = 1;
4091 vinfos[0].foffset = j0;
4092 vinfos[0].indices[0] = _ij0[0];
4093 vinfos[0].indices[1] = _ij0[1];
4094 vinfos[0].maxsolutions = _nj0;
4095 vinfos[1].jointtype = 1;
4096 vinfos[1].foffset = j1;
4097 vinfos[1].indices[0] = _ij1[0];
4098 vinfos[1].indices[1] = _ij1[1];
4099 vinfos[1].maxsolutions = _nj1;
4100 vinfos[2].jointtype = 1;
4101 vinfos[2].foffset = j2;
4102 vinfos[2].indices[0] = _ij2[0];
4103 vinfos[2].indices[1] = _ij2[1];
4104 vinfos[2].maxsolutions = _nj2;
4105 vinfos[3].jointtype = 1;
4106 vinfos[3].foffset = j3;
4107 vinfos[3].indices[0] = _ij3[0];
4108 vinfos[3].indices[1] = _ij3[1];
4109 vinfos[3].maxsolutions = _nj3;
4110 vinfos[4].jointtype = 1;
4111 vinfos[4].foffset = j4;
4112 vinfos[4].indices[0] = _ij4[0];
4113 vinfos[4].indices[1] = _ij4[1];
4114 vinfos[4].maxsolutions = _nj4;
4115 vinfos[5].jointtype = 1;
4116 vinfos[5].foffset = j5;
4117 vinfos[5].indices[0] = _ij5[0];
4118 vinfos[5].indices[1] = _ij5[1];
4119 vinfos[5].maxsolutions = _nj5;
4120 std::vector<int> vfree(0);
4121 solutions.AddSolution(vinfos,vfree);
4122 }
4123 }
4124 }
4125 
4126 }
4127 } while(0);
4128 if( bgotonextstatement )
4129 {
4130 bool bgotonextstatement = true;
4131 do
4132 {
4133 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4134 evalcond[1]=new_r02;
4135 evalcond[2]=new_r12;
4136 evalcond[3]=new_r21;
4137 evalcond[4]=new_r20;
4138 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 )
4139 {
4140 bgotonextstatement=false;
4141 {
4142 IkReal j5array[1], cj5array[1], sj5array[1];
4143 bool j5valid[1]={false};
4144 _nj5 = 1;
4145 IkReal x305=((1.0)*sj3);
4146 if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x305))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))))-1) <= IKFAST_SINCOS_THRESH )
4147  continue;
4148 j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x305))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))));
4149 sj5array[0]=IKsin(j5array[0]);
4150 cj5array[0]=IKcos(j5array[0]);
4151 if( j5array[0] > IKPI )
4152 {
4153  j5array[0]-=IK2PI;
4154 }
4155 else if( j5array[0] < -IKPI )
4156 { j5array[0]+=IK2PI;
4157 }
4158 j5valid[0] = true;
4159 for(int ij5 = 0; ij5 < 1; ++ij5)
4160 {
4161 if( !j5valid[ij5] )
4162 {
4163  continue;
4164 }
4165 _ij5[0] = ij5; _ij5[1] = -1;
4166 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4167 {
4168 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4169 {
4170  j5valid[iij5]=false; _ij5[1] = iij5; break;
4171 }
4172 }
4173 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4174 {
4175 IkReal evalcond[8];
4176 IkReal x306=IKcos(j5);
4177 IkReal x307=IKsin(j5);
4178 IkReal x308=((1.0)*cj3);
4179 IkReal x309=(sj3*x306);
4180 IkReal x310=((1.0)*x307);
4181 IkReal x311=(x307*x308);
4182 evalcond[0]=(((new_r10*sj3))+x306+((cj3*new_r00)));
4183 evalcond[1]=(((new_r00*sj3))+x307+(((-1.0)*new_r10*x308)));
4184 evalcond[2]=(((new_r01*sj3))+x306+(((-1.0)*new_r11*x308)));
4185 evalcond[3]=(((new_r11*sj3))+(((-1.0)*x310))+((cj3*new_r01)));
4186 evalcond[4]=(((sj3*x307))+((cj3*x306))+new_r00);
4187 evalcond[5]=(x309+(((-1.0)*x311))+new_r01);
4188 evalcond[6]=(x309+(((-1.0)*x311))+new_r10);
4189 evalcond[7]=((((-1.0)*sj3*x310))+(((-1.0)*x306*x308))+new_r11);
4190 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4191 {
4192 continue;
4193 }
4194 }
4195 
4196 {
4197 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4198 vinfos[0].jointtype = 1;
4199 vinfos[0].foffset = j0;
4200 vinfos[0].indices[0] = _ij0[0];
4201 vinfos[0].indices[1] = _ij0[1];
4202 vinfos[0].maxsolutions = _nj0;
4203 vinfos[1].jointtype = 1;
4204 vinfos[1].foffset = j1;
4205 vinfos[1].indices[0] = _ij1[0];
4206 vinfos[1].indices[1] = _ij1[1];
4207 vinfos[1].maxsolutions = _nj1;
4208 vinfos[2].jointtype = 1;
4209 vinfos[2].foffset = j2;
4210 vinfos[2].indices[0] = _ij2[0];
4211 vinfos[2].indices[1] = _ij2[1];
4212 vinfos[2].maxsolutions = _nj2;
4213 vinfos[3].jointtype = 1;
4214 vinfos[3].foffset = j3;
4215 vinfos[3].indices[0] = _ij3[0];
4216 vinfos[3].indices[1] = _ij3[1];
4217 vinfos[3].maxsolutions = _nj3;
4218 vinfos[4].jointtype = 1;
4219 vinfos[4].foffset = j4;
4220 vinfos[4].indices[0] = _ij4[0];
4221 vinfos[4].indices[1] = _ij4[1];
4222 vinfos[4].maxsolutions = _nj4;
4223 vinfos[5].jointtype = 1;
4224 vinfos[5].foffset = j5;
4225 vinfos[5].indices[0] = _ij5[0];
4226 vinfos[5].indices[1] = _ij5[1];
4227 vinfos[5].maxsolutions = _nj5;
4228 std::vector<int> vfree(0);
4229 solutions.AddSolution(vinfos,vfree);
4230 }
4231 }
4232 }
4233 
4234 }
4235 } while(0);
4236 if( bgotonextstatement )
4237 {
4238 bool bgotonextstatement = true;
4239 do
4240 {
4241 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4242 evalcond[1]=new_r02;
4243 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4244 {
4245 bgotonextstatement=false;
4246 {
4247 IkReal j5array[1], cj5array[1], sj5array[1];
4248 bool j5valid[1]={false};
4249 _nj5 = 1;
4250 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 )
4251  continue;
4252 j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4253 sj5array[0]=IKsin(j5array[0]);
4254 cj5array[0]=IKcos(j5array[0]);
4255 if( j5array[0] > IKPI )
4256 {
4257  j5array[0]-=IK2PI;
4258 }
4259 else if( j5array[0] < -IKPI )
4260 { j5array[0]+=IK2PI;
4261 }
4262 j5valid[0] = true;
4263 for(int ij5 = 0; ij5 < 1; ++ij5)
4264 {
4265 if( !j5valid[ij5] )
4266 {
4267  continue;
4268 }
4269 _ij5[0] = ij5; _ij5[1] = -1;
4270 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4271 {
4272 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4273 {
4274  j5valid[iij5]=false; _ij5[1] = iij5; break;
4275 }
4276 }
4277 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4278 {
4279 IkReal evalcond[8];
4280 IkReal x312=IKsin(j5);
4281 IkReal x313=IKcos(j5);
4282 IkReal x314=((1.0)*cj4);
4283 IkReal x315=((1.0)*sj4);
4284 evalcond[0]=(x312+new_r00);
4285 evalcond[1]=(x313+new_r01);
4286 evalcond[2]=(((sj4*x312))+new_r21);
4287 evalcond[3]=(((cj4*x312))+new_r11);
4288 evalcond[4]=((((-1.0)*x313*x315))+new_r20);
4289 evalcond[5]=((((-1.0)*x313*x314))+new_r10);
4290 evalcond[6]=((((-1.0)*new_r20*x315))+x313+(((-1.0)*new_r10*x314)));
4291 evalcond[7]=((((-1.0)*new_r21*x315))+(((-1.0)*new_r11*x314))+(((-1.0)*x312)));
4292 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4293 {
4294 continue;
4295 }
4296 }
4297 
4298 {
4299 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4300 vinfos[0].jointtype = 1;
4301 vinfos[0].foffset = j0;
4302 vinfos[0].indices[0] = _ij0[0];
4303 vinfos[0].indices[1] = _ij0[1];
4304 vinfos[0].maxsolutions = _nj0;
4305 vinfos[1].jointtype = 1;
4306 vinfos[1].foffset = j1;
4307 vinfos[1].indices[0] = _ij1[0];
4308 vinfos[1].indices[1] = _ij1[1];
4309 vinfos[1].maxsolutions = _nj1;
4310 vinfos[2].jointtype = 1;
4311 vinfos[2].foffset = j2;
4312 vinfos[2].indices[0] = _ij2[0];
4313 vinfos[2].indices[1] = _ij2[1];
4314 vinfos[2].maxsolutions = _nj2;
4315 vinfos[3].jointtype = 1;
4316 vinfos[3].foffset = j3;
4317 vinfos[3].indices[0] = _ij3[0];
4318 vinfos[3].indices[1] = _ij3[1];
4319 vinfos[3].maxsolutions = _nj3;
4320 vinfos[4].jointtype = 1;
4321 vinfos[4].foffset = j4;
4322 vinfos[4].indices[0] = _ij4[0];
4323 vinfos[4].indices[1] = _ij4[1];
4324 vinfos[4].maxsolutions = _nj4;
4325 vinfos[5].jointtype = 1;
4326 vinfos[5].foffset = j5;
4327 vinfos[5].indices[0] = _ij5[0];
4328 vinfos[5].indices[1] = _ij5[1];
4329 vinfos[5].maxsolutions = _nj5;
4330 std::vector<int> vfree(0);
4331 solutions.AddSolution(vinfos,vfree);
4332 }
4333 }
4334 }
4335 
4336 }
4337 } while(0);
4338 if( bgotonextstatement )
4339 {
4340 bool bgotonextstatement = true;
4341 do
4342 {
4343 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4344 evalcond[1]=new_r02;
4345 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4346 {
4347 bgotonextstatement=false;
4348 {
4349 IkReal j5array[1], cj5array[1], sj5array[1];
4350 bool j5valid[1]={false};
4351 _nj5 = 1;
4352 if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4353  continue;
4354 j5array[0]=IKatan2(new_r00, new_r01);
4355 sj5array[0]=IKsin(j5array[0]);
4356 cj5array[0]=IKcos(j5array[0]);
4357 if( j5array[0] > IKPI )
4358 {
4359  j5array[0]-=IK2PI;
4360 }
4361 else if( j5array[0] < -IKPI )
4362 { j5array[0]+=IK2PI;
4363 }
4364 j5valid[0] = true;
4365 for(int ij5 = 0; ij5 < 1; ++ij5)
4366 {
4367 if( !j5valid[ij5] )
4368 {
4369  continue;
4370 }
4371 _ij5[0] = ij5; _ij5[1] = -1;
4372 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4373 {
4374 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4375 {
4376  j5valid[iij5]=false; _ij5[1] = iij5; break;
4377 }
4378 }
4379 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4380 {
4381 IkReal evalcond[8];
4382 IkReal x316=IKsin(j5);
4383 IkReal x317=IKcos(j5);
4384 IkReal x318=((1.0)*sj4);
4385 IkReal x319=((1.0)*x317);
4386 evalcond[0]=(((sj4*x316))+new_r21);
4387 evalcond[1]=(x316+(((-1.0)*new_r00)));
4388 evalcond[2]=(x317+(((-1.0)*new_r01)));
4389 evalcond[3]=(new_r20+(((-1.0)*x317*x318)));
4390 evalcond[4]=(((cj4*x316))+(((-1.0)*new_r11)));
4391 evalcond[5]=((((-1.0)*cj4*x319))+(((-1.0)*new_r10)));
4392 evalcond[6]=((((-1.0)*new_r20*x318))+((cj4*new_r10))+x317);
4393 evalcond[7]=((((-1.0)*new_r21*x318))+((cj4*new_r11))+(((-1.0)*x316)));
4394 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4395 {
4396 continue;
4397 }
4398 }
4399 
4400 {
4401 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4402 vinfos[0].jointtype = 1;
4403 vinfos[0].foffset = j0;
4404 vinfos[0].indices[0] = _ij0[0];
4405 vinfos[0].indices[1] = _ij0[1];
4406 vinfos[0].maxsolutions = _nj0;
4407 vinfos[1].jointtype = 1;
4408 vinfos[1].foffset = j1;
4409 vinfos[1].indices[0] = _ij1[0];
4410 vinfos[1].indices[1] = _ij1[1];
4411 vinfos[1].maxsolutions = _nj1;
4412 vinfos[2].jointtype = 1;
4413 vinfos[2].foffset = j2;
4414 vinfos[2].indices[0] = _ij2[0];
4415 vinfos[2].indices[1] = _ij2[1];
4416 vinfos[2].maxsolutions = _nj2;
4417 vinfos[3].jointtype = 1;
4418 vinfos[3].foffset = j3;
4419 vinfos[3].indices[0] = _ij3[0];
4420 vinfos[3].indices[1] = _ij3[1];
4421 vinfos[3].maxsolutions = _nj3;
4422 vinfos[4].jointtype = 1;
4423 vinfos[4].foffset = j4;
4424 vinfos[4].indices[0] = _ij4[0];
4425 vinfos[4].indices[1] = _ij4[1];
4426 vinfos[4].maxsolutions = _nj4;
4427 vinfos[5].jointtype = 1;
4428 vinfos[5].foffset = j5;
4429 vinfos[5].indices[0] = _ij5[0];
4430 vinfos[5].indices[1] = _ij5[1];
4431 vinfos[5].maxsolutions = _nj5;
4432 std::vector<int> vfree(0);
4433 solutions.AddSolution(vinfos,vfree);
4434 }
4435 }
4436 }
4437 
4438 }
4439 } while(0);
4440 if( bgotonextstatement )
4441 {
4442 bool bgotonextstatement = true;
4443 do
4444 {
4445 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4446 evalcond[1]=new_r12;
4447 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4448 {
4449 bgotonextstatement=false;
4450 {
4451 IkReal j5array[1], cj5array[1], sj5array[1];
4452 bool j5valid[1]={false};
4453 _nj5 = 1;
4454 if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4455  continue;
4456 j5array[0]=IKatan2(new_r10, new_r11);
4457 sj5array[0]=IKsin(j5array[0]);
4458 cj5array[0]=IKcos(j5array[0]);
4459 if( j5array[0] > IKPI )
4460 {
4461  j5array[0]-=IK2PI;
4462 }
4463 else if( j5array[0] < -IKPI )
4464 { j5array[0]+=IK2PI;
4465 }
4466 j5valid[0] = true;
4467 for(int ij5 = 0; ij5 < 1; ++ij5)
4468 {
4469 if( !j5valid[ij5] )
4470 {
4471  continue;
4472 }
4473 _ij5[0] = ij5; _ij5[1] = -1;
4474 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4475 {
4476 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4477 {
4478  j5valid[iij5]=false; _ij5[1] = iij5; break;
4479 }
4480 }
4481 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4482 {
4483 IkReal evalcond[8];
4484 IkReal x320=IKcos(j5);
4485 IkReal x321=IKsin(j5);
4486 IkReal x322=((1.0)*cj4);
4487 IkReal x323=((1.0)*sj4);
4488 IkReal x324=((1.0)*x321);
4489 evalcond[0]=(new_r20+((new_r02*x320)));
4490 evalcond[1]=(x321+(((-1.0)*new_r10)));
4491 evalcond[2]=(x320+(((-1.0)*new_r11)));
4492 evalcond[3]=(((cj4*x321))+new_r01);
4493 evalcond[4]=((((-1.0)*new_r02*x324))+new_r21);
4494 evalcond[5]=((((-1.0)*x320*x322))+new_r00);
4495 evalcond[6]=(x320+(((-1.0)*new_r00*x322))+(((-1.0)*new_r20*x323)));
4496 evalcond[7]=((((-1.0)*x324))+(((-1.0)*new_r01*x322))+(((-1.0)*new_r21*x323)));
4497 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4498 {
4499 continue;
4500 }
4501 }
4502 
4503 {
4504 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4505 vinfos[0].jointtype = 1;
4506 vinfos[0].foffset = j0;
4507 vinfos[0].indices[0] = _ij0[0];
4508 vinfos[0].indices[1] = _ij0[1];
4509 vinfos[0].maxsolutions = _nj0;
4510 vinfos[1].jointtype = 1;
4511 vinfos[1].foffset = j1;
4512 vinfos[1].indices[0] = _ij1[0];
4513 vinfos[1].indices[1] = _ij1[1];
4514 vinfos[1].maxsolutions = _nj1;
4515 vinfos[2].jointtype = 1;
4516 vinfos[2].foffset = j2;
4517 vinfos[2].indices[0] = _ij2[0];
4518 vinfos[2].indices[1] = _ij2[1];
4519 vinfos[2].maxsolutions = _nj2;
4520 vinfos[3].jointtype = 1;
4521 vinfos[3].foffset = j3;
4522 vinfos[3].indices[0] = _ij3[0];
4523 vinfos[3].indices[1] = _ij3[1];
4524 vinfos[3].maxsolutions = _nj3;
4525 vinfos[4].jointtype = 1;
4526 vinfos[4].foffset = j4;
4527 vinfos[4].indices[0] = _ij4[0];
4528 vinfos[4].indices[1] = _ij4[1];
4529 vinfos[4].maxsolutions = _nj4;
4530 vinfos[5].jointtype = 1;
4531 vinfos[5].foffset = j5;
4532 vinfos[5].indices[0] = _ij5[0];
4533 vinfos[5].indices[1] = _ij5[1];
4534 vinfos[5].maxsolutions = _nj5;
4535 std::vector<int> vfree(0);
4536 solutions.AddSolution(vinfos,vfree);
4537 }
4538 }
4539 }
4540 
4541 }
4542 } while(0);
4543 if( bgotonextstatement )
4544 {
4545 bool bgotonextstatement = true;
4546 do
4547 {
4548 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4549 evalcond[1]=new_r12;
4550 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4551 {
4552 bgotonextstatement=false;
4553 {
4554 IkReal j5array[1], cj5array[1], sj5array[1];
4555 bool j5valid[1]={false};
4556 _nj5 = 1;
4557 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 )
4558  continue;
4559 j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4560 sj5array[0]=IKsin(j5array[0]);
4561 cj5array[0]=IKcos(j5array[0]);
4562 if( j5array[0] > IKPI )
4563 {
4564  j5array[0]-=IK2PI;
4565 }
4566 else if( j5array[0] < -IKPI )
4567 { j5array[0]+=IK2PI;
4568 }
4569 j5valid[0] = true;
4570 for(int ij5 = 0; ij5 < 1; ++ij5)
4571 {
4572 if( !j5valid[ij5] )
4573 {
4574  continue;
4575 }
4576 _ij5[0] = ij5; _ij5[1] = -1;
4577 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4578 {
4579 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4580 {
4581  j5valid[iij5]=false; _ij5[1] = iij5; break;
4582 }
4583 }
4584 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4585 {
4586 IkReal evalcond[8];
4587 IkReal x325=IKsin(j5);
4588 IkReal x326=IKcos(j5);
4589 IkReal x327=((1.0)*sj4);
4590 IkReal x328=((1.0)*x326);
4591 evalcond[0]=(x325+new_r10);
4592 evalcond[1]=(x326+new_r11);
4593 evalcond[2]=(new_r21+((new_r02*x325)));
4594 evalcond[3]=((((-1.0)*new_r02*x328))+new_r20);
4595 evalcond[4]=(((cj4*x325))+(((-1.0)*new_r01)));
4596 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x328)));
4597 evalcond[6]=(((cj4*new_r00))+x326+(((-1.0)*new_r20*x327)));
4598 evalcond[7]=(((cj4*new_r01))+(((-1.0)*x325))+(((-1.0)*new_r21*x327)));
4599 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4600 {
4601 continue;
4602 }
4603 }
4604 
4605 {
4606 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4607 vinfos[0].jointtype = 1;
4608 vinfos[0].foffset = j0;
4609 vinfos[0].indices[0] = _ij0[0];
4610 vinfos[0].indices[1] = _ij0[1];
4611 vinfos[0].maxsolutions = _nj0;
4612 vinfos[1].jointtype = 1;
4613 vinfos[1].foffset = j1;
4614 vinfos[1].indices[0] = _ij1[0];
4615 vinfos[1].indices[1] = _ij1[1];
4616 vinfos[1].maxsolutions = _nj1;
4617 vinfos[2].jointtype = 1;
4618 vinfos[2].foffset = j2;
4619 vinfos[2].indices[0] = _ij2[0];
4620 vinfos[2].indices[1] = _ij2[1];
4621 vinfos[2].maxsolutions = _nj2;
4622 vinfos[3].jointtype = 1;
4623 vinfos[3].foffset = j3;
4624 vinfos[3].indices[0] = _ij3[0];
4625 vinfos[3].indices[1] = _ij3[1];
4626 vinfos[3].maxsolutions = _nj3;
4627 vinfos[4].jointtype = 1;
4628 vinfos[4].foffset = j4;
4629 vinfos[4].indices[0] = _ij4[0];
4630 vinfos[4].indices[1] = _ij4[1];
4631 vinfos[4].maxsolutions = _nj4;
4632 vinfos[5].jointtype = 1;
4633 vinfos[5].foffset = j5;
4634 vinfos[5].indices[0] = _ij5[0];
4635 vinfos[5].indices[1] = _ij5[1];
4636 vinfos[5].maxsolutions = _nj5;
4637 std::vector<int> vfree(0);
4638 solutions.AddSolution(vinfos,vfree);
4639 }
4640 }
4641 }
4642 
4643 }
4644 } while(0);
4645 if( bgotonextstatement )
4646 {
4647 bool bgotonextstatement = true;
4648 do
4649 {
4650 evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4651 if( IKabs(evalcond[0]) < 0.0000050000000000 )
4652 {
4653 bgotonextstatement=false;
4654 {
4655 IkReal j5eval[1];
4656 new_r21=0;
4657 new_r20=0;
4658 new_r02=0;
4659 new_r12=0;
4660 j5eval[0]=1.0;
4661 if( IKabs(j5eval[0]) < 0.0000000100000000 )
4662 {
4663 continue; // no branches [j5]
4664 
4665 } else
4666 {
4667 IkReal op[2+1], zeror[2];
4668 int numroots;
4669 op[0]=-1.0;
4670 op[1]=0;
4671 op[2]=1.0;
4672 polyroots2(op,zeror,numroots);
4673 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4674 int numsolutions = 0;
4675 for(int ij5 = 0; ij5 < numroots; ++ij5)
4676 {
4677 IkReal htj5 = zeror[ij5];
4678 tempj5array[0]=((2.0)*(atan(htj5)));
4679 for(int kj5 = 0; kj5 < 1; ++kj5)
4680 {
4681 j5array[numsolutions] = tempj5array[kj5];
4682 if( j5array[numsolutions] > IKPI )
4683 {
4684  j5array[numsolutions]-=IK2PI;
4685 }
4686 else if( j5array[numsolutions] < -IKPI )
4687 {
4688  j5array[numsolutions]+=IK2PI;
4689 }
4690 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4691 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4692 numsolutions++;
4693 }
4694 }
4695 bool j5valid[2]={true,true};
4696 _nj5 = 2;
4697 for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4698  {
4699 if( !j5valid[ij5] )
4700 {
4701  continue;
4702 }
4703  j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4704 htj5 = IKtan(j5/2);
4705 
4706 _ij5[0] = ij5; _ij5[1] = -1;
4707 for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4708 {
4709 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4710 {
4711  j5valid[iij5]=false; _ij5[1] = iij5; break;
4712 }
4713 }
4714 {
4715 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4716 vinfos[0].jointtype = 1;
4717 vinfos[0].foffset = j0;
4718 vinfos[0].indices[0] = _ij0[0];
4719 vinfos[0].indices[1] = _ij0[1];
4720 vinfos[0].maxsolutions = _nj0;
4721 vinfos[1].jointtype = 1;
4722 vinfos[1].foffset = j1;
4723 vinfos[1].indices[0] = _ij1[0];
4724 vinfos[1].indices[1] = _ij1[1];
4725 vinfos[1].maxsolutions = _nj1;
4726 vinfos[2].jointtype = 1;
4727 vinfos[2].foffset = j2;
4728 vinfos[2].indices[0] = _ij2[0];
4729 vinfos[2].indices[1] = _ij2[1];
4730 vinfos[2].maxsolutions = _nj2;
4731 vinfos[3].jointtype = 1;
4732 vinfos[3].foffset = j3;
4733 vinfos[3].indices[0] = _ij3[0];
4734 vinfos[3].indices[1] = _ij3[1];
4735 vinfos[3].maxsolutions = _nj3;
4736 vinfos[4].jointtype = 1;
4737 vinfos[4].foffset = j4;
4738 vinfos[4].indices[0] = _ij4[0];
4739 vinfos[4].indices[1] = _ij4[1];
4740 vinfos[4].maxsolutions = _nj4;
4741 vinfos[5].jointtype = 1;
4742 vinfos[5].foffset = j5;
4743 vinfos[5].indices[0] = _ij5[0];
4744 vinfos[5].indices[1] = _ij5[1];
4745 vinfos[5].maxsolutions = _nj5;
4746 std::vector<int> vfree(0);
4747 solutions.AddSolution(vinfos,vfree);
4748 }
4749  }
4750 
4751 }
4752 
4753 }
4754 
4755 }
4756 } while(0);
4757 if( bgotonextstatement )
4758 {
4759 bool bgotonextstatement = true;
4760 do
4761 {
4762 if( 1 )
4763 {
4764 bgotonextstatement=false;
4765 continue; // branch miss [j5]
4766 
4767 }
4768 } while(0);
4769 if( bgotonextstatement )
4770 {
4771 }
4772 }
4773 }
4774 }
4775 }
4776 }
4777 }
4778 }
4779 }
4780 
4781 } else
4782 {
4783 {
4784 IkReal j5array[1], cj5array[1], sj5array[1];
4785 bool j5valid[1]={false};
4786 _nj5 = 1;
4787 CheckValue<IkReal> x330=IKPowWithIntegerCheck(sj4,-1);
4788 if(!x330.valid){
4789 continue;
4790 }
4791 IkReal x329=x330.value;
4792 CheckValue<IkReal> x331=IKPowWithIntegerCheck(cj3,-1);
4793 if(!x331.valid){
4794 continue;
4795 }
4796 if( IKabs(((-1.0)*new_r21*x329)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x329))+IKsqr((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4797  continue;
4798 j5array[0]=IKatan2(((-1.0)*new_r21*x329), (x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4799 sj5array[0]=IKsin(j5array[0]);
4800 cj5array[0]=IKcos(j5array[0]);
4801 if( j5array[0] > IKPI )
4802 {
4803  j5array[0]-=IK2PI;
4804 }
4805 else if( j5array[0] < -IKPI )
4806 { j5array[0]+=IK2PI;
4807 }
4808 j5valid[0] = true;
4809 for(int ij5 = 0; ij5 < 1; ++ij5)
4810 {
4811 if( !j5valid[ij5] )
4812 {
4813  continue;
4814 }
4815 _ij5[0] = ij5; _ij5[1] = -1;
4816 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4817 {
4818 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4819 {
4820  j5valid[iij5]=false; _ij5[1] = iij5; break;
4821 }
4822 }
4823 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4824 {
4825 IkReal evalcond[12];
4826 IkReal x332=IKsin(j5);
4827 IkReal x333=IKcos(j5);
4828 IkReal x334=(cj3*new_r00);
4829 IkReal x335=(cj3*cj4);
4830 IkReal x336=((1.0)*cj3);
4831 IkReal x337=(new_r11*sj3);
4832 IkReal x338=((1.0)*cj4);
4833 IkReal x339=(new_r10*sj3);
4834 IkReal x340=((1.0)*sj4);
4835 IkReal x341=(sj3*x332);
4836 IkReal x342=((1.0)*x333);
4837 IkReal x343=(sj3*x333);
4838 evalcond[0]=(((sj4*x332))+new_r21);
4839 evalcond[1]=(new_r20+(((-1.0)*x333*x340)));
4840 evalcond[2]=((((-1.0)*new_r10*x336))+((new_r00*sj3))+x332);
4841 evalcond[3]=((((-1.0)*new_r11*x336))+((new_r01*sj3))+x333);
4842 evalcond[4]=(x337+((cj4*x332))+((cj3*new_r01)));
4843 evalcond[5]=(((x332*x335))+x343+new_r01);
4844 evalcond[6]=((((-1.0)*x333*x338))+x339+x334);
4845 evalcond[7]=((((-1.0)*x335*x342))+x341+new_r00);
4846 evalcond[8]=((((-1.0)*x333*x336))+((cj4*x341))+new_r11);
4847 evalcond[9]=((((-1.0)*x332*x336))+new_r10+(((-1.0)*x338*x343)));
4848 evalcond[10]=((((-1.0)*x338*x339))+x333+(((-1.0)*new_r20*x340))+(((-1.0)*x334*x338)));
4849 evalcond[11]=((((-1.0)*x337*x338))+(((-1.0)*new_r01*x335))+(((-1.0)*x332))+(((-1.0)*new_r21*x340)));
4850 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4851 {
4852 continue;
4853 }
4854 }
4855 
4856 {
4857 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4858 vinfos[0].jointtype = 1;
4859 vinfos[0].foffset = j0;
4860 vinfos[0].indices[0] = _ij0[0];
4861 vinfos[0].indices[1] = _ij0[1];
4862 vinfos[0].maxsolutions = _nj0;
4863 vinfos[1].jointtype = 1;
4864 vinfos[1].foffset = j1;
4865 vinfos[1].indices[0] = _ij1[0];
4866 vinfos[1].indices[1] = _ij1[1];
4867 vinfos[1].maxsolutions = _nj1;
4868 vinfos[2].jointtype = 1;
4869 vinfos[2].foffset = j2;
4870 vinfos[2].indices[0] = _ij2[0];
4871 vinfos[2].indices[1] = _ij2[1];
4872 vinfos[2].maxsolutions = _nj2;
4873 vinfos[3].jointtype = 1;
4874 vinfos[3].foffset = j3;
4875 vinfos[3].indices[0] = _ij3[0];
4876 vinfos[3].indices[1] = _ij3[1];
4877 vinfos[3].maxsolutions = _nj3;
4878 vinfos[4].jointtype = 1;
4879 vinfos[4].foffset = j4;
4880 vinfos[4].indices[0] = _ij4[0];
4881 vinfos[4].indices[1] = _ij4[1];
4882 vinfos[4].maxsolutions = _nj4;
4883 vinfos[5].jointtype = 1;
4884 vinfos[5].foffset = j5;
4885 vinfos[5].indices[0] = _ij5[0];
4886 vinfos[5].indices[1] = _ij5[1];
4887 vinfos[5].maxsolutions = _nj5;
4888 std::vector<int> vfree(0);
4889 solutions.AddSolution(vinfos,vfree);
4890 }
4891 }
4892 }
4893 
4894 }
4895 
4896 }
4897 
4898 } else
4899 {
4900 {
4901 IkReal j5array[1], cj5array[1], sj5array[1];
4902 bool j5valid[1]={false};
4903 _nj5 = 1;
4904 CheckValue<IkReal> x345=IKPowWithIntegerCheck(sj4,-1);
4905 if(!x345.valid){
4906 continue;
4907 }
4908 IkReal x344=x345.value;
4909 CheckValue<IkReal> x346=IKPowWithIntegerCheck(sj3,-1);
4910 if(!x346.valid){
4911 continue;
4912 }
4913 if( IKabs(((-1.0)*new_r21*x344)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x344))+IKsqr((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4914  continue;
4915 j5array[0]=IKatan2(((-1.0)*new_r21*x344), (x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4916 sj5array[0]=IKsin(j5array[0]);
4917 cj5array[0]=IKcos(j5array[0]);
4918 if( j5array[0] > IKPI )
4919 {
4920  j5array[0]-=IK2PI;
4921 }
4922 else if( j5array[0] < -IKPI )
4923 { j5array[0]+=IK2PI;
4924 }
4925 j5valid[0] = true;
4926 for(int ij5 = 0; ij5 < 1; ++ij5)
4927 {
4928 if( !j5valid[ij5] )
4929 {
4930  continue;
4931 }
4932 _ij5[0] = ij5; _ij5[1] = -1;
4933 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4934 {
4935 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4936 {
4937  j5valid[iij5]=false; _ij5[1] = iij5; break;
4938 }
4939 }
4940 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4941 {
4942 IkReal evalcond[12];
4943 IkReal x347=IKsin(j5);
4944 IkReal x348=IKcos(j5);
4945 IkReal x349=(cj3*new_r00);
4946 IkReal x350=(cj3*cj4);
4947 IkReal x351=((1.0)*cj3);
4948 IkReal x352=(new_r11*sj3);
4949 IkReal x353=((1.0)*cj4);
4950 IkReal x354=(new_r10*sj3);
4951 IkReal x355=((1.0)*sj4);
4952 IkReal x356=(sj3*x347);
4953 IkReal x357=((1.0)*x348);
4954 IkReal x358=(sj3*x348);
4955 evalcond[0]=(((sj4*x347))+new_r21);
4956 evalcond[1]=((((-1.0)*x348*x355))+new_r20);
4957 evalcond[2]=(((new_r00*sj3))+x347+(((-1.0)*new_r10*x351)));
4958 evalcond[3]=(((new_r01*sj3))+x348+(((-1.0)*new_r11*x351)));
4959 evalcond[4]=(x352+((cj4*x347))+((cj3*new_r01)));
4960 evalcond[5]=(x358+((x347*x350))+new_r01);
4961 evalcond[6]=((((-1.0)*x348*x353))+x354+x349);
4962 evalcond[7]=(x356+new_r00+(((-1.0)*x350*x357)));
4963 evalcond[8]=((((-1.0)*x348*x351))+((cj4*x356))+new_r11);
4964 evalcond[9]=((((-1.0)*x347*x351))+(((-1.0)*x353*x358))+new_r10);
4965 evalcond[10]=((((-1.0)*x349*x353))+x348+(((-1.0)*x353*x354))+(((-1.0)*new_r20*x355)));
4966 evalcond[11]=((((-1.0)*new_r01*x350))+(((-1.0)*new_r21*x355))+(((-1.0)*x352*x353))+(((-1.0)*x347)));
4967 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
4968 {
4969 continue;
4970 }
4971 }
4972 
4973 {
4974 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4975 vinfos[0].jointtype = 1;
4976 vinfos[0].foffset = j0;
4977 vinfos[0].indices[0] = _ij0[0];
4978 vinfos[0].indices[1] = _ij0[1];
4979 vinfos[0].maxsolutions = _nj0;
4980 vinfos[1].jointtype = 1;
4981 vinfos[1].foffset = j1;
4982 vinfos[1].indices[0] = _ij1[0];
4983 vinfos[1].indices[1] = _ij1[1];
4984 vinfos[1].maxsolutions = _nj1;
4985 vinfos[2].jointtype = 1;
4986 vinfos[2].foffset = j2;
4987 vinfos[2].indices[0] = _ij2[0];
4988 vinfos[2].indices[1] = _ij2[1];
4989 vinfos[2].maxsolutions = _nj2;
4990 vinfos[3].jointtype = 1;
4991 vinfos[3].foffset = j3;
4992 vinfos[3].indices[0] = _ij3[0];
4993 vinfos[3].indices[1] = _ij3[1];
4994 vinfos[3].maxsolutions = _nj3;
4995 vinfos[4].jointtype = 1;
4996 vinfos[4].foffset = j4;
4997 vinfos[4].indices[0] = _ij4[0];
4998 vinfos[4].indices[1] = _ij4[1];
4999 vinfos[4].maxsolutions = _nj4;
5000 vinfos[5].jointtype = 1;
5001 vinfos[5].foffset = j5;
5002 vinfos[5].indices[0] = _ij5[0];
5003 vinfos[5].indices[1] = _ij5[1];
5004 vinfos[5].maxsolutions = _nj5;
5005 std::vector<int> vfree(0);
5006 solutions.AddSolution(vinfos,vfree);
5007 }
5008 }
5009 }
5010 
5011 }
5012 
5013 }
5014 
5015 } else
5016 {
5017 {
5018 IkReal j5array[1], cj5array[1], sj5array[1];
5019 bool j5valid[1]={false};
5020 _nj5 = 1;
5021 CheckValue<IkReal> x359 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5022 if(!x359.valid){
5023 continue;
5024 }
5025 CheckValue<IkReal> x360=IKPowWithIntegerCheck(IKsign(sj4),-1);
5026 if(!x360.valid){
5027 continue;
5028 }
5029 j5array[0]=((-1.5707963267949)+(x359.value)+(((1.5707963267949)*(x360.value))));
5030 sj5array[0]=IKsin(j5array[0]);
5031 cj5array[0]=IKcos(j5array[0]);
5032 if( j5array[0] > IKPI )
5033 {
5034  j5array[0]-=IK2PI;
5035 }
5036 else if( j5array[0] < -IKPI )
5037 { j5array[0]+=IK2PI;
5038 }
5039 j5valid[0] = true;
5040 for(int ij5 = 0; ij5 < 1; ++ij5)
5041 {
5042 if( !j5valid[ij5] )
5043 {
5044  continue;
5045 }
5046 _ij5[0] = ij5; _ij5[1] = -1;
5047 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5048 {
5049 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5050 {
5051  j5valid[iij5]=false; _ij5[1] = iij5; break;
5052 }
5053 }
5054 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5055 {
5056 IkReal evalcond[12];
5057 IkReal x361=IKsin(j5);
5058 IkReal x362=IKcos(j5);
5059 IkReal x363=(cj3*new_r00);
5060 IkReal x364=(cj3*cj4);
5061 IkReal x365=((1.0)*cj3);
5062 IkReal x366=(new_r11*sj3);
5063 IkReal x367=((1.0)*cj4);
5064 IkReal x368=(new_r10*sj3);
5065 IkReal x369=((1.0)*sj4);
5066 IkReal x370=(sj3*x361);
5067 IkReal x371=((1.0)*x362);
5068 IkReal x372=(sj3*x362);
5069 evalcond[0]=(((sj4*x361))+new_r21);
5070 evalcond[1]=((((-1.0)*x362*x369))+new_r20);
5071 evalcond[2]=(((new_r00*sj3))+x361+(((-1.0)*new_r10*x365)));
5072 evalcond[3]=(((new_r01*sj3))+x362+(((-1.0)*new_r11*x365)));
5073 evalcond[4]=(((cj4*x361))+x366+((cj3*new_r01)));
5074 evalcond[5]=(((x361*x364))+x372+new_r01);
5075 evalcond[6]=((((-1.0)*x362*x367))+x368+x363);
5076 evalcond[7]=(x370+new_r00+(((-1.0)*x364*x371)));
5077 evalcond[8]=((((-1.0)*x362*x365))+((cj4*x370))+new_r11);
5078 evalcond[9]=((((-1.0)*x361*x365))+(((-1.0)*x367*x372))+new_r10);
5079 evalcond[10]=((((-1.0)*x363*x367))+(((-1.0)*new_r20*x369))+(((-1.0)*x367*x368))+x362);
5080 evalcond[11]=((((-1.0)*x361))+(((-1.0)*x366*x367))+(((-1.0)*new_r01*x364))+(((-1.0)*new_r21*x369)));
5081 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
5082 {
5083 continue;
5084 }
5085 }
5086 
5087 {
5088 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5089 vinfos[0].jointtype = 1;
5090 vinfos[0].foffset = j0;
5091 vinfos[0].indices[0] = _ij0[0];
5092 vinfos[0].indices[1] = _ij0[1];
5093 vinfos[0].maxsolutions = _nj0;
5094 vinfos[1].jointtype = 1;
5095 vinfos[1].foffset = j1;
5096 vinfos[1].indices[0] = _ij1[0];
5097 vinfos[1].indices[1] = _ij1[1];
5098 vinfos[1].maxsolutions = _nj1;
5099 vinfos[2].jointtype = 1;
5100 vinfos[2].foffset = j2;
5101 vinfos[2].indices[0] = _ij2[0];
5102 vinfos[2].indices[1] = _ij2[1];
5103 vinfos[2].maxsolutions = _nj2;
5104 vinfos[3].jointtype = 1;
5105 vinfos[3].foffset = j3;
5106 vinfos[3].indices[0] = _ij3[0];
5107 vinfos[3].indices[1] = _ij3[1];
5108 vinfos[3].maxsolutions = _nj3;
5109 vinfos[4].jointtype = 1;
5110 vinfos[4].foffset = j4;
5111 vinfos[4].indices[0] = _ij4[0];
5112 vinfos[4].indices[1] = _ij4[1];
5113 vinfos[4].maxsolutions = _nj4;
5114 vinfos[5].jointtype = 1;
5115 vinfos[5].foffset = j5;
5116 vinfos[5].indices[0] = _ij5[0];
5117 vinfos[5].indices[1] = _ij5[1];
5118 vinfos[5].maxsolutions = _nj5;
5119 std::vector<int> vfree(0);
5120 solutions.AddSolution(vinfos,vfree);
5121 }
5122 }
5123 }
5124 
5125 }
5126 
5127 }
5128 }
5129 }
5130 
5131 }
5132 
5133 }
5134 
5135 } else
5136 {
5137 {
5138 IkReal j5array[1], cj5array[1], sj5array[1];
5139 bool j5valid[1]={false};
5140 _nj5 = 1;
5141 CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5142 if(!x373.valid){
5143 continue;
5144 }
5145 CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5146 if(!x374.valid){
5147 continue;
5148 }
5149 j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5150 sj5array[0]=IKsin(j5array[0]);
5151 cj5array[0]=IKcos(j5array[0]);
5152 if( j5array[0] > IKPI )
5153 {
5154  j5array[0]-=IK2PI;
5155 }
5156 else if( j5array[0] < -IKPI )
5157 { j5array[0]+=IK2PI;
5158 }
5159 j5valid[0] = true;
5160 for(int ij5 = 0; ij5 < 1; ++ij5)
5161 {
5162 if( !j5valid[ij5] )
5163 {
5164  continue;
5165 }
5166 _ij5[0] = ij5; _ij5[1] = -1;
5167 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5168 {
5169 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5170 {
5171  j5valid[iij5]=false; _ij5[1] = iij5; break;
5172 }
5173 }
5174 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5175 {
5176 IkReal evalcond[2];
5177 evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5178 evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5179 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5180 {
5181 continue;
5182 }
5183 }
5184 
5185 {
5186 IkReal j3eval[3];
5187 j3eval[0]=sj4;
5188 j3eval[1]=IKsign(sj4);
5189 j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5190 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5191 {
5192 {
5193 IkReal j3eval[2];
5194 j3eval[0]=new_r12;
5195 j3eval[1]=sj4;
5196 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5197 {
5198 {
5199 IkReal evalcond[5];
5200 bool bgotonextstatement = true;
5201 do
5202 {
5203 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5204 evalcond[1]=new_r02;
5205 evalcond[2]=new_r12;
5206 evalcond[3]=new_r21;
5207 evalcond[4]=new_r20;
5208 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 )
5209 {
5210 bgotonextstatement=false;
5211 {
5212 IkReal j3eval[3];
5213 sj4=0;
5214 cj4=1.0;
5215 j4=0;
5216 IkReal x375=((1.0)*new_r11);
5217 IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5218 j3eval[0]=x376;
5219 j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5220 j3eval[2]=IKsign(x376);
5221 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5222 {
5223 {
5224 IkReal j3eval[3];
5225 sj4=0;
5226 cj4=1.0;
5227 j4=0;
5228 IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5229 j3eval[0]=x377;
5230 j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5231 j3eval[2]=IKsign(x377);
5232 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5233 {
5234 {
5235 IkReal j3eval[3];
5236 sj4=0;
5237 cj4=1.0;
5238 j4=0;
5239 IkReal x378=((1.0)*new_r11);
5240 IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5241 j3eval[0]=x379;
5242 j3eval[1]=IKsign(x379);
5243 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5244 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5245 {
5246 {
5247 IkReal evalcond[1];
5248 bool bgotonextstatement = true;
5249 do
5250 {
5251 IkReal x380=((-1.0)*new_r01);
5252 IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5253 if(IKabs(x382)==0){
5254 continue;
5255 }
5256 IkReal x381=pow(x382,-0.5);
5257 CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5258 if(!x383.valid){
5259 continue;
5260 }
5261 IkReal gconst0=((-1.0)*(x383.value));
5262 IkReal gconst1=(new_r11*x381);
5263 IkReal gconst2=(x380*x381);
5264 CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5265 if(!x384.valid){
5266 continue;
5267 }
5268 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5269 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5270 {
5271 bgotonextstatement=false;
5272 {
5273 IkReal j3eval[3];
5274 IkReal x385=((-1.0)*new_r01);
5275 CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5276 if(!x388.valid){
5277 continue;
5278 }
5279 IkReal x386=((-1.0)*(x388.value));
5280 IkReal x387=x381;
5281 sj4=0;
5282 cj4=1.0;
5283 j4=0;
5284 sj5=gconst1;
5285 cj5=gconst2;
5286 j5=x386;
5287 IkReal gconst0=x386;
5288 IkReal gconst1=(new_r11*x387);
5289 IkReal gconst2=(x385*x387);
5290 IkReal x389=new_r11*new_r11;
5291 IkReal x390=(new_r10*new_r11);
5292 IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5293 IkReal x392=x381;
5294 IkReal x393=(new_r11*x392);
5295 j3eval[0]=x391;
5296 j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5297 j3eval[2]=IKsign(x391);
5298 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5299 {
5300 {
5301 IkReal j3eval[1];
5302 IkReal x394=((-1.0)*new_r01);
5303 CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5304 if(!x397.valid){
5305 continue;
5306 }
5307 IkReal x395=((-1.0)*(x397.value));
5308 IkReal x396=x381;
5309 sj4=0;
5310 cj4=1.0;
5311 j4=0;
5312 sj5=gconst1;
5313 cj5=gconst2;
5314 j5=x395;
5315 IkReal gconst0=x395;
5316 IkReal gconst1=(new_r11*x396);
5317 IkReal gconst2=(x394*x396);
5318 IkReal x398=new_r11*new_r11;
5319 CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5320 if(!x401.valid){
5321 continue;
5322 }
5323 IkReal x399=x401.value;
5324 IkReal x400=(x398*x399);
5325 j3eval[0]=((IKabs((((new_r01*new_r10))+x400)))+(IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5326 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5327 {
5328 {
5329 IkReal j3eval[1];
5330 IkReal x402=((-1.0)*new_r01);
5331 CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5332 if(!x405.valid){
5333 continue;
5334 }
5335 IkReal x403=((-1.0)*(x405.value));
5336 IkReal x404=x381;
5337 sj4=0;
5338 cj4=1.0;
5339 j4=0;
5340 sj5=gconst1;
5341 cj5=gconst2;
5342 j5=x403;
5343 IkReal gconst0=x403;
5344 IkReal gconst1=(new_r11*x404);
5345 IkReal gconst2=(x402*x404);
5346 IkReal x406=new_r01*new_r01;
5347 IkReal x407=new_r11*new_r11;
5348 CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5349 if(!x414.valid){
5350 continue;
5351 }
5352 IkReal x408=x414.value;
5353 IkReal x409=(x407*x408);
5354 CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5355 if(!x415.valid){
5356 continue;
5357 }
5358 IkReal x410=x415.value;
5359 IkReal x411=((1.0)*x410);
5360 IkReal x412=(new_r11*x411);
5361 IkReal x413=(new_r01*x411);
5362 j3eval[0]=((IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5363 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5364 {
5365 {
5366 IkReal evalcond[3];
5367 bool bgotonextstatement = true;
5368 do
5369 {
5370 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5371 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5372 {
5373 bgotonextstatement=false;
5374 {
5375 IkReal j3array[2], cj3array[2], sj3array[2];
5376 bool j3valid[2]={false};
5377 _nj3 = 2;
5378 CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst2,-1);
5379 if(!x416.valid){
5380 continue;
5381 }
5382 sj3array[0]=(new_r10*(x416.value));
5383 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5384 {
5385  j3valid[0] = j3valid[1] = true;
5386  j3array[0] = IKasin(sj3array[0]);
5387  cj3array[0] = IKcos(j3array[0]);
5388  sj3array[1] = sj3array[0];
5389  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5390  cj3array[1] = -cj3array[0];
5391 }
5392 else if( isnan(sj3array[0]) )
5393 {
5394  // probably any value will work
5395  j3valid[0] = true;
5396  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5397 }
5398 for(int ij3 = 0; ij3 < 2; ++ij3)
5399 {
5400 if( !j3valid[ij3] )
5401 {
5402  continue;
5403 }
5404 _ij3[0] = ij3; _ij3[1] = -1;
5405 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5406 {
5407 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5408 {
5409  j3valid[iij3]=false; _ij3[1] = iij3; break;
5410 }
5411 }
5412 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5413 {
5414 IkReal evalcond[6];
5415 IkReal x417=IKcos(j3);
5416 IkReal x418=IKsin(j3);
5417 IkReal x419=((-1.0)*x417);
5418 evalcond[0]=(new_r01*x417);
5419 evalcond[1]=(new_r10*x419);
5420 evalcond[2]=(gconst2*x419);
5421 evalcond[3]=(gconst2+((new_r01*x418)));
5422 evalcond[4]=(((gconst2*x418))+new_r01);
5423 evalcond[5]=((((-1.0)*gconst2))+((new_r10*x418)));
5424 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5425 {
5426 continue;
5427 }
5428 }
5429 
5430 {
5431 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5432 vinfos[0].jointtype = 1;
5433 vinfos[0].foffset = j0;
5434 vinfos[0].indices[0] = _ij0[0];
5435 vinfos[0].indices[1] = _ij0[1];
5436 vinfos[0].maxsolutions = _nj0;
5437 vinfos[1].jointtype = 1;
5438 vinfos[1].foffset = j1;
5439 vinfos[1].indices[0] = _ij1[0];
5440 vinfos[1].indices[1] = _ij1[1];
5441 vinfos[1].maxsolutions = _nj1;
5442 vinfos[2].jointtype = 1;
5443 vinfos[2].foffset = j2;
5444 vinfos[2].indices[0] = _ij2[0];
5445 vinfos[2].indices[1] = _ij2[1];
5446 vinfos[2].maxsolutions = _nj2;
5447 vinfos[3].jointtype = 1;
5448 vinfos[3].foffset = j3;
5449 vinfos[3].indices[0] = _ij3[0];
5450 vinfos[3].indices[1] = _ij3[1];
5451 vinfos[3].maxsolutions = _nj3;
5452 vinfos[4].jointtype = 1;
5453 vinfos[4].foffset = j4;
5454 vinfos[4].indices[0] = _ij4[0];
5455 vinfos[4].indices[1] = _ij4[1];
5456 vinfos[4].maxsolutions = _nj4;
5457 vinfos[5].jointtype = 1;
5458 vinfos[5].foffset = j5;
5459 vinfos[5].indices[0] = _ij5[0];
5460 vinfos[5].indices[1] = _ij5[1];
5461 vinfos[5].maxsolutions = _nj5;
5462 std::vector<int> vfree(0);
5463 solutions.AddSolution(vinfos,vfree);
5464 }
5465 }
5466 }
5467 
5468 }
5469 } while(0);
5470 if( bgotonextstatement )
5471 {
5472 bool bgotonextstatement = true;
5473 do
5474 {
5475 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5476 evalcond[1]=gconst1;
5477 evalcond[2]=gconst2;
5478 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
5479 {
5480 bgotonextstatement=false;
5481 {
5482 IkReal j3eval[3];
5483 IkReal x420=((-1.0)*new_r01);
5484 CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5485 if(!x422.valid){
5486 continue;
5487 }
5488 IkReal x421=((-1.0)*(x422.value));
5489 sj4=0;
5490 cj4=1.0;
5491 j4=0;
5492 sj5=gconst1;
5493 cj5=gconst2;
5494 j5=x421;
5495 new_r00=0;
5496 new_r10=0;
5497 new_r21=0;
5498 new_r22=0;
5499 IkReal gconst0=x421;
5500 IkReal gconst1=new_r11;
5501 IkReal gconst2=x420;
5502 j3eval[0]=-1.0;
5503 j3eval[1]=-1.0;
5504 j3eval[2]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5505 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5506 {
5507 {
5508 IkReal j3eval[3];
5509 IkReal x423=((-1.0)*new_r01);
5510 CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5511 if(!x425.valid){
5512 continue;
5513 }
5514 IkReal x424=((-1.0)*(x425.value));
5515 sj4=0;
5516 cj4=1.0;
5517 j4=0;
5518 sj5=gconst1;
5519 cj5=gconst2;
5520 j5=x424;
5521 new_r00=0;
5522 new_r10=0;
5523 new_r21=0;
5524 new_r22=0;
5525 IkReal gconst0=x424;
5526 IkReal gconst1=new_r11;
5527 IkReal gconst2=x423;
5528 j3eval[0]=-1.0;
5529 j3eval[1]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5530 j3eval[2]=-1.0;
5531 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5532 {
5533 {
5534 IkReal j3eval[3];
5535 IkReal x426=((-1.0)*new_r01);
5536 CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5537 if(!x428.valid){
5538 continue;
5539 }
5540 IkReal x427=((-1.0)*(x428.value));
5541 sj4=0;
5542 cj4=1.0;
5543 j4=0;
5544 sj5=gconst1;
5545 cj5=gconst2;
5546 j5=x427;
5547 new_r00=0;
5548 new_r10=0;
5549 new_r21=0;
5550 new_r22=0;
5551 IkReal gconst0=x427;
5552 IkReal gconst1=new_r11;
5553 IkReal gconst2=x426;
5554 j3eval[0]=1.0;
5555 j3eval[1]=1.0;
5556 j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5557 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5558 {
5559 continue; // 3 cases reached
5560 
5561 } else
5562 {
5563 {
5564 IkReal j3array[1], cj3array[1], sj3array[1];
5565 bool j3valid[1]={false};
5566 _nj3 = 1;
5567 IkReal x429=((1.0)*new_r01);
5568 CheckValue<IkReal> x430 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x429))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
5569 if(!x430.valid){
5570 continue;
5571 }
5572 CheckValue<IkReal> x431=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x429)))),-1);
5573 if(!x431.valid){
5574 continue;
5575 }
5576 j3array[0]=((-1.5707963267949)+(x430.value)+(((1.5707963267949)*(x431.value))));
5577 sj3array[0]=IKsin(j3array[0]);
5578 cj3array[0]=IKcos(j3array[0]);
5579 if( j3array[0] > IKPI )
5580 {
5581  j3array[0]-=IK2PI;
5582 }
5583 else if( j3array[0] < -IKPI )
5584 { j3array[0]+=IK2PI;
5585 }
5586 j3valid[0] = true;
5587 for(int ij3 = 0; ij3 < 1; ++ij3)
5588 {
5589 if( !j3valid[ij3] )
5590 {
5591  continue;
5592 }
5593 _ij3[0] = ij3; _ij3[1] = -1;
5594 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5595 {
5596 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5597 {
5598  j3valid[iij3]=false; _ij3[1] = iij3; break;
5599 }
5600 }
5601 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5602 {
5603 IkReal evalcond[6];
5604 IkReal x432=IKcos(j3);
5605 IkReal x433=IKsin(j3);
5606 IkReal x434=(gconst1*x433);
5607 IkReal x435=(gconst2*x433);
5608 IkReal x436=((1.0)*x432);
5609 IkReal x437=(gconst2*x436);
5610 evalcond[0]=(((new_r01*x432))+gconst1+((new_r11*x433)));
5611 evalcond[1]=(((gconst1*x432))+x435+new_r01);
5612 evalcond[2]=((((-1.0)*x437))+x434);
5613 evalcond[3]=(((new_r01*x433))+gconst2+(((-1.0)*new_r11*x436)));
5614 evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5615 evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst1*x436)));
5616 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5617 {
5618 continue;
5619 }
5620 }
5621 
5622 {
5623 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5624 vinfos[0].jointtype = 1;
5625 vinfos[0].foffset = j0;
5626 vinfos[0].indices[0] = _ij0[0];
5627 vinfos[0].indices[1] = _ij0[1];
5628 vinfos[0].maxsolutions = _nj0;
5629 vinfos[1].jointtype = 1;
5630 vinfos[1].foffset = j1;
5631 vinfos[1].indices[0] = _ij1[0];
5632 vinfos[1].indices[1] = _ij1[1];
5633 vinfos[1].maxsolutions = _nj1;
5634 vinfos[2].jointtype = 1;
5635 vinfos[2].foffset = j2;
5636 vinfos[2].indices[0] = _ij2[0];
5637 vinfos[2].indices[1] = _ij2[1];
5638 vinfos[2].maxsolutions = _nj2;
5639 vinfos[3].jointtype = 1;
5640 vinfos[3].foffset = j3;
5641 vinfos[3].indices[0] = _ij3[0];
5642 vinfos[3].indices[1] = _ij3[1];
5643 vinfos[3].maxsolutions = _nj3;
5644 vinfos[4].jointtype = 1;
5645 vinfos[4].foffset = j4;
5646 vinfos[4].indices[0] = _ij4[0];
5647 vinfos[4].indices[1] = _ij4[1];
5648 vinfos[4].maxsolutions = _nj4;
5649 vinfos[5].jointtype = 1;
5650 vinfos[5].foffset = j5;
5651 vinfos[5].indices[0] = _ij5[0];
5652 vinfos[5].indices[1] = _ij5[1];
5653 vinfos[5].maxsolutions = _nj5;
5654 std::vector<int> vfree(0);
5655 solutions.AddSolution(vinfos,vfree);
5656 }
5657 }
5658 }
5659 
5660 }
5661 
5662 }
5663 
5664 } else
5665 {
5666 {
5667 IkReal j3array[1], cj3array[1], sj3array[1];
5668 bool j3valid[1]={false};
5669 _nj3 = 1;
5670 CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst2*gconst2)))+(((-1.0)*(gconst1*gconst1))))),-1);
5671 if(!x438.valid){
5672 continue;
5673 }
5674 CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst2*new_r01)),IkReal((gconst1*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5675 if(!x439.valid){
5676 continue;
5677 }
5678 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5679 sj3array[0]=IKsin(j3array[0]);
5680 cj3array[0]=IKcos(j3array[0]);
5681 if( j3array[0] > IKPI )
5682 {
5683  j3array[0]-=IK2PI;
5684 }
5685 else if( j3array[0] < -IKPI )
5686 { j3array[0]+=IK2PI;
5687 }
5688 j3valid[0] = true;
5689 for(int ij3 = 0; ij3 < 1; ++ij3)
5690 {
5691 if( !j3valid[ij3] )
5692 {
5693  continue;
5694 }
5695 _ij3[0] = ij3; _ij3[1] = -1;
5696 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5697 {
5698 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5699 {
5700  j3valid[iij3]=false; _ij3[1] = iij3; break;
5701 }
5702 }
5703 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5704 {
5705 IkReal evalcond[6];
5706 IkReal x440=IKcos(j3);
5707 IkReal x441=IKsin(j3);
5708 IkReal x442=(gconst1*x441);
5709 IkReal x443=(gconst2*x441);
5710 IkReal x444=((1.0)*x440);
5711 IkReal x445=(gconst2*x444);
5712 evalcond[0]=(((new_r01*x440))+gconst1+((new_r11*x441)));
5713 evalcond[1]=(((gconst1*x440))+x443+new_r01);
5714 evalcond[2]=((((-1.0)*x445))+x442);
5715 evalcond[3]=(((new_r01*x441))+gconst2+(((-1.0)*new_r11*x444)));
5716 evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5717 evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst1*x444)));
5718 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5719 {
5720 continue;
5721 }
5722 }
5723 
5724 {
5725 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5726 vinfos[0].jointtype = 1;
5727 vinfos[0].foffset = j0;
5728 vinfos[0].indices[0] = _ij0[0];
5729 vinfos[0].indices[1] = _ij0[1];
5730 vinfos[0].maxsolutions = _nj0;
5731 vinfos[1].jointtype = 1;
5732 vinfos[1].foffset = j1;
5733 vinfos[1].indices[0] = _ij1[0];
5734 vinfos[1].indices[1] = _ij1[1];
5735 vinfos[1].maxsolutions = _nj1;
5736 vinfos[2].jointtype = 1;
5737 vinfos[2].foffset = j2;
5738 vinfos[2].indices[0] = _ij2[0];
5739 vinfos[2].indices[1] = _ij2[1];
5740 vinfos[2].maxsolutions = _nj2;
5741 vinfos[3].jointtype = 1;
5742 vinfos[3].foffset = j3;
5743 vinfos[3].indices[0] = _ij3[0];
5744 vinfos[3].indices[1] = _ij3[1];
5745 vinfos[3].maxsolutions = _nj3;
5746 vinfos[4].jointtype = 1;
5747 vinfos[4].foffset = j4;
5748 vinfos[4].indices[0] = _ij4[0];
5749 vinfos[4].indices[1] = _ij4[1];
5750 vinfos[4].maxsolutions = _nj4;
5751 vinfos[5].jointtype = 1;
5752 vinfos[5].foffset = j5;
5753 vinfos[5].indices[0] = _ij5[0];
5754 vinfos[5].indices[1] = _ij5[1];
5755 vinfos[5].maxsolutions = _nj5;
5756 std::vector<int> vfree(0);
5757 solutions.AddSolution(vinfos,vfree);
5758 }
5759 }
5760 }
5761 
5762 }
5763 
5764 }
5765 
5766 } else
5767 {
5768 {
5769 IkReal j3array[1], cj3array[1], sj3array[1];
5770 bool j3valid[1]={false};
5771 _nj3 = 1;
5772 CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(gconst1*gconst1),IkReal(((-1.0)*gconst1*gconst2)),IKFAST_ATAN2_MAGTHRESH);
5773 if(!x446.valid){
5774 continue;
5775 }
5776 CheckValue<IkReal> x447=IKPowWithIntegerCheck(IKsign((((gconst2*new_r01))+(((-1.0)*gconst1*new_r11)))),-1);
5777 if(!x447.valid){
5778 continue;
5779 }
5780 j3array[0]=((-1.5707963267949)+(x446.value)+(((1.5707963267949)*(x447.value))));
5781 sj3array[0]=IKsin(j3array[0]);
5782 cj3array[0]=IKcos(j3array[0]);
5783 if( j3array[0] > IKPI )
5784 {
5785  j3array[0]-=IK2PI;
5786 }
5787 else if( j3array[0] < -IKPI )
5788 { j3array[0]+=IK2PI;
5789 }
5790 j3valid[0] = true;
5791 for(int ij3 = 0; ij3 < 1; ++ij3)
5792 {
5793 if( !j3valid[ij3] )
5794 {
5795  continue;
5796 }
5797 _ij3[0] = ij3; _ij3[1] = -1;
5798 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5799 {
5800 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5801 {
5802  j3valid[iij3]=false; _ij3[1] = iij3; break;
5803 }
5804 }
5805 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5806 {
5807 IkReal evalcond[6];
5808 IkReal x448=IKcos(j3);
5809 IkReal x449=IKsin(j3);
5810 IkReal x450=(gconst1*x449);
5811 IkReal x451=(gconst2*x449);
5812 IkReal x452=((1.0)*x448);
5813 IkReal x453=(gconst2*x452);
5814 evalcond[0]=(((new_r01*x448))+gconst1+((new_r11*x449)));
5815 evalcond[1]=(((gconst1*x448))+x451+new_r01);
5816 evalcond[2]=((((-1.0)*x453))+x450);
5817 evalcond[3]=(((new_r01*x449))+gconst2+(((-1.0)*new_r11*x452)));
5818 evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5819 evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst1*x452)));
5820 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5821 {
5822 continue;
5823 }
5824 }
5825 
5826 {
5827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5828 vinfos[0].jointtype = 1;
5829 vinfos[0].foffset = j0;
5830 vinfos[0].indices[0] = _ij0[0];
5831 vinfos[0].indices[1] = _ij0[1];
5832 vinfos[0].maxsolutions = _nj0;
5833 vinfos[1].jointtype = 1;
5834 vinfos[1].foffset = j1;
5835 vinfos[1].indices[0] = _ij1[0];
5836 vinfos[1].indices[1] = _ij1[1];
5837 vinfos[1].maxsolutions = _nj1;
5838 vinfos[2].jointtype = 1;
5839 vinfos[2].foffset = j2;
5840 vinfos[2].indices[0] = _ij2[0];
5841 vinfos[2].indices[1] = _ij2[1];
5842 vinfos[2].maxsolutions = _nj2;
5843 vinfos[3].jointtype = 1;
5844 vinfos[3].foffset = j3;
5845 vinfos[3].indices[0] = _ij3[0];
5846 vinfos[3].indices[1] = _ij3[1];
5847 vinfos[3].maxsolutions = _nj3;
5848 vinfos[4].jointtype = 1;
5849 vinfos[4].foffset = j4;
5850 vinfos[4].indices[0] = _ij4[0];
5851 vinfos[4].indices[1] = _ij4[1];
5852 vinfos[4].maxsolutions = _nj4;
5853 vinfos[5].jointtype = 1;
5854 vinfos[5].foffset = j5;
5855 vinfos[5].indices[0] = _ij5[0];
5856 vinfos[5].indices[1] = _ij5[1];
5857 vinfos[5].maxsolutions = _nj5;
5858 std::vector<int> vfree(0);
5859 solutions.AddSolution(vinfos,vfree);
5860 }
5861 }
5862 }
5863 
5864 }
5865 
5866 }
5867 
5868 }
5869 } while(0);
5870 if( bgotonextstatement )
5871 {
5872 bool bgotonextstatement = true;
5873 do
5874 {
5875 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5876 if( IKabs(evalcond[0]) < 0.0000050000000000 )
5877 {
5878 bgotonextstatement=false;
5879 {
5880 IkReal j3eval[1];
5881 CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5882 if(!x455.valid){
5883 continue;
5884 }
5885 IkReal x454=((-1.0)*(x455.value));
5886 sj4=0;
5887 cj4=1.0;
5888 j4=0;
5889 sj5=gconst1;
5890 cj5=gconst2;
5891 j5=x454;
5892 new_r01=0;
5893 new_r10=0;
5894 IkReal gconst0=x454;
5895 IkReal x456 = new_r11*new_r11;
5896 if(IKabs(x456)==0){
5897 continue;
5898 }
5899 IkReal gconst1=(new_r11*(pow(x456,-0.5)));
5900 IkReal gconst2=0;
5901 j3eval[0]=new_r00;
5902 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5903 {
5904 {
5905 IkReal j3eval[1];
5906 CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5907 if(!x458.valid){
5908 continue;
5909 }
5910 IkReal x457=((-1.0)*(x458.value));
5911 sj4=0;
5912 cj4=1.0;
5913 j4=0;
5914 sj5=gconst1;
5915 cj5=gconst2;
5916 j5=x457;
5917 new_r01=0;
5918 new_r10=0;
5919 IkReal gconst0=x457;
5920 IkReal x459 = new_r11*new_r11;
5921 if(IKabs(x459)==0){
5922 continue;
5923 }
5924 IkReal gconst1=(new_r11*(pow(x459,-0.5)));
5925 IkReal gconst2=0;
5926 j3eval[0]=new_r11;
5927 if( IKabs(j3eval[0]) < 0.0000010000000000 )
5928 {
5929 {
5930 IkReal j3array[2], cj3array[2], sj3array[2];
5931 bool j3valid[2]={false};
5932 _nj3 = 2;
5933 CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst1,-1);
5934 if(!x460.valid){
5935 continue;
5936 }
5937 sj3array[0]=((-1.0)*new_r00*(x460.value));
5938 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5939 {
5940  j3valid[0] = j3valid[1] = true;
5941  j3array[0] = IKasin(sj3array[0]);
5942  cj3array[0] = IKcos(j3array[0]);
5943  sj3array[1] = sj3array[0];
5944  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5945  cj3array[1] = -cj3array[0];
5946 }
5947 else if( isnan(sj3array[0]) )
5948 {
5949  // probably any value will work
5950  j3valid[0] = true;
5951  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5952 }
5953 for(int ij3 = 0; ij3 < 2; ++ij3)
5954 {
5955 if( !j3valid[ij3] )
5956 {
5957  continue;
5958 }
5959 _ij3[0] = ij3; _ij3[1] = -1;
5960 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5961 {
5962 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5963 {
5964  j3valid[iij3]=false; _ij3[1] = iij3; break;
5965 }
5966 }
5967 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5968 {
5969 IkReal evalcond[6];
5970 IkReal x461=IKcos(j3);
5971 IkReal x462=IKsin(j3);
5972 evalcond[0]=(gconst1*x461);
5973 evalcond[1]=(new_r00*x461);
5974 evalcond[2]=((-1.0)*new_r11*x461);
5975 evalcond[3]=(((new_r00*x462))+gconst1);
5976 evalcond[4]=(((new_r11*x462))+gconst1);
5977 evalcond[5]=(((gconst1*x462))+new_r11);
5978 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5979 {
5980 continue;
5981 }
5982 }
5983 
5984 {
5985 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5986 vinfos[0].jointtype = 1;
5987 vinfos[0].foffset = j0;
5988 vinfos[0].indices[0] = _ij0[0];
5989 vinfos[0].indices[1] = _ij0[1];
5990 vinfos[0].maxsolutions = _nj0;
5991 vinfos[1].jointtype = 1;
5992 vinfos[1].foffset = j1;
5993 vinfos[1].indices[0] = _ij1[0];
5994 vinfos[1].indices[1] = _ij1[1];
5995 vinfos[1].maxsolutions = _nj1;
5996 vinfos[2].jointtype = 1;
5997 vinfos[2].foffset = j2;
5998 vinfos[2].indices[0] = _ij2[0];
5999 vinfos[2].indices[1] = _ij2[1];
6000 vinfos[2].maxsolutions = _nj2;
6001 vinfos[3].jointtype = 1;
6002 vinfos[3].foffset = j3;
6003 vinfos[3].indices[0] = _ij3[0];
6004 vinfos[3].indices[1] = _ij3[1];
6005 vinfos[3].maxsolutions = _nj3;
6006 vinfos[4].jointtype = 1;
6007 vinfos[4].foffset = j4;
6008 vinfos[4].indices[0] = _ij4[0];
6009 vinfos[4].indices[1] = _ij4[1];
6010 vinfos[4].maxsolutions = _nj4;
6011 vinfos[5].jointtype = 1;
6012 vinfos[5].foffset = j5;
6013 vinfos[5].indices[0] = _ij5[0];
6014 vinfos[5].indices[1] = _ij5[1];
6015 vinfos[5].maxsolutions = _nj5;
6016 std::vector<int> vfree(0);
6017 solutions.AddSolution(vinfos,vfree);
6018 }
6019 }
6020 }
6021 
6022 } else
6023 {
6024 {
6025 IkReal j3array[2], cj3array[2], sj3array[2];
6026 bool j3valid[2]={false};
6027 _nj3 = 2;
6028 CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
6029 if(!x463.valid){
6030 continue;
6031 }
6032 sj3array[0]=((-1.0)*gconst1*(x463.value));
6033 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6034 {
6035  j3valid[0] = j3valid[1] = true;
6036  j3array[0] = IKasin(sj3array[0]);
6037  cj3array[0] = IKcos(j3array[0]);
6038  sj3array[1] = sj3array[0];
6039  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6040  cj3array[1] = -cj3array[0];
6041 }
6042 else if( isnan(sj3array[0]) )
6043 {
6044  // probably any value will work
6045  j3valid[0] = true;
6046  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6047 }
6048 for(int ij3 = 0; ij3 < 2; ++ij3)
6049 {
6050 if( !j3valid[ij3] )
6051 {
6052  continue;
6053 }
6054 _ij3[0] = ij3; _ij3[1] = -1;
6055 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6056 {
6057 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6058 {
6059  j3valid[iij3]=false; _ij3[1] = iij3; break;
6060 }
6061 }
6062 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6063 {
6064 IkReal evalcond[6];
6065 IkReal x464=IKcos(j3);
6066 IkReal x465=IKsin(j3);
6067 IkReal x466=(gconst1*x465);
6068 evalcond[0]=(gconst1*x464);
6069 evalcond[1]=(new_r00*x464);
6070 evalcond[2]=((-1.0)*new_r11*x464);
6071 evalcond[3]=(((new_r00*x465))+gconst1);
6072 evalcond[4]=(x466+new_r00);
6073 evalcond[5]=(x466+new_r11);
6074 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6075 {
6076 continue;
6077 }
6078 }
6079 
6080 {
6081 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6082 vinfos[0].jointtype = 1;
6083 vinfos[0].foffset = j0;
6084 vinfos[0].indices[0] = _ij0[0];
6085 vinfos[0].indices[1] = _ij0[1];
6086 vinfos[0].maxsolutions = _nj0;
6087 vinfos[1].jointtype = 1;
6088 vinfos[1].foffset = j1;
6089 vinfos[1].indices[0] = _ij1[0];
6090 vinfos[1].indices[1] = _ij1[1];
6091 vinfos[1].maxsolutions = _nj1;
6092 vinfos[2].jointtype = 1;
6093 vinfos[2].foffset = j2;
6094 vinfos[2].indices[0] = _ij2[0];
6095 vinfos[2].indices[1] = _ij2[1];
6096 vinfos[2].maxsolutions = _nj2;
6097 vinfos[3].jointtype = 1;
6098 vinfos[3].foffset = j3;
6099 vinfos[3].indices[0] = _ij3[0];
6100 vinfos[3].indices[1] = _ij3[1];
6101 vinfos[3].maxsolutions = _nj3;
6102 vinfos[4].jointtype = 1;
6103 vinfos[4].foffset = j4;
6104 vinfos[4].indices[0] = _ij4[0];
6105 vinfos[4].indices[1] = _ij4[1];
6106 vinfos[4].maxsolutions = _nj4;
6107 vinfos[5].jointtype = 1;
6108 vinfos[5].foffset = j5;
6109 vinfos[5].indices[0] = _ij5[0];
6110 vinfos[5].indices[1] = _ij5[1];
6111 vinfos[5].maxsolutions = _nj5;
6112 std::vector<int> vfree(0);
6113 solutions.AddSolution(vinfos,vfree);
6114 }
6115 }
6116 }
6117 
6118 }
6119 
6120 }
6121 
6122 } else
6123 {
6124 {
6125 IkReal j3array[2], cj3array[2], sj3array[2];
6126 bool j3valid[2]={false};
6127 _nj3 = 2;
6128 CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6129 if(!x467.valid){
6130 continue;
6131 }
6132 sj3array[0]=((-1.0)*gconst1*(x467.value));
6133 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6134 {
6135  j3valid[0] = j3valid[1] = true;
6136  j3array[0] = IKasin(sj3array[0]);
6137  cj3array[0] = IKcos(j3array[0]);
6138  sj3array[1] = sj3array[0];
6139  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6140  cj3array[1] = -cj3array[0];
6141 }
6142 else if( isnan(sj3array[0]) )
6143 {
6144  // probably any value will work
6145  j3valid[0] = true;
6146  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6147 }
6148 for(int ij3 = 0; ij3 < 2; ++ij3)
6149 {
6150 if( !j3valid[ij3] )
6151 {
6152  continue;
6153 }
6154 _ij3[0] = ij3; _ij3[1] = -1;
6155 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6156 {
6157 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6158 {
6159  j3valid[iij3]=false; _ij3[1] = iij3; break;
6160 }
6161 }
6162 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6163 {
6164 IkReal evalcond[6];
6165 IkReal x468=IKcos(j3);
6166 IkReal x469=IKsin(j3);
6167 IkReal x470=(gconst1*x469);
6168 evalcond[0]=(gconst1*x468);
6169 evalcond[1]=(new_r00*x468);
6170 evalcond[2]=((-1.0)*new_r11*x468);
6171 evalcond[3]=(((new_r11*x469))+gconst1);
6172 evalcond[4]=(x470+new_r00);
6173 evalcond[5]=(x470+new_r11);
6174 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6175 {
6176 continue;
6177 }
6178 }
6179 
6180 {
6181 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6182 vinfos[0].jointtype = 1;
6183 vinfos[0].foffset = j0;
6184 vinfos[0].indices[0] = _ij0[0];
6185 vinfos[0].indices[1] = _ij0[1];
6186 vinfos[0].maxsolutions = _nj0;
6187 vinfos[1].jointtype = 1;
6188 vinfos[1].foffset = j1;
6189 vinfos[1].indices[0] = _ij1[0];
6190 vinfos[1].indices[1] = _ij1[1];
6191 vinfos[1].maxsolutions = _nj1;
6192 vinfos[2].jointtype = 1;
6193 vinfos[2].foffset = j2;
6194 vinfos[2].indices[0] = _ij2[0];
6195 vinfos[2].indices[1] = _ij2[1];
6196 vinfos[2].maxsolutions = _nj2;
6197 vinfos[3].jointtype = 1;
6198 vinfos[3].foffset = j3;
6199 vinfos[3].indices[0] = _ij3[0];
6200 vinfos[3].indices[1] = _ij3[1];
6201 vinfos[3].maxsolutions = _nj3;
6202 vinfos[4].jointtype = 1;
6203 vinfos[4].foffset = j4;
6204 vinfos[4].indices[0] = _ij4[0];
6205 vinfos[4].indices[1] = _ij4[1];
6206 vinfos[4].maxsolutions = _nj4;
6207 vinfos[5].jointtype = 1;
6208 vinfos[5].foffset = j5;
6209 vinfos[5].indices[0] = _ij5[0];
6210 vinfos[5].indices[1] = _ij5[1];
6211 vinfos[5].maxsolutions = _nj5;
6212 std::vector<int> vfree(0);
6213 solutions.AddSolution(vinfos,vfree);
6214 }
6215 }
6216 }
6217 
6218 }
6219 
6220 }
6221 
6222 }
6223 } while(0);
6224 if( bgotonextstatement )
6225 {
6226 bool bgotonextstatement = true;
6227 do
6228 {
6229 evalcond[0]=IKabs(new_r11);
6230 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6231 {
6232 bgotonextstatement=false;
6233 {
6234 IkReal j3eval[1];
6235 IkReal x471=((-1.0)*new_r01);
6236 CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6237 if(!x473.valid){
6238 continue;
6239 }
6240 IkReal x472=((-1.0)*(x473.value));
6241 sj4=0;
6242 cj4=1.0;
6243 j4=0;
6244 sj5=gconst1;
6245 cj5=gconst2;
6246 j5=x472;
6247 new_r11=0;
6248 IkReal gconst0=x472;
6249 IkReal gconst1=0;
6250 IkReal x474 = new_r01*new_r01;
6251 if(IKabs(x474)==0){
6252 continue;
6253 }
6254 IkReal gconst2=(x471*(pow(x474,-0.5)));
6255 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6256 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6257 {
6258 {
6259 IkReal j3eval[1];
6260 IkReal x475=((-1.0)*new_r01);
6261 CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6262 if(!x477.valid){
6263 continue;
6264 }
6265 IkReal x476=((-1.0)*(x477.value));
6266 sj4=0;
6267 cj4=1.0;
6268 j4=0;
6269 sj5=gconst1;
6270 cj5=gconst2;
6271 j5=x476;
6272 new_r11=0;
6273 IkReal gconst0=x476;
6274 IkReal gconst1=0;
6275 IkReal x478 = new_r01*new_r01;
6276 if(IKabs(x478)==0){
6277 continue;
6278 }
6279 IkReal gconst2=(x475*(pow(x478,-0.5)));
6280 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6281 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6282 {
6283 {
6284 IkReal j3eval[1];
6285 IkReal x479=((-1.0)*new_r01);
6286 CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal(0),IkReal(x479),IKFAST_ATAN2_MAGTHRESH);
6287 if(!x481.valid){
6288 continue;
6289 }
6290 IkReal x480=((-1.0)*(x481.value));
6291 sj4=0;
6292 cj4=1.0;
6293 j4=0;
6294 sj5=gconst1;
6295 cj5=gconst2;
6296 j5=x480;
6297 new_r11=0;
6298 IkReal gconst0=x480;
6299 IkReal gconst1=0;
6300 IkReal x482 = new_r01*new_r01;
6301 if(IKabs(x482)==0){
6302 continue;
6303 }
6304 IkReal gconst2=(x479*(pow(x482,-0.5)));
6305 j3eval[0]=new_r01;
6306 if( IKabs(j3eval[0]) < 0.0000010000000000 )
6307 {
6308 continue; // 3 cases reached
6309 
6310 } else
6311 {
6312 {
6313 IkReal j3array[1], cj3array[1], sj3array[1];
6314 bool j3valid[1]={false};
6315 _nj3 = 1;
6316 CheckValue<IkReal> x483=IKPowWithIntegerCheck(new_r01,-1);
6317 if(!x483.valid){
6318 continue;
6319 }
6320 CheckValue<IkReal> x484=IKPowWithIntegerCheck(gconst2,-1);
6321 if(!x484.valid){
6322 continue;
6323 }
6324 if( IKabs(((-1.0)*gconst2*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x484.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst2*(x483.value)))+IKsqr((new_r00*(x484.value)))-1) <= IKFAST_SINCOS_THRESH )
6325  continue;
6326 j3array[0]=IKatan2(((-1.0)*gconst2*(x483.value)), (new_r00*(x484.value)));
6327 sj3array[0]=IKsin(j3array[0]);
6328 cj3array[0]=IKcos(j3array[0]);
6329 if( j3array[0] > IKPI )
6330 {
6331  j3array[0]-=IK2PI;
6332 }
6333 else if( j3array[0] < -IKPI )
6334 { j3array[0]+=IK2PI;
6335 }
6336 j3valid[0] = true;
6337 for(int ij3 = 0; ij3 < 1; ++ij3)
6338 {
6339 if( !j3valid[ij3] )
6340 {
6341  continue;
6342 }
6343 _ij3[0] = ij3; _ij3[1] = -1;
6344 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6345 {
6346 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6347 {
6348  j3valid[iij3]=false; _ij3[1] = iij3; break;
6349 }
6350 }
6351 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6352 {
6353 IkReal evalcond[8];
6354 IkReal x485=IKcos(j3);
6355 IkReal x486=IKsin(j3);
6356 IkReal x487=((1.0)*gconst2);
6357 IkReal x488=(gconst2*x486);
6358 evalcond[0]=(new_r01*x485);
6359 evalcond[1]=((-1.0)*gconst2*x485);
6360 evalcond[2]=(gconst2+((new_r01*x486)));
6361 evalcond[3]=(x488+new_r01);
6362 evalcond[4]=(new_r00+(((-1.0)*x485*x487)));
6363 evalcond[5]=((((-1.0)*x486*x487))+new_r10);
6364 evalcond[6]=((((-1.0)*new_r10*x485))+((new_r00*x486)));
6365 evalcond[7]=((((-1.0)*x487))+((new_r10*x486))+((new_r00*x485)));
6366 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6367 {
6368 continue;
6369 }
6370 }
6371 
6372 {
6373 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6374 vinfos[0].jointtype = 1;
6375 vinfos[0].foffset = j0;
6376 vinfos[0].indices[0] = _ij0[0];
6377 vinfos[0].indices[1] = _ij0[1];
6378 vinfos[0].maxsolutions = _nj0;
6379 vinfos[1].jointtype = 1;
6380 vinfos[1].foffset = j1;
6381 vinfos[1].indices[0] = _ij1[0];
6382 vinfos[1].indices[1] = _ij1[1];
6383 vinfos[1].maxsolutions = _nj1;
6384 vinfos[2].jointtype = 1;
6385 vinfos[2].foffset = j2;
6386 vinfos[2].indices[0] = _ij2[0];
6387 vinfos[2].indices[1] = _ij2[1];
6388 vinfos[2].maxsolutions = _nj2;
6389 vinfos[3].jointtype = 1;
6390 vinfos[3].foffset = j3;
6391 vinfos[3].indices[0] = _ij3[0];
6392 vinfos[3].indices[1] = _ij3[1];
6393 vinfos[3].maxsolutions = _nj3;
6394 vinfos[4].jointtype = 1;
6395 vinfos[4].foffset = j4;
6396 vinfos[4].indices[0] = _ij4[0];
6397 vinfos[4].indices[1] = _ij4[1];
6398 vinfos[4].maxsolutions = _nj4;
6399 vinfos[5].jointtype = 1;
6400 vinfos[5].foffset = j5;
6401 vinfos[5].indices[0] = _ij5[0];
6402 vinfos[5].indices[1] = _ij5[1];
6403 vinfos[5].maxsolutions = _nj5;
6404 std::vector<int> vfree(0);
6405 solutions.AddSolution(vinfos,vfree);
6406 }
6407 }
6408 }
6409 
6410 }
6411 
6412 }
6413 
6414 } else
6415 {
6416 {
6417 IkReal j3array[1], cj3array[1], sj3array[1];
6418 bool j3valid[1]={false};
6419 _nj3 = 1;
6420 CheckValue<IkReal> x489 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6421 if(!x489.valid){
6422 continue;
6423 }
6424 CheckValue<IkReal> x490=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6425 if(!x490.valid){
6426 continue;
6427 }
6428 j3array[0]=((-1.5707963267949)+(x489.value)+(((1.5707963267949)*(x490.value))));
6429 sj3array[0]=IKsin(j3array[0]);
6430 cj3array[0]=IKcos(j3array[0]);
6431 if( j3array[0] > IKPI )
6432 {
6433  j3array[0]-=IK2PI;
6434 }
6435 else if( j3array[0] < -IKPI )
6436 { j3array[0]+=IK2PI;
6437 }
6438 j3valid[0] = true;
6439 for(int ij3 = 0; ij3 < 1; ++ij3)
6440 {
6441 if( !j3valid[ij3] )
6442 {
6443  continue;
6444 }
6445 _ij3[0] = ij3; _ij3[1] = -1;
6446 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6447 {
6448 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6449 {
6450  j3valid[iij3]=false; _ij3[1] = iij3; break;
6451 }
6452 }
6453 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6454 {
6455 IkReal evalcond[8];
6456 IkReal x491=IKcos(j3);
6457 IkReal x492=IKsin(j3);
6458 IkReal x493=((1.0)*gconst2);
6459 IkReal x494=(gconst2*x492);
6460 evalcond[0]=(new_r01*x491);
6461 evalcond[1]=((-1.0)*gconst2*x491);
6462 evalcond[2]=(gconst2+((new_r01*x492)));
6463 evalcond[3]=(x494+new_r01);
6464 evalcond[4]=((((-1.0)*x491*x493))+new_r00);
6465 evalcond[5]=(new_r10+(((-1.0)*x492*x493)));
6466 evalcond[6]=((((-1.0)*new_r10*x491))+((new_r00*x492)));
6467 evalcond[7]=((((-1.0)*x493))+((new_r10*x492))+((new_r00*x491)));
6468 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6469 {
6470 continue;
6471 }
6472 }
6473 
6474 {
6475 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6476 vinfos[0].jointtype = 1;
6477 vinfos[0].foffset = j0;
6478 vinfos[0].indices[0] = _ij0[0];
6479 vinfos[0].indices[1] = _ij0[1];
6480 vinfos[0].maxsolutions = _nj0;
6481 vinfos[1].jointtype = 1;
6482 vinfos[1].foffset = j1;
6483 vinfos[1].indices[0] = _ij1[0];
6484 vinfos[1].indices[1] = _ij1[1];
6485 vinfos[1].maxsolutions = _nj1;
6486 vinfos[2].jointtype = 1;
6487 vinfos[2].foffset = j2;
6488 vinfos[2].indices[0] = _ij2[0];
6489 vinfos[2].indices[1] = _ij2[1];
6490 vinfos[2].maxsolutions = _nj2;
6491 vinfos[3].jointtype = 1;
6492 vinfos[3].foffset = j3;
6493 vinfos[3].indices[0] = _ij3[0];
6494 vinfos[3].indices[1] = _ij3[1];
6495 vinfos[3].maxsolutions = _nj3;
6496 vinfos[4].jointtype = 1;
6497 vinfos[4].foffset = j4;
6498 vinfos[4].indices[0] = _ij4[0];
6499 vinfos[4].indices[1] = _ij4[1];
6500 vinfos[4].maxsolutions = _nj4;
6501 vinfos[5].jointtype = 1;
6502 vinfos[5].foffset = j5;
6503 vinfos[5].indices[0] = _ij5[0];
6504 vinfos[5].indices[1] = _ij5[1];
6505 vinfos[5].maxsolutions = _nj5;
6506 std::vector<int> vfree(0);
6507 solutions.AddSolution(vinfos,vfree);
6508 }
6509 }
6510 }
6511 
6512 }
6513 
6514 }
6515 
6516 } else
6517 {
6518 {
6519 IkReal j3array[1], cj3array[1], sj3array[1];
6520 bool j3valid[1]={false};
6521 _nj3 = 1;
6522 CheckValue<IkReal> x495=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6523 if(!x495.valid){
6524 continue;
6525 }
6526 CheckValue<IkReal> x496 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6527 if(!x496.valid){
6528 continue;
6529 }
6530 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x495.value)))+(x496.value));
6531 sj3array[0]=IKsin(j3array[0]);
6532 cj3array[0]=IKcos(j3array[0]);
6533 if( j3array[0] > IKPI )
6534 {
6535  j3array[0]-=IK2PI;
6536 }
6537 else if( j3array[0] < -IKPI )
6538 { j3array[0]+=IK2PI;
6539 }
6540 j3valid[0] = true;
6541 for(int ij3 = 0; ij3 < 1; ++ij3)
6542 {
6543 if( !j3valid[ij3] )
6544 {
6545  continue;
6546 }
6547 _ij3[0] = ij3; _ij3[1] = -1;
6548 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6549 {
6550 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6551 {
6552  j3valid[iij3]=false; _ij3[1] = iij3; break;
6553 }
6554 }
6555 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6556 {
6557 IkReal evalcond[8];
6558 IkReal x497=IKcos(j3);
6559 IkReal x498=IKsin(j3);
6560 IkReal x499=((1.0)*gconst2);
6561 IkReal x500=(gconst2*x498);
6562 evalcond[0]=(new_r01*x497);
6563 evalcond[1]=((-1.0)*gconst2*x497);
6564 evalcond[2]=(gconst2+((new_r01*x498)));
6565 evalcond[3]=(x500+new_r01);
6566 evalcond[4]=((((-1.0)*x497*x499))+new_r00);
6567 evalcond[5]=((((-1.0)*x498*x499))+new_r10);
6568 evalcond[6]=((((-1.0)*new_r10*x497))+((new_r00*x498)));
6569 evalcond[7]=((((-1.0)*x499))+((new_r10*x498))+((new_r00*x497)));
6570 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6571 {
6572 continue;
6573 }
6574 }
6575 
6576 {
6577 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6578 vinfos[0].jointtype = 1;
6579 vinfos[0].foffset = j0;
6580 vinfos[0].indices[0] = _ij0[0];
6581 vinfos[0].indices[1] = _ij0[1];
6582 vinfos[0].maxsolutions = _nj0;
6583 vinfos[1].jointtype = 1;
6584 vinfos[1].foffset = j1;
6585 vinfos[1].indices[0] = _ij1[0];
6586 vinfos[1].indices[1] = _ij1[1];
6587 vinfos[1].maxsolutions = _nj1;
6588 vinfos[2].jointtype = 1;
6589 vinfos[2].foffset = j2;
6590 vinfos[2].indices[0] = _ij2[0];
6591 vinfos[2].indices[1] = _ij2[1];
6592 vinfos[2].maxsolutions = _nj2;
6593 vinfos[3].jointtype = 1;
6594 vinfos[3].foffset = j3;
6595 vinfos[3].indices[0] = _ij3[0];
6596 vinfos[3].indices[1] = _ij3[1];
6597 vinfos[3].maxsolutions = _nj3;
6598 vinfos[4].jointtype = 1;
6599 vinfos[4].foffset = j4;
6600 vinfos[4].indices[0] = _ij4[0];
6601 vinfos[4].indices[1] = _ij4[1];
6602 vinfos[4].maxsolutions = _nj4;
6603 vinfos[5].jointtype = 1;
6604 vinfos[5].foffset = j5;
6605 vinfos[5].indices[0] = _ij5[0];
6606 vinfos[5].indices[1] = _ij5[1];
6607 vinfos[5].maxsolutions = _nj5;
6608 std::vector<int> vfree(0);
6609 solutions.AddSolution(vinfos,vfree);
6610 }
6611 }
6612 }
6613 
6614 }
6615 
6616 }
6617 
6618 }
6619 } while(0);
6620 if( bgotonextstatement )
6621 {
6622 bool bgotonextstatement = true;
6623 do
6624 {
6625 if( 1 )
6626 {
6627 bgotonextstatement=false;
6628 continue; // branch miss [j3]
6629 
6630 }
6631 } while(0);
6632 if( bgotonextstatement )
6633 {
6634 }
6635 }
6636 }
6637 }
6638 }
6639 }
6640 
6641 } else
6642 {
6643 {
6644 IkReal j3array[1], cj3array[1], sj3array[1];
6645 bool j3valid[1]={false};
6646 _nj3 = 1;
6647 IkReal x501=((1.0)*new_r01);
6648 CheckValue<IkReal> x502 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x501))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
6649 if(!x502.valid){
6650 continue;
6651 }
6652 CheckValue<IkReal> x503=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x501)))),-1);
6653 if(!x503.valid){
6654 continue;
6655 }
6656 j3array[0]=((-1.5707963267949)+(x502.value)+(((1.5707963267949)*(x503.value))));
6657 sj3array[0]=IKsin(j3array[0]);
6658 cj3array[0]=IKcos(j3array[0]);
6659 if( j3array[0] > IKPI )
6660 {
6661  j3array[0]-=IK2PI;
6662 }
6663 else if( j3array[0] < -IKPI )
6664 { j3array[0]+=IK2PI;
6665 }
6666 j3valid[0] = true;
6667 for(int ij3 = 0; ij3 < 1; ++ij3)
6668 {
6669 if( !j3valid[ij3] )
6670 {
6671  continue;
6672 }
6673 _ij3[0] = ij3; _ij3[1] = -1;
6674 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6675 {
6676 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6677 {
6678  j3valid[iij3]=false; _ij3[1] = iij3; break;
6679 }
6680 }
6681 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6682 {
6683 IkReal evalcond[8];
6684 IkReal x504=IKcos(j3);
6685 IkReal x505=IKsin(j3);
6686 IkReal x506=((1.0)*gconst2);
6687 IkReal x507=(gconst1*x505);
6688 IkReal x508=(gconst2*x505);
6689 IkReal x509=(gconst1*x504);
6690 IkReal x510=((1.0)*x504);
6691 IkReal x511=(x504*x506);
6692 evalcond[0]=(gconst1+((new_r11*x505))+((new_r01*x504)));
6693 evalcond[1]=(x508+x509+new_r01);
6694 evalcond[2]=((((-1.0)*new_r10*x510))+gconst1+((new_r00*x505)));
6695 evalcond[3]=((((-1.0)*new_r11*x510))+gconst2+((new_r01*x505)));
6696 evalcond[4]=(x507+new_r00+(((-1.0)*x511)));
6697 evalcond[5]=(x507+new_r11+(((-1.0)*x511)));
6698 evalcond[6]=((((-1.0)*x506))+((new_r10*x505))+((new_r00*x504)));
6699 evalcond[7]=((((-1.0)*x505*x506))+new_r10+(((-1.0)*x509)));
6700 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6701 {
6702 continue;
6703 }
6704 }
6705 
6706 {
6707 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6708 vinfos[0].jointtype = 1;
6709 vinfos[0].foffset = j0;
6710 vinfos[0].indices[0] = _ij0[0];
6711 vinfos[0].indices[1] = _ij0[1];
6712 vinfos[0].maxsolutions = _nj0;
6713 vinfos[1].jointtype = 1;
6714 vinfos[1].foffset = j1;
6715 vinfos[1].indices[0] = _ij1[0];
6716 vinfos[1].indices[1] = _ij1[1];
6717 vinfos[1].maxsolutions = _nj1;
6718 vinfos[2].jointtype = 1;
6719 vinfos[2].foffset = j2;
6720 vinfos[2].indices[0] = _ij2[0];
6721 vinfos[2].indices[1] = _ij2[1];
6722 vinfos[2].maxsolutions = _nj2;
6723 vinfos[3].jointtype = 1;
6724 vinfos[3].foffset = j3;
6725 vinfos[3].indices[0] = _ij3[0];
6726 vinfos[3].indices[1] = _ij3[1];
6727 vinfos[3].maxsolutions = _nj3;
6728 vinfos[4].jointtype = 1;
6729 vinfos[4].foffset = j4;
6730 vinfos[4].indices[0] = _ij4[0];
6731 vinfos[4].indices[1] = _ij4[1];
6732 vinfos[4].maxsolutions = _nj4;
6733 vinfos[5].jointtype = 1;
6734 vinfos[5].foffset = j5;
6735 vinfos[5].indices[0] = _ij5[0];
6736 vinfos[5].indices[1] = _ij5[1];
6737 vinfos[5].maxsolutions = _nj5;
6738 std::vector<int> vfree(0);
6739 solutions.AddSolution(vinfos,vfree);
6740 }
6741 }
6742 }
6743 
6744 }
6745 
6746 }
6747 
6748 } else
6749 {
6750 {
6751 IkReal j3array[1], cj3array[1], sj3array[1];
6752 bool j3valid[1]={false};
6753 _nj3 = 1;
6754 IkReal x512=((1.0)*gconst2);
6755 CheckValue<IkReal> x513 = IKatan2WithCheck(IkReal(((gconst1*gconst1)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst1*x512)))),IKFAST_ATAN2_MAGTHRESH);
6756 if(!x513.valid){
6757 continue;
6758 }
6759 CheckValue<IkReal> x514=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x512))+(((-1.0)*gconst1*new_r00)))),-1);
6760 if(!x514.valid){
6761 continue;
6762 }
6763 j3array[0]=((-1.5707963267949)+(x513.value)+(((1.5707963267949)*(x514.value))));
6764 sj3array[0]=IKsin(j3array[0]);
6765 cj3array[0]=IKcos(j3array[0]);
6766 if( j3array[0] > IKPI )
6767 {
6768  j3array[0]-=IK2PI;
6769 }
6770 else if( j3array[0] < -IKPI )
6771 { j3array[0]+=IK2PI;
6772 }
6773 j3valid[0] = true;
6774 for(int ij3 = 0; ij3 < 1; ++ij3)
6775 {
6776 if( !j3valid[ij3] )
6777 {
6778  continue;
6779 }
6780 _ij3[0] = ij3; _ij3[1] = -1;
6781 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6782 {
6783 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6784 {
6785  j3valid[iij3]=false; _ij3[1] = iij3; break;
6786 }
6787 }
6788 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6789 {
6790 IkReal evalcond[8];
6791 IkReal x515=IKcos(j3);
6792 IkReal x516=IKsin(j3);
6793 IkReal x517=((1.0)*gconst2);
6794 IkReal x518=(gconst1*x516);
6795 IkReal x519=(gconst2*x516);
6796 IkReal x520=(gconst1*x515);
6797 IkReal x521=((1.0)*x515);
6798 IkReal x522=(x515*x517);
6799 evalcond[0]=(((new_r01*x515))+((new_r11*x516))+gconst1);
6800 evalcond[1]=(x520+x519+new_r01);
6801 evalcond[2]=(((new_r00*x516))+gconst1+(((-1.0)*new_r10*x521)));
6802 evalcond[3]=(((new_r01*x516))+gconst2+(((-1.0)*new_r11*x521)));
6803 evalcond[4]=((((-1.0)*x522))+x518+new_r00);
6804 evalcond[5]=((((-1.0)*x522))+x518+new_r11);
6805 evalcond[6]=(((new_r00*x515))+((new_r10*x516))+(((-1.0)*x517)));
6806 evalcond[7]=((((-1.0)*x520))+(((-1.0)*x516*x517))+new_r10);
6807 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6808 {
6809 continue;
6810 }
6811 }
6812 
6813 {
6814 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6815 vinfos[0].jointtype = 1;
6816 vinfos[0].foffset = j0;
6817 vinfos[0].indices[0] = _ij0[0];
6818 vinfos[0].indices[1] = _ij0[1];
6819 vinfos[0].maxsolutions = _nj0;
6820 vinfos[1].jointtype = 1;
6821 vinfos[1].foffset = j1;
6822 vinfos[1].indices[0] = _ij1[0];
6823 vinfos[1].indices[1] = _ij1[1];
6824 vinfos[1].maxsolutions = _nj1;
6825 vinfos[2].jointtype = 1;
6826 vinfos[2].foffset = j2;
6827 vinfos[2].indices[0] = _ij2[0];
6828 vinfos[2].indices[1] = _ij2[1];
6829 vinfos[2].maxsolutions = _nj2;
6830 vinfos[3].jointtype = 1;
6831 vinfos[3].foffset = j3;
6832 vinfos[3].indices[0] = _ij3[0];
6833 vinfos[3].indices[1] = _ij3[1];
6834 vinfos[3].maxsolutions = _nj3;
6835 vinfos[4].jointtype = 1;
6836 vinfos[4].foffset = j4;
6837 vinfos[4].indices[0] = _ij4[0];
6838 vinfos[4].indices[1] = _ij4[1];
6839 vinfos[4].maxsolutions = _nj4;
6840 vinfos[5].jointtype = 1;
6841 vinfos[5].foffset = j5;
6842 vinfos[5].indices[0] = _ij5[0];
6843 vinfos[5].indices[1] = _ij5[1];
6844 vinfos[5].maxsolutions = _nj5;
6845 std::vector<int> vfree(0);
6846 solutions.AddSolution(vinfos,vfree);
6847 }
6848 }
6849 }
6850 
6851 }
6852 
6853 }
6854 
6855 } else
6856 {
6857 {
6858 IkReal j3array[1], cj3array[1], sj3array[1];
6859 bool j3valid[1]={false};
6860 _nj3 = 1;
6861 IkReal x523=((1.0)*new_r11);
6862 CheckValue<IkReal> x524 = IKatan2WithCheck(IkReal((((gconst1*new_r10))+((gconst1*new_r01)))),IkReal((((gconst1*new_r00))+(((-1.0)*gconst1*x523)))),IKFAST_ATAN2_MAGTHRESH);
6863 if(!x524.valid){
6864 continue;
6865 }
6866 CheckValue<IkReal> x525=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x523))+(((-1.0)*new_r00*new_r01)))),-1);
6867 if(!x525.valid){
6868 continue;
6869 }
6870 j3array[0]=((-1.5707963267949)+(x524.value)+(((1.5707963267949)*(x525.value))));
6871 sj3array[0]=IKsin(j3array[0]);
6872 cj3array[0]=IKcos(j3array[0]);
6873 if( j3array[0] > IKPI )
6874 {
6875  j3array[0]-=IK2PI;
6876 }
6877 else if( j3array[0] < -IKPI )
6878 { j3array[0]+=IK2PI;
6879 }
6880 j3valid[0] = true;
6881 for(int ij3 = 0; ij3 < 1; ++ij3)
6882 {
6883 if( !j3valid[ij3] )
6884 {
6885  continue;
6886 }
6887 _ij3[0] = ij3; _ij3[1] = -1;
6888 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6889 {
6890 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6891 {
6892  j3valid[iij3]=false; _ij3[1] = iij3; break;
6893 }
6894 }
6895 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6896 {
6897 IkReal evalcond[8];
6898 IkReal x526=IKcos(j3);
6899 IkReal x527=IKsin(j3);
6900 IkReal x528=((1.0)*gconst2);
6901 IkReal x529=(gconst1*x527);
6902 IkReal x530=(gconst2*x527);
6903 IkReal x531=(gconst1*x526);
6904 IkReal x532=((1.0)*x526);
6905 IkReal x533=(x526*x528);
6906 evalcond[0]=(((new_r01*x526))+gconst1+((new_r11*x527)));
6907 evalcond[1]=(x531+x530+new_r01);
6908 evalcond[2]=(gconst1+(((-1.0)*new_r10*x532))+((new_r00*x527)));
6909 evalcond[3]=(((new_r01*x527))+gconst2+(((-1.0)*new_r11*x532)));
6910 evalcond[4]=((((-1.0)*x533))+x529+new_r00);
6911 evalcond[5]=((((-1.0)*x533))+x529+new_r11);
6912 evalcond[6]=((((-1.0)*x528))+((new_r10*x527))+((new_r00*x526)));
6913 evalcond[7]=((((-1.0)*x527*x528))+(((-1.0)*x531))+new_r10);
6914 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6915 {
6916 continue;
6917 }
6918 }
6919 
6920 {
6921 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6922 vinfos[0].jointtype = 1;
6923 vinfos[0].foffset = j0;
6924 vinfos[0].indices[0] = _ij0[0];
6925 vinfos[0].indices[1] = _ij0[1];
6926 vinfos[0].maxsolutions = _nj0;
6927 vinfos[1].jointtype = 1;
6928 vinfos[1].foffset = j1;
6929 vinfos[1].indices[0] = _ij1[0];
6930 vinfos[1].indices[1] = _ij1[1];
6931 vinfos[1].maxsolutions = _nj1;
6932 vinfos[2].jointtype = 1;
6933 vinfos[2].foffset = j2;
6934 vinfos[2].indices[0] = _ij2[0];
6935 vinfos[2].indices[1] = _ij2[1];
6936 vinfos[2].maxsolutions = _nj2;
6937 vinfos[3].jointtype = 1;
6938 vinfos[3].foffset = j3;
6939 vinfos[3].indices[0] = _ij3[0];
6940 vinfos[3].indices[1] = _ij3[1];
6941 vinfos[3].maxsolutions = _nj3;
6942 vinfos[4].jointtype = 1;
6943 vinfos[4].foffset = j4;
6944 vinfos[4].indices[0] = _ij4[0];
6945 vinfos[4].indices[1] = _ij4[1];
6946 vinfos[4].maxsolutions = _nj4;
6947 vinfos[5].jointtype = 1;
6948 vinfos[5].foffset = j5;
6949 vinfos[5].indices[0] = _ij5[0];
6950 vinfos[5].indices[1] = _ij5[1];
6951 vinfos[5].maxsolutions = _nj5;
6952 std::vector<int> vfree(0);
6953 solutions.AddSolution(vinfos,vfree);
6954 }
6955 }
6956 }
6957 
6958 }
6959 
6960 }
6961 
6962 }
6963 } while(0);
6964 if( bgotonextstatement )
6965 {
6966 bool bgotonextstatement = true;
6967 do
6968 {
6969 IkReal x534=((-1.0)*new_r11);
6970 IkReal x536 = ((new_r01*new_r01)+(new_r11*new_r11));
6971 if(IKabs(x536)==0){
6972 continue;
6973 }
6974 IkReal x535=pow(x536,-0.5);
6975 CheckValue<IkReal> x537 = IKatan2WithCheck(IkReal(x534),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6976 if(!x537.valid){
6977 continue;
6978 }
6979 IkReal gconst3=((3.14159265358979)+(((-1.0)*(x537.value))));
6980 IkReal gconst4=(x534*x535);
6981 IkReal gconst5=((1.0)*new_r01*x535);
6982 CheckValue<IkReal> x538 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6983 if(!x538.valid){
6984 continue;
6985 }
6986 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x538.value)+j5)))), 6.28318530717959)));
6987 if( IKabs(evalcond[0]) < 0.0000050000000000 )
6988 {
6989 bgotonextstatement=false;
6990 {
6991 IkReal j3eval[3];
6992 IkReal x539=((-1.0)*new_r11);
6993 CheckValue<IkReal> x542 = IKatan2WithCheck(IkReal(x539),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6994 if(!x542.valid){
6995 continue;
6996 }
6997 IkReal x540=((1.0)*(x542.value));
6998 IkReal x541=x535;
6999 sj4=0;
7000 cj4=1.0;
7001 j4=0;
7002 sj5=gconst4;
7003 cj5=gconst5;
7004 j5=((3.14159265)+(((-1.0)*x540)));
7005 IkReal gconst3=((3.14159265358979)+(((-1.0)*x540)));
7006 IkReal gconst4=(x539*x541);
7007 IkReal gconst5=((1.0)*new_r01*x541);
7008 IkReal x543=new_r11*new_r11;
7009 IkReal x544=((1.0)*new_r01);
7010 IkReal x545=((1.0)*new_r10);
7011 IkReal x546=((((-1.0)*new_r00*x544))+(((-1.0)*new_r11*x545)));
7012 IkReal x547=x535;
7013 IkReal x548=(new_r11*x547);
7014 j3eval[0]=x546;
7015 j3eval[1]=((IKabs((((x543*x547))+(((-1.0)*new_r00*x548)))))+(IKabs(((((-1.0)*x544*x548))+(((-1.0)*x545*x548))))));
7016 j3eval[2]=IKsign(x546);
7017 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7018 {
7019 {
7020 IkReal j3eval[1];
7021 IkReal x549=((-1.0)*new_r11);
7022 CheckValue<IkReal> x552 = IKatan2WithCheck(IkReal(x549),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7023 if(!x552.valid){
7024 continue;
7025 }
7026 IkReal x550=((1.0)*(x552.value));
7027 IkReal x551=x535;
7028 sj4=0;
7029 cj4=1.0;
7030 j4=0;
7031 sj5=gconst4;
7032 cj5=gconst5;
7033 j5=((3.14159265)+(((-1.0)*x550)));
7034 IkReal gconst3=((3.14159265358979)+(((-1.0)*x550)));
7035 IkReal gconst4=(x549*x551);
7036 IkReal gconst5=((1.0)*new_r01*x551);
7037 IkReal x553=new_r11*new_r11;
7038 IkReal x554=new_r01*new_r01*new_r01;
7039 CheckValue<IkReal> x558=IKPowWithIntegerCheck(((new_r01*new_r01)+x553),-1);
7040 if(!x558.valid){
7041 continue;
7042 }
7043 IkReal x555=x558.value;
7044 IkReal x556=(x553*x555);
7045 IkReal x557=(x554*x555);
7046 j3eval[0]=((IKabs((((new_r10*x557))+x556+((new_r01*new_r10*x556)))))+(IKabs((((new_r00*new_r01*x556))+((new_r01*new_r11*x555))+((new_r00*x557))))));
7047 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7048 {
7049 {
7050 IkReal j3eval[1];
7051 IkReal x559=((-1.0)*new_r11);
7052 CheckValue<IkReal> x562 = IKatan2WithCheck(IkReal(x559),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7053 if(!x562.valid){
7054 continue;
7055 }
7056 IkReal x560=((1.0)*(x562.value));
7057 IkReal x561=x535;
7058 sj4=0;
7059 cj4=1.0;
7060 j4=0;
7061 sj5=gconst4;
7062 cj5=gconst5;
7063 j5=((3.14159265)+(((-1.0)*x560)));
7064 IkReal gconst3=((3.14159265358979)+(((-1.0)*x560)));
7065 IkReal gconst4=(x559*x561);
7066 IkReal gconst5=((1.0)*new_r01*x561);
7067 IkReal x563=new_r01*new_r01;
7068 IkReal x564=new_r11*new_r11;
7069 CheckValue<IkReal> x571=IKPowWithIntegerCheck((x564+x563),-1);
7070 if(!x571.valid){
7071 continue;
7072 }
7073 IkReal x565=x571.value;
7074 IkReal x566=(x564*x565);
7075 CheckValue<IkReal> x572=IKPowWithIntegerCheck(((((-1.0)*x563))+(((-1.0)*x564))),-1);
7076 if(!x572.valid){
7077 continue;
7078 }
7079 IkReal x567=x572.value;
7080 IkReal x568=((1.0)*x567);
7081 IkReal x569=(new_r11*x568);
7082 IkReal x570=(new_r01*x568);
7083 j3eval[0]=((IKabs((((x563*x566))+((x565*(x563*x563)))+(((-1.0)*x566)))))+(IKabs(((((-1.0)*new_r01*x569*(new_r11*new_r11)))+(((-1.0)*x569*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x569))))));
7084 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7085 {
7086 {
7087 IkReal evalcond[3];
7088 bool bgotonextstatement = true;
7089 do
7090 {
7091 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7092 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7093 {
7094 bgotonextstatement=false;
7095 {
7096 IkReal j3array[2], cj3array[2], sj3array[2];
7097 bool j3valid[2]={false};
7098 _nj3 = 2;
7099 CheckValue<IkReal> x573=IKPowWithIntegerCheck(gconst5,-1);
7100 if(!x573.valid){
7101 continue;
7102 }
7103 sj3array[0]=(new_r10*(x573.value));
7104 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7105 {
7106  j3valid[0] = j3valid[1] = true;
7107  j3array[0] = IKasin(sj3array[0]);
7108  cj3array[0] = IKcos(j3array[0]);
7109  sj3array[1] = sj3array[0];
7110  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7111  cj3array[1] = -cj3array[0];
7112 }
7113 else if( isnan(sj3array[0]) )
7114 {
7115  // probably any value will work
7116  j3valid[0] = true;
7117  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7118 }
7119 for(int ij3 = 0; ij3 < 2; ++ij3)
7120 {
7121 if( !j3valid[ij3] )
7122 {
7123  continue;
7124 }
7125 _ij3[0] = ij3; _ij3[1] = -1;
7126 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7127 {
7128 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7129 {
7130  j3valid[iij3]=false; _ij3[1] = iij3; break;
7131 }
7132 }
7133 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7134 {
7135 IkReal evalcond[6];
7136 IkReal x574=IKcos(j3);
7137 IkReal x575=IKsin(j3);
7138 IkReal x576=((-1.0)*x574);
7139 evalcond[0]=(new_r01*x574);
7140 evalcond[1]=(new_r10*x576);
7141 evalcond[2]=(gconst5*x576);
7142 evalcond[3]=(((new_r01*x575))+gconst5);
7143 evalcond[4]=(((gconst5*x575))+new_r01);
7144 evalcond[5]=(((new_r10*x575))+(((-1.0)*gconst5)));
7145 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7146 {
7147 continue;
7148 }
7149 }
7150 
7151 {
7152 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7153 vinfos[0].jointtype = 1;
7154 vinfos[0].foffset = j0;
7155 vinfos[0].indices[0] = _ij0[0];
7156 vinfos[0].indices[1] = _ij0[1];
7157 vinfos[0].maxsolutions = _nj0;
7158 vinfos[1].jointtype = 1;
7159 vinfos[1].foffset = j1;
7160 vinfos[1].indices[0] = _ij1[0];
7161 vinfos[1].indices[1] = _ij1[1];
7162 vinfos[1].maxsolutions = _nj1;
7163 vinfos[2].jointtype = 1;
7164 vinfos[2].foffset = j2;
7165 vinfos[2].indices[0] = _ij2[0];
7166 vinfos[2].indices[1] = _ij2[1];
7167 vinfos[2].maxsolutions = _nj2;
7168 vinfos[3].jointtype = 1;
7169 vinfos[3].foffset = j3;
7170 vinfos[3].indices[0] = _ij3[0];
7171 vinfos[3].indices[1] = _ij3[1];
7172 vinfos[3].maxsolutions = _nj3;
7173 vinfos[4].jointtype = 1;
7174 vinfos[4].foffset = j4;
7175 vinfos[4].indices[0] = _ij4[0];
7176 vinfos[4].indices[1] = _ij4[1];
7177 vinfos[4].maxsolutions = _nj4;
7178 vinfos[5].jointtype = 1;
7179 vinfos[5].foffset = j5;
7180 vinfos[5].indices[0] = _ij5[0];
7181 vinfos[5].indices[1] = _ij5[1];
7182 vinfos[5].maxsolutions = _nj5;
7183 std::vector<int> vfree(0);
7184 solutions.AddSolution(vinfos,vfree);
7185 }
7186 }
7187 }
7188 
7189 }
7190 } while(0);
7191 if( bgotonextstatement )
7192 {
7193 bool bgotonextstatement = true;
7194 do
7195 {
7196 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7197 evalcond[1]=gconst4;
7198 evalcond[2]=gconst5;
7199 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
7200 {
7201 bgotonextstatement=false;
7202 {
7203 IkReal j3eval[3];
7204 IkReal x577=((-1.0)*new_r11);
7205 CheckValue<IkReal> x579 = IKatan2WithCheck(IkReal(x577),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7206 if(!x579.valid){
7207 continue;
7208 }
7209 IkReal x578=((1.0)*(x579.value));
7210 sj4=0;
7211 cj4=1.0;
7212 j4=0;
7213 sj5=gconst4;
7214 cj5=gconst5;
7215 j5=((3.14159265)+(((-1.0)*x578)));
7216 new_r00=0;
7217 new_r10=0;
7218 new_r21=0;
7219 new_r22=0;
7220 IkReal gconst3=((3.14159265358979)+(((-1.0)*x578)));
7221 IkReal gconst4=x577;
7222 IkReal gconst5=((1.0)*new_r01);
7223 j3eval[0]=1.0;
7224 j3eval[1]=1.0;
7225 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(IKabs(((1.0)*new_r01*new_r11))));
7226 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7227 {
7228 {
7229 IkReal j3eval[4];
7230 IkReal x580=((-1.0)*new_r11);
7231 CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal(x580),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7232 if(!x582.valid){
7233 continue;
7234 }
7235 IkReal x581=((1.0)*(x582.value));
7236 sj4=0;
7237 cj4=1.0;
7238 j4=0;
7239 sj5=gconst4;
7240 cj5=gconst5;
7241 j5=((3.14159265)+(((-1.0)*x581)));
7242 new_r00=0;
7243 new_r10=0;
7244 new_r21=0;
7245 new_r22=0;
7246 IkReal gconst3=((3.14159265358979)+(((-1.0)*x581)));
7247 IkReal gconst4=x580;
7248 IkReal gconst5=((1.0)*new_r01);
7249 j3eval[0]=-1.0;
7250 j3eval[1]=new_r01;
7251 j3eval[2]=1.0;
7252 j3eval[3]=-1.0;
7253 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
7254 {
7255 {
7256 IkReal j3eval[3];
7257 IkReal x583=((-1.0)*new_r11);
7258 CheckValue<IkReal> x585 = IKatan2WithCheck(IkReal(x583),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7259 if(!x585.valid){
7260 continue;
7261 }
7262 IkReal x584=((1.0)*(x585.value));
7263 sj4=0;
7264 cj4=1.0;
7265 j4=0;
7266 sj5=gconst4;
7267 cj5=gconst5;
7268 j5=((3.14159265)+(((-1.0)*x584)));
7269 new_r00=0;
7270 new_r10=0;
7271 new_r21=0;
7272 new_r22=0;
7273 IkReal gconst3=((3.14159265358979)+(((-1.0)*x584)));
7274 IkReal gconst4=x583;
7275 IkReal gconst5=((1.0)*new_r01);
7276 j3eval[0]=-1.0;
7277 j3eval[1]=-1.0;
7278 j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7279 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7280 {
7281 continue; // 3 cases reached
7282 
7283 } else
7284 {
7285 {
7286 IkReal j3array[1], cj3array[1], sj3array[1];
7287 bool j3valid[1]={false};
7288 _nj3 = 1;
7289 IkReal x586=((1.0)*new_r01);
7290 CheckValue<IkReal> x587 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x586))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
7291 if(!x587.valid){
7292 continue;
7293 }
7294 CheckValue<IkReal> x588=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x586)))),-1);
7295 if(!x588.valid){
7296 continue;
7297 }
7298 j3array[0]=((-1.5707963267949)+(x587.value)+(((1.5707963267949)*(x588.value))));
7299 sj3array[0]=IKsin(j3array[0]);
7300 cj3array[0]=IKcos(j3array[0]);
7301 if( j3array[0] > IKPI )
7302 {
7303  j3array[0]-=IK2PI;
7304 }
7305 else if( j3array[0] < -IKPI )
7306 { j3array[0]+=IK2PI;
7307 }
7308 j3valid[0] = true;
7309 for(int ij3 = 0; ij3 < 1; ++ij3)
7310 {
7311 if( !j3valid[ij3] )
7312 {
7313  continue;
7314 }
7315 _ij3[0] = ij3; _ij3[1] = -1;
7316 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7317 {
7318 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7319 {
7320  j3valid[iij3]=false; _ij3[1] = iij3; break;
7321 }
7322 }
7323 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7324 {
7325 IkReal evalcond[6];
7326 IkReal x589=IKsin(j3);
7327 IkReal x590=IKcos(j3);
7328 IkReal x591=(gconst4*x589);
7329 IkReal x592=(gconst4*x590);
7330 IkReal x593=((1.0)*x590);
7331 IkReal x594=(gconst5*x589);
7332 IkReal x595=(gconst5*x593);
7333 evalcond[0]=(gconst4+((new_r01*x590))+((new_r11*x589)));
7334 evalcond[1]=(x594+x592+new_r01);
7335 evalcond[2]=((((-1.0)*x595))+x591);
7336 evalcond[3]=((((-1.0)*new_r11*x593))+gconst5+((new_r01*x589)));
7337 evalcond[4]=((((-1.0)*x595))+x591+new_r11);
7338 evalcond[5]=((((-1.0)*x594))+(((-1.0)*x592)));
7339 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7340 {
7341 continue;
7342 }
7343 }
7344 
7345 {
7346 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7347 vinfos[0].jointtype = 1;
7348 vinfos[0].foffset = j0;
7349 vinfos[0].indices[0] = _ij0[0];
7350 vinfos[0].indices[1] = _ij0[1];
7351 vinfos[0].maxsolutions = _nj0;
7352 vinfos[1].jointtype = 1;
7353 vinfos[1].foffset = j1;
7354 vinfos[1].indices[0] = _ij1[0];
7355 vinfos[1].indices[1] = _ij1[1];
7356 vinfos[1].maxsolutions = _nj1;
7357 vinfos[2].jointtype = 1;
7358 vinfos[2].foffset = j2;
7359 vinfos[2].indices[0] = _ij2[0];
7360 vinfos[2].indices[1] = _ij2[1];
7361 vinfos[2].maxsolutions = _nj2;
7362 vinfos[3].jointtype = 1;
7363 vinfos[3].foffset = j3;
7364 vinfos[3].indices[0] = _ij3[0];
7365 vinfos[3].indices[1] = _ij3[1];
7366 vinfos[3].maxsolutions = _nj3;
7367 vinfos[4].jointtype = 1;
7368 vinfos[4].foffset = j4;
7369 vinfos[4].indices[0] = _ij4[0];
7370 vinfos[4].indices[1] = _ij4[1];
7371 vinfos[4].maxsolutions = _nj4;
7372 vinfos[5].jointtype = 1;
7373 vinfos[5].foffset = j5;
7374 vinfos[5].indices[0] = _ij5[0];
7375 vinfos[5].indices[1] = _ij5[1];
7376 vinfos[5].maxsolutions = _nj5;
7377 std::vector<int> vfree(0);
7378 solutions.AddSolution(vinfos,vfree);
7379 }
7380 }
7381 }
7382 
7383 }
7384 
7385 }
7386 
7387 } else
7388 {
7389 {
7390 IkReal j3array[1], cj3array[1], sj3array[1];
7391 bool j3valid[1]={false};
7392 _nj3 = 1;
7393 CheckValue<IkReal> x596=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst4*gconst4)))+(((-1.0)*(gconst5*gconst5))))),-1);
7394 if(!x596.valid){
7395 continue;
7396 }
7397 CheckValue<IkReal> x597 = IKatan2WithCheck(IkReal((gconst5*new_r01)),IkReal((gconst4*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7398 if(!x597.valid){
7399 continue;
7400 }
7401 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x596.value)))+(x597.value));
7402 sj3array[0]=IKsin(j3array[0]);
7403 cj3array[0]=IKcos(j3array[0]);
7404 if( j3array[0] > IKPI )
7405 {
7406  j3array[0]-=IK2PI;
7407 }
7408 else if( j3array[0] < -IKPI )
7409 { j3array[0]+=IK2PI;
7410 }
7411 j3valid[0] = true;
7412 for(int ij3 = 0; ij3 < 1; ++ij3)
7413 {
7414 if( !j3valid[ij3] )
7415 {
7416  continue;
7417 }
7418 _ij3[0] = ij3; _ij3[1] = -1;
7419 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7420 {
7421 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7422 {
7423  j3valid[iij3]=false; _ij3[1] = iij3; break;
7424 }
7425 }
7426 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7427 {
7428 IkReal evalcond[6];
7429 IkReal x598=IKsin(j3);
7430 IkReal x599=IKcos(j3);
7431 IkReal x600=(gconst4*x598);
7432 IkReal x601=(gconst4*x599);
7433 IkReal x602=((1.0)*x599);
7434 IkReal x603=(gconst5*x598);
7435 IkReal x604=(gconst5*x602);
7436 evalcond[0]=(((new_r11*x598))+gconst4+((new_r01*x599)));
7437 evalcond[1]=(x603+x601+new_r01);
7438 evalcond[2]=(x600+(((-1.0)*x604)));
7439 evalcond[3]=((((-1.0)*new_r11*x602))+gconst5+((new_r01*x598)));
7440 evalcond[4]=(x600+(((-1.0)*x604))+new_r11);
7441 evalcond[5]=((((-1.0)*x601))+(((-1.0)*x603)));
7442 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7443 {
7444 continue;
7445 }
7446 }
7447 
7448 {
7449 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7450 vinfos[0].jointtype = 1;
7451 vinfos[0].foffset = j0;
7452 vinfos[0].indices[0] = _ij0[0];
7453 vinfos[0].indices[1] = _ij0[1];
7454 vinfos[0].maxsolutions = _nj0;
7455 vinfos[1].jointtype = 1;
7456 vinfos[1].foffset = j1;
7457 vinfos[1].indices[0] = _ij1[0];
7458 vinfos[1].indices[1] = _ij1[1];
7459 vinfos[1].maxsolutions = _nj1;
7460 vinfos[2].jointtype = 1;
7461 vinfos[2].foffset = j2;
7462 vinfos[2].indices[0] = _ij2[0];
7463 vinfos[2].indices[1] = _ij2[1];
7464 vinfos[2].maxsolutions = _nj2;
7465 vinfos[3].jointtype = 1;
7466 vinfos[3].foffset = j3;
7467 vinfos[3].indices[0] = _ij3[0];
7468 vinfos[3].indices[1] = _ij3[1];
7469 vinfos[3].maxsolutions = _nj3;
7470 vinfos[4].jointtype = 1;
7471 vinfos[4].foffset = j4;
7472 vinfos[4].indices[0] = _ij4[0];
7473 vinfos[4].indices[1] = _ij4[1];
7474 vinfos[4].maxsolutions = _nj4;
7475 vinfos[5].jointtype = 1;
7476 vinfos[5].foffset = j5;
7477 vinfos[5].indices[0] = _ij5[0];
7478 vinfos[5].indices[1] = _ij5[1];
7479 vinfos[5].maxsolutions = _nj5;
7480 std::vector<int> vfree(0);
7481 solutions.AddSolution(vinfos,vfree);
7482 }
7483 }
7484 }
7485 
7486 }
7487 
7488 }
7489 
7490 } else
7491 {
7492 {
7493 IkReal j3array[1], cj3array[1], sj3array[1];
7494 bool j3valid[1]={false};
7495 _nj3 = 1;
7496 CheckValue<IkReal> x605=IKPowWithIntegerCheck(IKsign((((gconst5*new_r01))+(((-1.0)*gconst4*new_r11)))),-1);
7497 if(!x605.valid){
7498 continue;
7499 }
7500 CheckValue<IkReal> x606 = IKatan2WithCheck(IkReal(gconst4*gconst4),IkReal(((-1.0)*gconst4*gconst5)),IKFAST_ATAN2_MAGTHRESH);
7501 if(!x606.valid){
7502 continue;
7503 }
7504 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x605.value)))+(x606.value));
7505 sj3array[0]=IKsin(j3array[0]);
7506 cj3array[0]=IKcos(j3array[0]);
7507 if( j3array[0] > IKPI )
7508 {
7509  j3array[0]-=IK2PI;
7510 }
7511 else if( j3array[0] < -IKPI )
7512 { j3array[0]+=IK2PI;
7513 }
7514 j3valid[0] = true;
7515 for(int ij3 = 0; ij3 < 1; ++ij3)
7516 {
7517 if( !j3valid[ij3] )
7518 {
7519  continue;
7520 }
7521 _ij3[0] = ij3; _ij3[1] = -1;
7522 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7523 {
7524 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7525 {
7526  j3valid[iij3]=false; _ij3[1] = iij3; break;
7527 }
7528 }
7529 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7530 {
7531 IkReal evalcond[6];
7532 IkReal x607=IKsin(j3);
7533 IkReal x608=IKcos(j3);
7534 IkReal x609=(gconst4*x607);
7535 IkReal x610=(gconst4*x608);
7536 IkReal x611=((1.0)*x608);
7537 IkReal x612=(gconst5*x607);
7538 IkReal x613=(gconst5*x611);
7539 evalcond[0]=(gconst4+((new_r11*x607))+((new_r01*x608)));
7540 evalcond[1]=(x610+x612+new_r01);
7541 evalcond[2]=((((-1.0)*x613))+x609);
7542 evalcond[3]=(gconst5+(((-1.0)*new_r11*x611))+((new_r01*x607)));
7543 evalcond[4]=((((-1.0)*x613))+x609+new_r11);
7544 evalcond[5]=((((-1.0)*x610))+(((-1.0)*x612)));
7545 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7546 {
7547 continue;
7548 }
7549 }
7550 
7551 {
7552 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7553 vinfos[0].jointtype = 1;
7554 vinfos[0].foffset = j0;
7555 vinfos[0].indices[0] = _ij0[0];
7556 vinfos[0].indices[1] = _ij0[1];
7557 vinfos[0].maxsolutions = _nj0;
7558 vinfos[1].jointtype = 1;
7559 vinfos[1].foffset = j1;
7560 vinfos[1].indices[0] = _ij1[0];
7561 vinfos[1].indices[1] = _ij1[1];
7562 vinfos[1].maxsolutions = _nj1;
7563 vinfos[2].jointtype = 1;
7564 vinfos[2].foffset = j2;
7565 vinfos[2].indices[0] = _ij2[0];
7566 vinfos[2].indices[1] = _ij2[1];
7567 vinfos[2].maxsolutions = _nj2;
7568 vinfos[3].jointtype = 1;
7569 vinfos[3].foffset = j3;
7570 vinfos[3].indices[0] = _ij3[0];
7571 vinfos[3].indices[1] = _ij3[1];
7572 vinfos[3].maxsolutions = _nj3;
7573 vinfos[4].jointtype = 1;
7574 vinfos[4].foffset = j4;
7575 vinfos[4].indices[0] = _ij4[0];
7576 vinfos[4].indices[1] = _ij4[1];
7577 vinfos[4].maxsolutions = _nj4;
7578 vinfos[5].jointtype = 1;
7579 vinfos[5].foffset = j5;
7580 vinfos[5].indices[0] = _ij5[0];
7581 vinfos[5].indices[1] = _ij5[1];
7582 vinfos[5].maxsolutions = _nj5;
7583 std::vector<int> vfree(0);
7584 solutions.AddSolution(vinfos,vfree);
7585 }
7586 }
7587 }
7588 
7589 }
7590 
7591 }
7592 
7593 }
7594 } while(0);
7595 if( bgotonextstatement )
7596 {
7597 bool bgotonextstatement = true;
7598 do
7599 {
7600 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
7601 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7602 {
7603 bgotonextstatement=false;
7604 {
7605 IkReal j3eval[1];
7606 IkReal x614=((-1.0)*new_r11);
7607 CheckValue<IkReal> x616 = IKatan2WithCheck(IkReal(x614),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7608 if(!x616.valid){
7609 continue;
7610 }
7611 IkReal x615=((1.0)*(x616.value));
7612 sj4=0;
7613 cj4=1.0;
7614 j4=0;
7615 sj5=gconst4;
7616 cj5=gconst5;
7617 j5=((3.14159265)+(((-1.0)*x615)));
7618 new_r01=0;
7619 new_r10=0;
7620 IkReal gconst3=((3.14159265358979)+(((-1.0)*x615)));
7621 IkReal x617 = new_r11*new_r11;
7622 if(IKabs(x617)==0){
7623 continue;
7624 }
7625 IkReal gconst4=(x614*(pow(x617,-0.5)));
7626 IkReal gconst5=0;
7627 j3eval[0]=new_r00;
7628 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7629 {
7630 {
7631 IkReal j3eval[1];
7632 IkReal x618=((-1.0)*new_r11);
7633 CheckValue<IkReal> x620 = IKatan2WithCheck(IkReal(x618),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7634 if(!x620.valid){
7635 continue;
7636 }
7637 IkReal x619=((1.0)*(x620.value));
7638 sj4=0;
7639 cj4=1.0;
7640 j4=0;
7641 sj5=gconst4;
7642 cj5=gconst5;
7643 j5=((3.14159265)+(((-1.0)*x619)));
7644 new_r01=0;
7645 new_r10=0;
7646 IkReal gconst3=((3.14159265358979)+(((-1.0)*x619)));
7647 IkReal x621 = new_r11*new_r11;
7648 if(IKabs(x621)==0){
7649 continue;
7650 }
7651 IkReal gconst4=(x618*(pow(x621,-0.5)));
7652 IkReal gconst5=0;
7653 j3eval[0]=new_r11;
7654 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7655 {
7656 {
7657 IkReal j3array[2], cj3array[2], sj3array[2];
7658 bool j3valid[2]={false};
7659 _nj3 = 2;
7660 CheckValue<IkReal> x622=IKPowWithIntegerCheck(gconst4,-1);
7661 if(!x622.valid){
7662 continue;
7663 }
7664 sj3array[0]=((-1.0)*new_r00*(x622.value));
7665 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7666 {
7667  j3valid[0] = j3valid[1] = true;
7668  j3array[0] = IKasin(sj3array[0]);
7669  cj3array[0] = IKcos(j3array[0]);
7670  sj3array[1] = sj3array[0];
7671  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7672  cj3array[1] = -cj3array[0];
7673 }
7674 else if( isnan(sj3array[0]) )
7675 {
7676  // probably any value will work
7677  j3valid[0] = true;
7678  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7679 }
7680 for(int ij3 = 0; ij3 < 2; ++ij3)
7681 {
7682 if( !j3valid[ij3] )
7683 {
7684  continue;
7685 }
7686 _ij3[0] = ij3; _ij3[1] = -1;
7687 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7688 {
7689 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7690 {
7691  j3valid[iij3]=false; _ij3[1] = iij3; break;
7692 }
7693 }
7694 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7695 {
7696 IkReal evalcond[6];
7697 IkReal x623=IKcos(j3);
7698 IkReal x624=IKsin(j3);
7699 evalcond[0]=(gconst4*x623);
7700 evalcond[1]=(new_r00*x623);
7701 evalcond[2]=((-1.0)*new_r11*x623);
7702 evalcond[3]=(gconst4+((new_r00*x624)));
7703 evalcond[4]=(gconst4+((new_r11*x624)));
7704 evalcond[5]=(((gconst4*x624))+new_r11);
7705 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7706 {
7707 continue;
7708 }
7709 }
7710 
7711 {
7712 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7713 vinfos[0].jointtype = 1;
7714 vinfos[0].foffset = j0;
7715 vinfos[0].indices[0] = _ij0[0];
7716 vinfos[0].indices[1] = _ij0[1];
7717 vinfos[0].maxsolutions = _nj0;
7718 vinfos[1].jointtype = 1;
7719 vinfos[1].foffset = j1;
7720 vinfos[1].indices[0] = _ij1[0];
7721 vinfos[1].indices[1] = _ij1[1];
7722 vinfos[1].maxsolutions = _nj1;
7723 vinfos[2].jointtype = 1;
7724 vinfos[2].foffset = j2;
7725 vinfos[2].indices[0] = _ij2[0];
7726 vinfos[2].indices[1] = _ij2[1];
7727 vinfos[2].maxsolutions = _nj2;
7728 vinfos[3].jointtype = 1;
7729 vinfos[3].foffset = j3;
7730 vinfos[3].indices[0] = _ij3[0];
7731 vinfos[3].indices[1] = _ij3[1];
7732 vinfos[3].maxsolutions = _nj3;
7733 vinfos[4].jointtype = 1;
7734 vinfos[4].foffset = j4;
7735 vinfos[4].indices[0] = _ij4[0];
7736 vinfos[4].indices[1] = _ij4[1];
7737 vinfos[4].maxsolutions = _nj4;
7738 vinfos[5].jointtype = 1;
7739 vinfos[5].foffset = j5;
7740 vinfos[5].indices[0] = _ij5[0];
7741 vinfos[5].indices[1] = _ij5[1];
7742 vinfos[5].maxsolutions = _nj5;
7743 std::vector<int> vfree(0);
7744 solutions.AddSolution(vinfos,vfree);
7745 }
7746 }
7747 }
7748 
7749 } else
7750 {
7751 {
7752 IkReal j3array[2], cj3array[2], sj3array[2];
7753 bool j3valid[2]={false};
7754 _nj3 = 2;
7755 CheckValue<IkReal> x625=IKPowWithIntegerCheck(new_r11,-1);
7756 if(!x625.valid){
7757 continue;
7758 }
7759 sj3array[0]=((-1.0)*gconst4*(x625.value));
7760 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7761 {
7762  j3valid[0] = j3valid[1] = true;
7763  j3array[0] = IKasin(sj3array[0]);
7764  cj3array[0] = IKcos(j3array[0]);
7765  sj3array[1] = sj3array[0];
7766  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7767  cj3array[1] = -cj3array[0];
7768 }
7769 else if( isnan(sj3array[0]) )
7770 {
7771  // probably any value will work
7772  j3valid[0] = true;
7773  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7774 }
7775 for(int ij3 = 0; ij3 < 2; ++ij3)
7776 {
7777 if( !j3valid[ij3] )
7778 {
7779  continue;
7780 }
7781 _ij3[0] = ij3; _ij3[1] = -1;
7782 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7783 {
7784 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7785 {
7786  j3valid[iij3]=false; _ij3[1] = iij3; break;
7787 }
7788 }
7789 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7790 {
7791 IkReal evalcond[6];
7792 IkReal x626=IKcos(j3);
7793 IkReal x627=IKsin(j3);
7794 IkReal x628=(gconst4*x627);
7795 evalcond[0]=(gconst4*x626);
7796 evalcond[1]=(new_r00*x626);
7797 evalcond[2]=((-1.0)*new_r11*x626);
7798 evalcond[3]=(gconst4+((new_r00*x627)));
7799 evalcond[4]=(x628+new_r00);
7800 evalcond[5]=(x628+new_r11);
7801 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7802 {
7803 continue;
7804 }
7805 }
7806 
7807 {
7808 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7809 vinfos[0].jointtype = 1;
7810 vinfos[0].foffset = j0;
7811 vinfos[0].indices[0] = _ij0[0];
7812 vinfos[0].indices[1] = _ij0[1];
7813 vinfos[0].maxsolutions = _nj0;
7814 vinfos[1].jointtype = 1;
7815 vinfos[1].foffset = j1;
7816 vinfos[1].indices[0] = _ij1[0];
7817 vinfos[1].indices[1] = _ij1[1];
7818 vinfos[1].maxsolutions = _nj1;
7819 vinfos[2].jointtype = 1;
7820 vinfos[2].foffset = j2;
7821 vinfos[2].indices[0] = _ij2[0];
7822 vinfos[2].indices[1] = _ij2[1];
7823 vinfos[2].maxsolutions = _nj2;
7824 vinfos[3].jointtype = 1;
7825 vinfos[3].foffset = j3;
7826 vinfos[3].indices[0] = _ij3[0];
7827 vinfos[3].indices[1] = _ij3[1];
7828 vinfos[3].maxsolutions = _nj3;
7829 vinfos[4].jointtype = 1;
7830 vinfos[4].foffset = j4;
7831 vinfos[4].indices[0] = _ij4[0];
7832 vinfos[4].indices[1] = _ij4[1];
7833 vinfos[4].maxsolutions = _nj4;
7834 vinfos[5].jointtype = 1;
7835 vinfos[5].foffset = j5;
7836 vinfos[5].indices[0] = _ij5[0];
7837 vinfos[5].indices[1] = _ij5[1];
7838 vinfos[5].maxsolutions = _nj5;
7839 std::vector<int> vfree(0);
7840 solutions.AddSolution(vinfos,vfree);
7841 }
7842 }
7843 }
7844 
7845 }
7846 
7847 }
7848 
7849 } else
7850 {
7851 {
7852 IkReal j3array[2], cj3array[2], sj3array[2];
7853 bool j3valid[2]={false};
7854 _nj3 = 2;
7855 CheckValue<IkReal> x629=IKPowWithIntegerCheck(new_r00,-1);
7856 if(!x629.valid){
7857 continue;
7858 }
7859 sj3array[0]=((-1.0)*gconst4*(x629.value));
7860 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7861 {
7862  j3valid[0] = j3valid[1] = true;
7863  j3array[0] = IKasin(sj3array[0]);
7864  cj3array[0] = IKcos(j3array[0]);
7865  sj3array[1] = sj3array[0];
7866  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7867  cj3array[1] = -cj3array[0];
7868 }
7869 else if( isnan(sj3array[0]) )
7870 {
7871  // probably any value will work
7872  j3valid[0] = true;
7873  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7874 }
7875 for(int ij3 = 0; ij3 < 2; ++ij3)
7876 {
7877 if( !j3valid[ij3] )
7878 {
7879  continue;
7880 }
7881 _ij3[0] = ij3; _ij3[1] = -1;
7882 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7883 {
7884 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7885 {
7886  j3valid[iij3]=false; _ij3[1] = iij3; break;
7887 }
7888 }
7889 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7890 {
7891 IkReal evalcond[6];
7892 IkReal x630=IKcos(j3);
7893 IkReal x631=IKsin(j3);
7894 IkReal x632=(gconst4*x631);
7895 evalcond[0]=(gconst4*x630);
7896 evalcond[1]=(new_r00*x630);
7897 evalcond[2]=((-1.0)*new_r11*x630);
7898 evalcond[3]=(gconst4+((new_r11*x631)));
7899 evalcond[4]=(x632+new_r00);
7900 evalcond[5]=(x632+new_r11);
7901 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7902 {
7903 continue;
7904 }
7905 }
7906 
7907 {
7908 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7909 vinfos[0].jointtype = 1;
7910 vinfos[0].foffset = j0;
7911 vinfos[0].indices[0] = _ij0[0];
7912 vinfos[0].indices[1] = _ij0[1];
7913 vinfos[0].maxsolutions = _nj0;
7914 vinfos[1].jointtype = 1;
7915 vinfos[1].foffset = j1;
7916 vinfos[1].indices[0] = _ij1[0];
7917 vinfos[1].indices[1] = _ij1[1];
7918 vinfos[1].maxsolutions = _nj1;
7919 vinfos[2].jointtype = 1;
7920 vinfos[2].foffset = j2;
7921 vinfos[2].indices[0] = _ij2[0];
7922 vinfos[2].indices[1] = _ij2[1];
7923 vinfos[2].maxsolutions = _nj2;
7924 vinfos[3].jointtype = 1;
7925 vinfos[3].foffset = j3;
7926 vinfos[3].indices[0] = _ij3[0];
7927 vinfos[3].indices[1] = _ij3[1];
7928 vinfos[3].maxsolutions = _nj3;
7929 vinfos[4].jointtype = 1;
7930 vinfos[4].foffset = j4;
7931 vinfos[4].indices[0] = _ij4[0];
7932 vinfos[4].indices[1] = _ij4[1];
7933 vinfos[4].maxsolutions = _nj4;
7934 vinfos[5].jointtype = 1;
7935 vinfos[5].foffset = j5;
7936 vinfos[5].indices[0] = _ij5[0];
7937 vinfos[5].indices[1] = _ij5[1];
7938 vinfos[5].maxsolutions = _nj5;
7939 std::vector<int> vfree(0);
7940 solutions.AddSolution(vinfos,vfree);
7941 }
7942 }
7943 }
7944 
7945 }
7946 
7947 }
7948 
7949 }
7950 } while(0);
7951 if( bgotonextstatement )
7952 {
7953 bool bgotonextstatement = true;
7954 do
7955 {
7956 evalcond[0]=IKabs(new_r11);
7957 if( IKabs(evalcond[0]) < 0.0000050000000000 )
7958 {
7959 bgotonextstatement=false;
7960 {
7961 IkReal j3eval[1];
7962 CheckValue<IkReal> x634 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7963 if(!x634.valid){
7964 continue;
7965 }
7966 IkReal x633=((1.0)*(x634.value));
7967 sj4=0;
7968 cj4=1.0;
7969 j4=0;
7970 sj5=gconst4;
7971 cj5=gconst5;
7972 j5=((3.14159265)+(((-1.0)*x633)));
7973 new_r11=0;
7974 IkReal gconst3=((3.14159265358979)+(((-1.0)*x633)));
7975 IkReal gconst4=0;
7976 IkReal x635 = new_r01*new_r01;
7977 if(IKabs(x635)==0){
7978 continue;
7979 }
7980 IkReal gconst5=((1.0)*new_r01*(pow(x635,-0.5)));
7981 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7982 if( IKabs(j3eval[0]) < 0.0000010000000000 )
7983 {
7984 {
7985 IkReal j3eval[1];
7986 CheckValue<IkReal> x637 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7987 if(!x637.valid){
7988 continue;
7989 }
7990 IkReal x636=((1.0)*(x637.value));
7991 sj4=0;
7992 cj4=1.0;
7993 j4=0;
7994 sj5=gconst4;
7995 cj5=gconst5;
7996 j5=((3.14159265)+(((-1.0)*x636)));
7997 new_r11=0;
7998 IkReal gconst3=((3.14159265358979)+(((-1.0)*x636)));
7999 IkReal gconst4=0;
8000 IkReal x638 = new_r01*new_r01;
8001 if(IKabs(x638)==0){
8002 continue;
8003 }
8004 IkReal gconst5=((1.0)*new_r01*(pow(x638,-0.5)));
8005 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
8006 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8007 {
8008 {
8009 IkReal j3eval[1];
8010 CheckValue<IkReal> x640 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
8011 if(!x640.valid){
8012 continue;
8013 }
8014 IkReal x639=((1.0)*(x640.value));
8015 sj4=0;
8016 cj4=1.0;
8017 j4=0;
8018 sj5=gconst4;
8019 cj5=gconst5;
8020 j5=((3.14159265)+(((-1.0)*x639)));
8021 new_r11=0;
8022 IkReal gconst3=((3.14159265358979)+(((-1.0)*x639)));
8023 IkReal gconst4=0;
8024 IkReal x641 = new_r01*new_r01;
8025 if(IKabs(x641)==0){
8026 continue;
8027 }
8028 IkReal gconst5=((1.0)*new_r01*(pow(x641,-0.5)));
8029 j3eval[0]=new_r01;
8030 if( IKabs(j3eval[0]) < 0.0000010000000000 )
8031 {
8032 continue; // 3 cases reached
8033 
8034 } else
8035 {
8036 {
8037 IkReal j3array[1], cj3array[1], sj3array[1];
8038 bool j3valid[1]={false};
8039 _nj3 = 1;
8040 CheckValue<IkReal> x642=IKPowWithIntegerCheck(new_r01,-1);
8041 if(!x642.valid){
8042 continue;
8043 }
8044 CheckValue<IkReal> x643=IKPowWithIntegerCheck(gconst5,-1);
8045 if(!x643.valid){
8046 continue;
8047 }
8048 if( IKabs(((-1.0)*gconst5*(x642.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x643.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst5*(x642.value)))+IKsqr((new_r00*(x643.value)))-1) <= IKFAST_SINCOS_THRESH )
8049  continue;
8050 j3array[0]=IKatan2(((-1.0)*gconst5*(x642.value)), (new_r00*(x643.value)));
8051 sj3array[0]=IKsin(j3array[0]);
8052 cj3array[0]=IKcos(j3array[0]);
8053 if( j3array[0] > IKPI )
8054 {
8055  j3array[0]-=IK2PI;
8056 }
8057 else if( j3array[0] < -IKPI )
8058 { j3array[0]+=IK2PI;
8059 }
8060 j3valid[0] = true;
8061 for(int ij3 = 0; ij3 < 1; ++ij3)
8062 {
8063 if( !j3valid[ij3] )
8064 {
8065  continue;
8066 }
8067 _ij3[0] = ij3; _ij3[1] = -1;
8068 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8069 {
8070 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8071 {
8072  j3valid[iij3]=false; _ij3[1] = iij3; break;
8073 }
8074 }
8075 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8076 {
8077 IkReal evalcond[8];
8078 IkReal x644=IKcos(j3);
8079 IkReal x645=IKsin(j3);
8080 IkReal x646=((1.0)*gconst5);
8081 evalcond[0]=(new_r01*x644);
8082 evalcond[1]=((-1.0)*gconst5*x644);
8083 evalcond[2]=(gconst5+((new_r01*x645)));
8084 evalcond[3]=(new_r01+((gconst5*x645)));
8085 evalcond[4]=((((-1.0)*x644*x646))+new_r00);
8086 evalcond[5]=((((-1.0)*x645*x646))+new_r10);
8087 evalcond[6]=((((-1.0)*new_r10*x644))+((new_r00*x645)));
8088 evalcond[7]=((((-1.0)*x646))+((new_r10*x645))+((new_r00*x644)));
8089 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8090 {
8091 continue;
8092 }
8093 }
8094 
8095 {
8096 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8097 vinfos[0].jointtype = 1;
8098 vinfos[0].foffset = j0;
8099 vinfos[0].indices[0] = _ij0[0];
8100 vinfos[0].indices[1] = _ij0[1];
8101 vinfos[0].maxsolutions = _nj0;
8102 vinfos[1].jointtype = 1;
8103 vinfos[1].foffset = j1;
8104 vinfos[1].indices[0] = _ij1[0];
8105 vinfos[1].indices[1] = _ij1[1];
8106 vinfos[1].maxsolutions = _nj1;
8107 vinfos[2].jointtype = 1;
8108 vinfos[2].foffset = j2;
8109 vinfos[2].indices[0] = _ij2[0];
8110 vinfos[2].indices[1] = _ij2[1];
8111 vinfos[2].maxsolutions = _nj2;
8112 vinfos[3].jointtype = 1;
8113 vinfos[3].foffset = j3;
8114 vinfos[3].indices[0] = _ij3[0];
8115 vinfos[3].indices[1] = _ij3[1];
8116 vinfos[3].maxsolutions = _nj3;
8117 vinfos[4].jointtype = 1;
8118 vinfos[4].foffset = j4;
8119 vinfos[4].indices[0] = _ij4[0];
8120 vinfos[4].indices[1] = _ij4[1];
8121 vinfos[4].maxsolutions = _nj4;
8122 vinfos[5].jointtype = 1;
8123 vinfos[5].foffset = j5;
8124 vinfos[5].indices[0] = _ij5[0];
8125 vinfos[5].indices[1] = _ij5[1];
8126 vinfos[5].maxsolutions = _nj5;
8127 std::vector<int> vfree(0);
8128 solutions.AddSolution(vinfos,vfree);
8129 }
8130 }
8131 }
8132 
8133 }
8134 
8135 }
8136 
8137 } else
8138 {
8139 {
8140 IkReal j3array[1], cj3array[1], sj3array[1];
8141 bool j3valid[1]={false};
8142 _nj3 = 1;
8143 CheckValue<IkReal> x647 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8144 if(!x647.valid){
8145 continue;
8146 }
8147 CheckValue<IkReal> x648=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8148 if(!x648.valid){
8149 continue;
8150 }
8151 j3array[0]=((-1.5707963267949)+(x647.value)+(((1.5707963267949)*(x648.value))));
8152 sj3array[0]=IKsin(j3array[0]);
8153 cj3array[0]=IKcos(j3array[0]);
8154 if( j3array[0] > IKPI )
8155 {
8156  j3array[0]-=IK2PI;
8157 }
8158 else if( j3array[0] < -IKPI )
8159 { j3array[0]+=IK2PI;
8160 }
8161 j3valid[0] = true;
8162 for(int ij3 = 0; ij3 < 1; ++ij3)
8163 {
8164 if( !j3valid[ij3] )
8165 {
8166  continue;
8167 }
8168 _ij3[0] = ij3; _ij3[1] = -1;
8169 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8170 {
8171 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8172 {
8173  j3valid[iij3]=false; _ij3[1] = iij3; break;
8174 }
8175 }
8176 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8177 {
8178 IkReal evalcond[8];
8179 IkReal x649=IKcos(j3);
8180 IkReal x650=IKsin(j3);
8181 IkReal x651=((1.0)*gconst5);
8182 evalcond[0]=(new_r01*x649);
8183 evalcond[1]=((-1.0)*gconst5*x649);
8184 evalcond[2]=(gconst5+((new_r01*x650)));
8185 evalcond[3]=(new_r01+((gconst5*x650)));
8186 evalcond[4]=((((-1.0)*x649*x651))+new_r00);
8187 evalcond[5]=((((-1.0)*x650*x651))+new_r10);
8188 evalcond[6]=((((-1.0)*new_r10*x649))+((new_r00*x650)));
8189 evalcond[7]=((((-1.0)*x651))+((new_r00*x649))+((new_r10*x650)));
8190 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8191 {
8192 continue;
8193 }
8194 }
8195 
8196 {
8197 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8198 vinfos[0].jointtype = 1;
8199 vinfos[0].foffset = j0;
8200 vinfos[0].indices[0] = _ij0[0];
8201 vinfos[0].indices[1] = _ij0[1];
8202 vinfos[0].maxsolutions = _nj0;
8203 vinfos[1].jointtype = 1;
8204 vinfos[1].foffset = j1;
8205 vinfos[1].indices[0] = _ij1[0];
8206 vinfos[1].indices[1] = _ij1[1];
8207 vinfos[1].maxsolutions = _nj1;
8208 vinfos[2].jointtype = 1;
8209 vinfos[2].foffset = j2;
8210 vinfos[2].indices[0] = _ij2[0];
8211 vinfos[2].indices[1] = _ij2[1];
8212 vinfos[2].maxsolutions = _nj2;
8213 vinfos[3].jointtype = 1;
8214 vinfos[3].foffset = j3;
8215 vinfos[3].indices[0] = _ij3[0];
8216 vinfos[3].indices[1] = _ij3[1];
8217 vinfos[3].maxsolutions = _nj3;
8218 vinfos[4].jointtype = 1;
8219 vinfos[4].foffset = j4;
8220 vinfos[4].indices[0] = _ij4[0];
8221 vinfos[4].indices[1] = _ij4[1];
8222 vinfos[4].maxsolutions = _nj4;
8223 vinfos[5].jointtype = 1;
8224 vinfos[5].foffset = j5;
8225 vinfos[5].indices[0] = _ij5[0];
8226 vinfos[5].indices[1] = _ij5[1];
8227 vinfos[5].maxsolutions = _nj5;
8228 std::vector<int> vfree(0);
8229 solutions.AddSolution(vinfos,vfree);
8230 }
8231 }
8232 }
8233 
8234 }
8235 
8236 }
8237 
8238 } else
8239 {
8240 {
8241 IkReal j3array[1], cj3array[1], sj3array[1];
8242 bool j3valid[1]={false};
8243 _nj3 = 1;
8244 CheckValue<IkReal> x652 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8245 if(!x652.valid){
8246 continue;
8247 }
8248 CheckValue<IkReal> x653=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8249 if(!x653.valid){
8250 continue;
8251 }
8252 j3array[0]=((-1.5707963267949)+(x652.value)+(((1.5707963267949)*(x653.value))));
8253 sj3array[0]=IKsin(j3array[0]);
8254 cj3array[0]=IKcos(j3array[0]);
8255 if( j3array[0] > IKPI )
8256 {
8257  j3array[0]-=IK2PI;
8258 }
8259 else if( j3array[0] < -IKPI )
8260 { j3array[0]+=IK2PI;
8261 }
8262 j3valid[0] = true;
8263 for(int ij3 = 0; ij3 < 1; ++ij3)
8264 {
8265 if( !j3valid[ij3] )
8266 {
8267  continue;
8268 }
8269 _ij3[0] = ij3; _ij3[1] = -1;
8270 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8271 {
8272 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8273 {
8274  j3valid[iij3]=false; _ij3[1] = iij3; break;
8275 }
8276 }
8277 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8278 {
8279 IkReal evalcond[8];
8280 IkReal x654=IKcos(j3);
8281 IkReal x655=IKsin(j3);
8282 IkReal x656=((1.0)*gconst5);
8283 evalcond[0]=(new_r01*x654);
8284 evalcond[1]=((-1.0)*gconst5*x654);
8285 evalcond[2]=(gconst5+((new_r01*x655)));
8286 evalcond[3]=(new_r01+((gconst5*x655)));
8287 evalcond[4]=(new_r00+(((-1.0)*x654*x656)));
8288 evalcond[5]=((((-1.0)*x655*x656))+new_r10);
8289 evalcond[6]=((((-1.0)*new_r10*x654))+((new_r00*x655)));
8290 evalcond[7]=((((-1.0)*x656))+((new_r10*x655))+((new_r00*x654)));
8291 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8292 {
8293 continue;
8294 }
8295 }
8296 
8297 {
8298 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8299 vinfos[0].jointtype = 1;
8300 vinfos[0].foffset = j0;
8301 vinfos[0].indices[0] = _ij0[0];
8302 vinfos[0].indices[1] = _ij0[1];
8303 vinfos[0].maxsolutions = _nj0;
8304 vinfos[1].jointtype = 1;
8305 vinfos[1].foffset = j1;
8306 vinfos[1].indices[0] = _ij1[0];
8307 vinfos[1].indices[1] = _ij1[1];
8308 vinfos[1].maxsolutions = _nj1;
8309 vinfos[2].jointtype = 1;
8310 vinfos[2].foffset = j2;
8311 vinfos[2].indices[0] = _ij2[0];
8312 vinfos[2].indices[1] = _ij2[1];
8313 vinfos[2].maxsolutions = _nj2;
8314 vinfos[3].jointtype = 1;
8315 vinfos[3].foffset = j3;
8316 vinfos[3].indices[0] = _ij3[0];
8317 vinfos[3].indices[1] = _ij3[1];
8318 vinfos[3].maxsolutions = _nj3;
8319 vinfos[4].jointtype = 1;
8320 vinfos[4].foffset = j4;
8321 vinfos[4].indices[0] = _ij4[0];
8322 vinfos[4].indices[1] = _ij4[1];
8323 vinfos[4].maxsolutions = _nj4;
8324 vinfos[5].jointtype = 1;
8325 vinfos[5].foffset = j5;
8326 vinfos[5].indices[0] = _ij5[0];
8327 vinfos[5].indices[1] = _ij5[1];
8328 vinfos[5].maxsolutions = _nj5;
8329 std::vector<int> vfree(0);
8330 solutions.AddSolution(vinfos,vfree);
8331 }
8332 }
8333 }
8334 
8335 }
8336 
8337 }
8338 
8339 }
8340 } while(0);
8341 if( bgotonextstatement )
8342 {
8343 bool bgotonextstatement = true;
8344 do
8345 {
8346 if( 1 )
8347 {
8348 bgotonextstatement=false;
8349 continue; // branch miss [j3]
8350 
8351 }
8352 } while(0);
8353 if( bgotonextstatement )
8354 {
8355 }
8356 }
8357 }
8358 }
8359 }
8360 }
8361 
8362 } else
8363 {
8364 {
8365 IkReal j3array[1], cj3array[1], sj3array[1];
8366 bool j3valid[1]={false};
8367 _nj3 = 1;
8368 IkReal x657=((1.0)*new_r01);
8369 CheckValue<IkReal> x658 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x657))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
8370 if(!x658.valid){
8371 continue;
8372 }
8373 CheckValue<IkReal> x659=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x657)))),-1);
8374 if(!x659.valid){
8375 continue;
8376 }
8377 j3array[0]=((-1.5707963267949)+(x658.value)+(((1.5707963267949)*(x659.value))));
8378 sj3array[0]=IKsin(j3array[0]);
8379 cj3array[0]=IKcos(j3array[0]);
8380 if( j3array[0] > IKPI )
8381 {
8382  j3array[0]-=IK2PI;
8383 }
8384 else if( j3array[0] < -IKPI )
8385 { j3array[0]+=IK2PI;
8386 }
8387 j3valid[0] = true;
8388 for(int ij3 = 0; ij3 < 1; ++ij3)
8389 {
8390 if( !j3valid[ij3] )
8391 {
8392  continue;
8393 }
8394 _ij3[0] = ij3; _ij3[1] = -1;
8395 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8396 {
8397 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8398 {
8399  j3valid[iij3]=false; _ij3[1] = iij3; break;
8400 }
8401 }
8402 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8403 {
8404 IkReal evalcond[8];
8405 IkReal x660=IKsin(j3);
8406 IkReal x661=IKcos(j3);
8407 IkReal x662=((1.0)*gconst5);
8408 IkReal x663=(gconst4*x660);
8409 IkReal x664=(gconst4*x661);
8410 IkReal x665=((1.0)*x661);
8411 IkReal x666=(x661*x662);
8412 evalcond[0]=(gconst4+((new_r11*x660))+((new_r01*x661)));
8413 evalcond[1]=(((gconst5*x660))+x664+new_r01);
8414 evalcond[2]=(gconst4+(((-1.0)*new_r10*x665))+((new_r00*x660)));
8415 evalcond[3]=(gconst5+(((-1.0)*new_r11*x665))+((new_r01*x660)));
8416 evalcond[4]=((((-1.0)*x666))+x663+new_r00);
8417 evalcond[5]=((((-1.0)*x666))+x663+new_r11);
8418 evalcond[6]=(((new_r10*x660))+(((-1.0)*x662))+((new_r00*x661)));
8419 evalcond[7]=((((-1.0)*x660*x662))+(((-1.0)*x664))+new_r10);
8420 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8421 {
8422 continue;
8423 }
8424 }
8425 
8426 {
8427 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8428 vinfos[0].jointtype = 1;
8429 vinfos[0].foffset = j0;
8430 vinfos[0].indices[0] = _ij0[0];
8431 vinfos[0].indices[1] = _ij0[1];
8432 vinfos[0].maxsolutions = _nj0;
8433 vinfos[1].jointtype = 1;
8434 vinfos[1].foffset = j1;
8435 vinfos[1].indices[0] = _ij1[0];
8436 vinfos[1].indices[1] = _ij1[1];
8437 vinfos[1].maxsolutions = _nj1;
8438 vinfos[2].jointtype = 1;
8439 vinfos[2].foffset = j2;
8440 vinfos[2].indices[0] = _ij2[0];
8441 vinfos[2].indices[1] = _ij2[1];
8442 vinfos[2].maxsolutions = _nj2;
8443 vinfos[3].jointtype = 1;
8444 vinfos[3].foffset = j3;
8445 vinfos[3].indices[0] = _ij3[0];
8446 vinfos[3].indices[1] = _ij3[1];
8447 vinfos[3].maxsolutions = _nj3;
8448 vinfos[4].jointtype = 1;
8449 vinfos[4].foffset = j4;
8450 vinfos[4].indices[0] = _ij4[0];
8451 vinfos[4].indices[1] = _ij4[1];
8452 vinfos[4].maxsolutions = _nj4;
8453 vinfos[5].jointtype = 1;
8454 vinfos[5].foffset = j5;
8455 vinfos[5].indices[0] = _ij5[0];
8456 vinfos[5].indices[1] = _ij5[1];
8457 vinfos[5].maxsolutions = _nj5;
8458 std::vector<int> vfree(0);
8459 solutions.AddSolution(vinfos,vfree);
8460 }
8461 }
8462 }
8463 
8464 }
8465 
8466 }
8467 
8468 } else
8469 {
8470 {
8471 IkReal j3array[1], cj3array[1], sj3array[1];
8472 bool j3valid[1]={false};
8473 _nj3 = 1;
8474 IkReal x667=((1.0)*gconst4);
8475 CheckValue<IkReal> x668 = IKatan2WithCheck(IkReal(((gconst4*gconst4)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst5*x667)))),IKFAST_ATAN2_MAGTHRESH);
8476 if(!x668.valid){
8477 continue;
8478 }
8479 CheckValue<IkReal> x669=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst5*new_r10))+(((-1.0)*new_r00*x667)))),-1);
8480 if(!x669.valid){
8481 continue;
8482 }
8483 j3array[0]=((-1.5707963267949)+(x668.value)+(((1.5707963267949)*(x669.value))));
8484 sj3array[0]=IKsin(j3array[0]);
8485 cj3array[0]=IKcos(j3array[0]);
8486 if( j3array[0] > IKPI )
8487 {
8488  j3array[0]-=IK2PI;
8489 }
8490 else if( j3array[0] < -IKPI )
8491 { j3array[0]+=IK2PI;
8492 }
8493 j3valid[0] = true;
8494 for(int ij3 = 0; ij3 < 1; ++ij3)
8495 {
8496 if( !j3valid[ij3] )
8497 {
8498  continue;
8499 }
8500 _ij3[0] = ij3; _ij3[1] = -1;
8501 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8502 {
8503 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8504 {
8505  j3valid[iij3]=false; _ij3[1] = iij3; break;
8506 }
8507 }
8508 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8509 {
8510 IkReal evalcond[8];
8511 IkReal x670=IKsin(j3);
8512 IkReal x671=IKcos(j3);
8513 IkReal x672=((1.0)*gconst5);
8514 IkReal x673=(gconst4*x670);
8515 IkReal x674=(gconst4*x671);
8516 IkReal x675=((1.0)*x671);
8517 IkReal x676=(x671*x672);
8518 evalcond[0]=(((new_r11*x670))+((new_r01*x671))+gconst4);
8519 evalcond[1]=(((gconst5*x670))+x674+new_r01);
8520 evalcond[2]=(((new_r00*x670))+gconst4+(((-1.0)*new_r10*x675)));
8521 evalcond[3]=(((new_r01*x670))+gconst5+(((-1.0)*new_r11*x675)));
8522 evalcond[4]=(x673+new_r00+(((-1.0)*x676)));
8523 evalcond[5]=(x673+new_r11+(((-1.0)*x676)));
8524 evalcond[6]=(((new_r00*x671))+((new_r10*x670))+(((-1.0)*x672)));
8525 evalcond[7]=((((-1.0)*x670*x672))+(((-1.0)*x674))+new_r10);
8526 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8527 {
8528 continue;
8529 }
8530 }
8531 
8532 {
8533 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8534 vinfos[0].jointtype = 1;
8535 vinfos[0].foffset = j0;
8536 vinfos[0].indices[0] = _ij0[0];
8537 vinfos[0].indices[1] = _ij0[1];
8538 vinfos[0].maxsolutions = _nj0;
8539 vinfos[1].jointtype = 1;
8540 vinfos[1].foffset = j1;
8541 vinfos[1].indices[0] = _ij1[0];
8542 vinfos[1].indices[1] = _ij1[1];
8543 vinfos[1].maxsolutions = _nj1;
8544 vinfos[2].jointtype = 1;
8545 vinfos[2].foffset = j2;
8546 vinfos[2].indices[0] = _ij2[0];
8547 vinfos[2].indices[1] = _ij2[1];
8548 vinfos[2].maxsolutions = _nj2;
8549 vinfos[3].jointtype = 1;
8550 vinfos[3].foffset = j3;
8551 vinfos[3].indices[0] = _ij3[0];
8552 vinfos[3].indices[1] = _ij3[1];
8553 vinfos[3].maxsolutions = _nj3;
8554 vinfos[4].jointtype = 1;
8555 vinfos[4].foffset = j4;
8556 vinfos[4].indices[0] = _ij4[0];
8557 vinfos[4].indices[1] = _ij4[1];
8558 vinfos[4].maxsolutions = _nj4;
8559 vinfos[5].jointtype = 1;
8560 vinfos[5].foffset = j5;
8561 vinfos[5].indices[0] = _ij5[0];
8562 vinfos[5].indices[1] = _ij5[1];
8563 vinfos[5].maxsolutions = _nj5;
8564 std::vector<int> vfree(0);
8565 solutions.AddSolution(vinfos,vfree);
8566 }
8567 }
8568 }
8569 
8570 }
8571 
8572 }
8573 
8574 } else
8575 {
8576 {
8577 IkReal j3array[1], cj3array[1], sj3array[1];
8578 bool j3valid[1]={false};
8579 _nj3 = 1;
8580 IkReal x677=((1.0)*new_r11);
8581 CheckValue<IkReal> x678=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r10*x677)))),-1);
8582 if(!x678.valid){
8583 continue;
8584 }
8585 CheckValue<IkReal> x679 = IKatan2WithCheck(IkReal((((gconst4*new_r10))+((gconst4*new_r01)))),IkReal((((gconst4*new_r00))+(((-1.0)*gconst4*x677)))),IKFAST_ATAN2_MAGTHRESH);
8586 if(!x679.valid){
8587 continue;
8588 }
8589 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x678.value)))+(x679.value));
8590 sj3array[0]=IKsin(j3array[0]);
8591 cj3array[0]=IKcos(j3array[0]);
8592 if( j3array[0] > IKPI )
8593 {
8594  j3array[0]-=IK2PI;
8595 }
8596 else if( j3array[0] < -IKPI )
8597 { j3array[0]+=IK2PI;
8598 }
8599 j3valid[0] = true;
8600 for(int ij3 = 0; ij3 < 1; ++ij3)
8601 {
8602 if( !j3valid[ij3] )
8603 {
8604  continue;
8605 }
8606 _ij3[0] = ij3; _ij3[1] = -1;
8607 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8608 {
8609 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8610 {
8611  j3valid[iij3]=false; _ij3[1] = iij3; break;
8612 }
8613 }
8614 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8615 {
8616 IkReal evalcond[8];
8617 IkReal x680=IKsin(j3);
8618 IkReal x681=IKcos(j3);
8619 IkReal x682=((1.0)*gconst5);
8620 IkReal x683=(gconst4*x680);
8621 IkReal x684=(gconst4*x681);
8622 IkReal x685=((1.0)*x681);
8623 IkReal x686=(x681*x682);
8624 evalcond[0]=(gconst4+((new_r01*x681))+((new_r11*x680)));
8625 evalcond[1]=(((gconst5*x680))+x684+new_r01);
8626 evalcond[2]=((((-1.0)*new_r10*x685))+gconst4+((new_r00*x680)));
8627 evalcond[3]=(gconst5+((new_r01*x680))+(((-1.0)*new_r11*x685)));
8628 evalcond[4]=((((-1.0)*x686))+x683+new_r00);
8629 evalcond[5]=((((-1.0)*x686))+x683+new_r11);
8630 evalcond[6]=((((-1.0)*x682))+((new_r00*x681))+((new_r10*x680)));
8631 evalcond[7]=((((-1.0)*x680*x682))+new_r10+(((-1.0)*x684)));
8632 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8633 {
8634 continue;
8635 }
8636 }
8637 
8638 {
8639 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8640 vinfos[0].jointtype = 1;
8641 vinfos[0].foffset = j0;
8642 vinfos[0].indices[0] = _ij0[0];
8643 vinfos[0].indices[1] = _ij0[1];
8644 vinfos[0].maxsolutions = _nj0;
8645 vinfos[1].jointtype = 1;
8646 vinfos[1].foffset = j1;
8647 vinfos[1].indices[0] = _ij1[0];
8648 vinfos[1].indices[1] = _ij1[1];
8649 vinfos[1].maxsolutions = _nj1;
8650 vinfos[2].jointtype = 1;
8651 vinfos[2].foffset = j2;
8652 vinfos[2].indices[0] = _ij2[0];
8653 vinfos[2].indices[1] = _ij2[1];
8654 vinfos[2].maxsolutions = _nj2;
8655 vinfos[3].jointtype = 1;
8656 vinfos[3].foffset = j3;
8657 vinfos[3].indices[0] = _ij3[0];
8658 vinfos[3].indices[1] = _ij3[1];
8659 vinfos[3].maxsolutions = _nj3;
8660 vinfos[4].jointtype = 1;
8661 vinfos[4].foffset = j4;
8662 vinfos[4].indices[0] = _ij4[0];
8663 vinfos[4].indices[1] = _ij4[1];
8664 vinfos[4].maxsolutions = _nj4;
8665 vinfos[5].jointtype = 1;
8666 vinfos[5].foffset = j5;
8667 vinfos[5].indices[0] = _ij5[0];
8668 vinfos[5].indices[1] = _ij5[1];
8669 vinfos[5].maxsolutions = _nj5;
8670 std::vector<int> vfree(0);
8671 solutions.AddSolution(vinfos,vfree);
8672 }
8673 }
8674 }
8675 
8676 }
8677 
8678 }
8679 
8680 }
8681 } while(0);
8682 if( bgotonextstatement )
8683 {
8684 bool bgotonextstatement = true;
8685 do
8686 {
8687 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8688 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8689 {
8690 bgotonextstatement=false;
8691 {
8692 IkReal j3array[1], cj3array[1], sj3array[1];
8693 bool j3valid[1]={false};
8694 _nj3 = 1;
8695 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 )
8696  continue;
8697 j3array[0]=IKatan2(((-1.0)*new_r01), new_r00);
8698 sj3array[0]=IKsin(j3array[0]);
8699 cj3array[0]=IKcos(j3array[0]);
8700 if( j3array[0] > IKPI )
8701 {
8702  j3array[0]-=IK2PI;
8703 }
8704 else if( j3array[0] < -IKPI )
8705 { j3array[0]+=IK2PI;
8706 }
8707 j3valid[0] = true;
8708 for(int ij3 = 0; ij3 < 1; ++ij3)
8709 {
8710 if( !j3valid[ij3] )
8711 {
8712  continue;
8713 }
8714 _ij3[0] = ij3; _ij3[1] = -1;
8715 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8716 {
8717 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8718 {
8719  j3valid[iij3]=false; _ij3[1] = iij3; break;
8720 }
8721 }
8722 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8723 {
8724 IkReal evalcond[8];
8725 IkReal x687=IKsin(j3);
8726 IkReal x688=IKcos(j3);
8727 IkReal x689=((1.0)*x688);
8728 evalcond[0]=(x687+new_r01);
8729 evalcond[1]=((((-1.0)*x689))+new_r00);
8730 evalcond[2]=((((-1.0)*x689))+new_r11);
8731 evalcond[3]=(new_r10+(((-1.0)*x687)));
8732 evalcond[4]=(((new_r01*x688))+((new_r11*x687)));
8733 evalcond[5]=((((-1.0)*new_r10*x689))+((new_r00*x687)));
8734 evalcond[6]=((-1.0)+((new_r00*x688))+((new_r10*x687)));
8735 evalcond[7]=((1.0)+((new_r01*x687))+(((-1.0)*new_r11*x689)));
8736 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8737 {
8738 continue;
8739 }
8740 }
8741 
8742 {
8743 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8744 vinfos[0].jointtype = 1;
8745 vinfos[0].foffset = j0;
8746 vinfos[0].indices[0] = _ij0[0];
8747 vinfos[0].indices[1] = _ij0[1];
8748 vinfos[0].maxsolutions = _nj0;
8749 vinfos[1].jointtype = 1;
8750 vinfos[1].foffset = j1;
8751 vinfos[1].indices[0] = _ij1[0];
8752 vinfos[1].indices[1] = _ij1[1];
8753 vinfos[1].maxsolutions = _nj1;
8754 vinfos[2].jointtype = 1;
8755 vinfos[2].foffset = j2;
8756 vinfos[2].indices[0] = _ij2[0];
8757 vinfos[2].indices[1] = _ij2[1];
8758 vinfos[2].maxsolutions = _nj2;
8759 vinfos[3].jointtype = 1;
8760 vinfos[3].foffset = j3;
8761 vinfos[3].indices[0] = _ij3[0];
8762 vinfos[3].indices[1] = _ij3[1];
8763 vinfos[3].maxsolutions = _nj3;
8764 vinfos[4].jointtype = 1;
8765 vinfos[4].foffset = j4;
8766 vinfos[4].indices[0] = _ij4[0];
8767 vinfos[4].indices[1] = _ij4[1];
8768 vinfos[4].maxsolutions = _nj4;
8769 vinfos[5].jointtype = 1;
8770 vinfos[5].foffset = j5;
8771 vinfos[5].indices[0] = _ij5[0];
8772 vinfos[5].indices[1] = _ij5[1];
8773 vinfos[5].maxsolutions = _nj5;
8774 std::vector<int> vfree(0);
8775 solutions.AddSolution(vinfos,vfree);
8776 }
8777 }
8778 }
8779 
8780 }
8781 } while(0);
8782 if( bgotonextstatement )
8783 {
8784 bool bgotonextstatement = true;
8785 do
8786 {
8787 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8788 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8789 {
8790 bgotonextstatement=false;
8791 {
8792 IkReal j3array[1], cj3array[1], sj3array[1];
8793 bool j3valid[1]={false};
8794 _nj3 = 1;
8795 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 )
8796  continue;
8797 j3array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8798 sj3array[0]=IKsin(j3array[0]);
8799 cj3array[0]=IKcos(j3array[0]);
8800 if( j3array[0] > IKPI )
8801 {
8802  j3array[0]-=IK2PI;
8803 }
8804 else if( j3array[0] < -IKPI )
8805 { j3array[0]+=IK2PI;
8806 }
8807 j3valid[0] = true;
8808 for(int ij3 = 0; ij3 < 1; ++ij3)
8809 {
8810 if( !j3valid[ij3] )
8811 {
8812  continue;
8813 }
8814 _ij3[0] = ij3; _ij3[1] = -1;
8815 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8816 {
8817 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8818 {
8819  j3valid[iij3]=false; _ij3[1] = iij3; break;
8820 }
8821 }
8822 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8823 {
8824 IkReal evalcond[8];
8825 IkReal x690=IKcos(j3);
8826 IkReal x691=IKsin(j3);
8827 IkReal x692=((1.0)*x690);
8828 evalcond[0]=(x690+new_r00);
8829 evalcond[1]=(x690+new_r11);
8830 evalcond[2]=(x691+new_r10);
8831 evalcond[3]=(new_r01+(((-1.0)*x691)));
8832 evalcond[4]=(((new_r11*x691))+((new_r01*x690)));
8833 evalcond[5]=((((-1.0)*new_r10*x692))+((new_r00*x691)));
8834 evalcond[6]=((1.0)+((new_r10*x691))+((new_r00*x690)));
8835 evalcond[7]=((-1.0)+(((-1.0)*new_r11*x692))+((new_r01*x691)));
8836 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8837 {
8838 continue;
8839 }
8840 }
8841 
8842 {
8843 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8844 vinfos[0].jointtype = 1;
8845 vinfos[0].foffset = j0;
8846 vinfos[0].indices[0] = _ij0[0];
8847 vinfos[0].indices[1] = _ij0[1];
8848 vinfos[0].maxsolutions = _nj0;
8849 vinfos[1].jointtype = 1;
8850 vinfos[1].foffset = j1;
8851 vinfos[1].indices[0] = _ij1[0];
8852 vinfos[1].indices[1] = _ij1[1];
8853 vinfos[1].maxsolutions = _nj1;
8854 vinfos[2].jointtype = 1;
8855 vinfos[2].foffset = j2;
8856 vinfos[2].indices[0] = _ij2[0];
8857 vinfos[2].indices[1] = _ij2[1];
8858 vinfos[2].maxsolutions = _nj2;
8859 vinfos[3].jointtype = 1;
8860 vinfos[3].foffset = j3;
8861 vinfos[3].indices[0] = _ij3[0];
8862 vinfos[3].indices[1] = _ij3[1];
8863 vinfos[3].maxsolutions = _nj3;
8864 vinfos[4].jointtype = 1;
8865 vinfos[4].foffset = j4;
8866 vinfos[4].indices[0] = _ij4[0];
8867 vinfos[4].indices[1] = _ij4[1];
8868 vinfos[4].maxsolutions = _nj4;
8869 vinfos[5].jointtype = 1;
8870 vinfos[5].foffset = j5;
8871 vinfos[5].indices[0] = _ij5[0];
8872 vinfos[5].indices[1] = _ij5[1];
8873 vinfos[5].maxsolutions = _nj5;
8874 std::vector<int> vfree(0);
8875 solutions.AddSolution(vinfos,vfree);
8876 }
8877 }
8878 }
8879 
8880 }
8881 } while(0);
8882 if( bgotonextstatement )
8883 {
8884 bool bgotonextstatement = true;
8885 do
8886 {
8887 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
8888 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8889 {
8890 bgotonextstatement=false;
8891 {
8892 IkReal j3eval[3];
8893 sj4=0;
8894 cj4=1.0;
8895 j4=0;
8896 new_r11=0;
8897 new_r00=0;
8898 j3eval[0]=new_r01;
8899 j3eval[1]=IKsign(new_r01);
8900 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
8901 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
8902 {
8903 {
8904 IkReal j3eval[2];
8905 sj4=0;
8906 cj4=1.0;
8907 j4=0;
8908 new_r11=0;
8909 new_r00=0;
8910 j3eval[0]=new_r01;
8911 j3eval[1]=new_r10;
8912 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8913 {
8914 {
8915 IkReal j3eval[2];
8916 sj4=0;
8917 cj4=1.0;
8918 j4=0;
8919 new_r11=0;
8920 new_r00=0;
8921 j3eval[0]=new_r01;
8922 j3eval[1]=sj5;
8923 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8924 {
8925 {
8926 IkReal evalcond[1];
8927 bool bgotonextstatement = true;
8928 do
8929 {
8930 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8931 if( IKabs(evalcond[0]) < 0.0000050000000000 )
8932 {
8933 bgotonextstatement=false;
8934 {
8935 IkReal j3array[2], cj3array[2], sj3array[2];
8936 bool j3valid[2]={false};
8937 _nj3 = 2;
8938 sj3array[0]=new_r10;
8939 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8940 {
8941  j3valid[0] = j3valid[1] = true;
8942  j3array[0] = IKasin(sj3array[0]);
8943  cj3array[0] = IKcos(j3array[0]);
8944  sj3array[1] = sj3array[0];
8945  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8946  cj3array[1] = -cj3array[0];
8947 }
8948 else if( isnan(sj3array[0]) )
8949 {
8950  // probably any value will work
8951  j3valid[0] = true;
8952  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8953 }
8954 for(int ij3 = 0; ij3 < 2; ++ij3)
8955 {
8956 if( !j3valid[ij3] )
8957 {
8958  continue;
8959 }
8960 _ij3[0] = ij3; _ij3[1] = -1;
8961 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8962 {
8963 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8964 {
8965  j3valid[iij3]=false; _ij3[1] = iij3; break;
8966 }
8967 }
8968 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8969 {
8970 IkReal evalcond[6];
8971 IkReal x693=IKcos(j3);
8972 IkReal x694=IKsin(j3);
8973 IkReal x695=((-1.0)*x693);
8974 evalcond[0]=(new_r01*x693);
8975 evalcond[1]=(x694+new_r01);
8976 evalcond[2]=x695;
8977 evalcond[3]=(new_r10*x695);
8978 evalcond[4]=((1.0)+((new_r01*x694)));
8979 evalcond[5]=((-1.0)+((new_r10*x694)));
8980 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8981 {
8982 continue;
8983 }
8984 }
8985 
8986 {
8987 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8988 vinfos[0].jointtype = 1;
8989 vinfos[0].foffset = j0;
8990 vinfos[0].indices[0] = _ij0[0];
8991 vinfos[0].indices[1] = _ij0[1];
8992 vinfos[0].maxsolutions = _nj0;
8993 vinfos[1].jointtype = 1;
8994 vinfos[1].foffset = j1;
8995 vinfos[1].indices[0] = _ij1[0];
8996 vinfos[1].indices[1] = _ij1[1];
8997 vinfos[1].maxsolutions = _nj1;
8998 vinfos[2].jointtype = 1;
8999 vinfos[2].foffset = j2;
9000 vinfos[2].indices[0] = _ij2[0];
9001 vinfos[2].indices[1] = _ij2[1];
9002 vinfos[2].maxsolutions = _nj2;
9003 vinfos[3].jointtype = 1;
9004 vinfos[3].foffset = j3;
9005 vinfos[3].indices[0] = _ij3[0];
9006 vinfos[3].indices[1] = _ij3[1];
9007 vinfos[3].maxsolutions = _nj3;
9008 vinfos[4].jointtype = 1;
9009 vinfos[4].foffset = j4;
9010 vinfos[4].indices[0] = _ij4[0];
9011 vinfos[4].indices[1] = _ij4[1];
9012 vinfos[4].maxsolutions = _nj4;
9013 vinfos[5].jointtype = 1;
9014 vinfos[5].foffset = j5;
9015 vinfos[5].indices[0] = _ij5[0];
9016 vinfos[5].indices[1] = _ij5[1];
9017 vinfos[5].maxsolutions = _nj5;
9018 std::vector<int> vfree(0);
9019 solutions.AddSolution(vinfos,vfree);
9020 }
9021 }
9022 }
9023 
9024 }
9025 } while(0);
9026 if( bgotonextstatement )
9027 {
9028 bool bgotonextstatement = true;
9029 do
9030 {
9031 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
9032 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9033 {
9034 bgotonextstatement=false;
9035 {
9036 IkReal j3array[2], cj3array[2], sj3array[2];
9037 bool j3valid[2]={false};
9038 _nj3 = 2;
9039 sj3array[0]=new_r01;
9040 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
9041 {
9042  j3valid[0] = j3valid[1] = true;
9043  j3array[0] = IKasin(sj3array[0]);
9044  cj3array[0] = IKcos(j3array[0]);
9045  sj3array[1] = sj3array[0];
9046  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
9047  cj3array[1] = -cj3array[0];
9048 }
9049 else if( isnan(sj3array[0]) )
9050 {
9051  // probably any value will work
9052  j3valid[0] = true;
9053  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
9054 }
9055 for(int ij3 = 0; ij3 < 2; ++ij3)
9056 {
9057 if( !j3valid[ij3] )
9058 {
9059  continue;
9060 }
9061 _ij3[0] = ij3; _ij3[1] = -1;
9062 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9063 {
9064 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9065 {
9066  j3valid[iij3]=false; _ij3[1] = iij3; break;
9067 }
9068 }
9069 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9070 {
9071 IkReal evalcond[6];
9072 IkReal x696=IKcos(j3);
9073 IkReal x697=IKsin(j3);
9074 evalcond[0]=x696;
9075 evalcond[1]=(new_r01*x696);
9076 evalcond[2]=(x697+new_r10);
9077 evalcond[3]=((-1.0)*new_r10*x696);
9078 evalcond[4]=((-1.0)+((new_r01*x697)));
9079 evalcond[5]=((1.0)+((new_r10*x697)));
9080 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
9081 {
9082 continue;
9083 }
9084 }
9085 
9086 {
9087 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9088 vinfos[0].jointtype = 1;
9089 vinfos[0].foffset = j0;
9090 vinfos[0].indices[0] = _ij0[0];
9091 vinfos[0].indices[1] = _ij0[1];
9092 vinfos[0].maxsolutions = _nj0;
9093 vinfos[1].jointtype = 1;
9094 vinfos[1].foffset = j1;
9095 vinfos[1].indices[0] = _ij1[0];
9096 vinfos[1].indices[1] = _ij1[1];
9097 vinfos[1].maxsolutions = _nj1;
9098 vinfos[2].jointtype = 1;
9099 vinfos[2].foffset = j2;
9100 vinfos[2].indices[0] = _ij2[0];
9101 vinfos[2].indices[1] = _ij2[1];
9102 vinfos[2].maxsolutions = _nj2;
9103 vinfos[3].jointtype = 1;
9104 vinfos[3].foffset = j3;
9105 vinfos[3].indices[0] = _ij3[0];
9106 vinfos[3].indices[1] = _ij3[1];
9107 vinfos[3].maxsolutions = _nj3;
9108 vinfos[4].jointtype = 1;
9109 vinfos[4].foffset = j4;
9110 vinfos[4].indices[0] = _ij4[0];
9111 vinfos[4].indices[1] = _ij4[1];
9112 vinfos[4].maxsolutions = _nj4;
9113 vinfos[5].jointtype = 1;
9114 vinfos[5].foffset = j5;
9115 vinfos[5].indices[0] = _ij5[0];
9116 vinfos[5].indices[1] = _ij5[1];
9117 vinfos[5].maxsolutions = _nj5;
9118 std::vector<int> vfree(0);
9119 solutions.AddSolution(vinfos,vfree);
9120 }
9121 }
9122 }
9123 
9124 }
9125 } while(0);
9126 if( bgotonextstatement )
9127 {
9128 bool bgotonextstatement = true;
9129 do
9130 {
9131 if( 1 )
9132 {
9133 bgotonextstatement=false;
9134 continue; // branch miss [j3]
9135 
9136 }
9137 } while(0);
9138 if( bgotonextstatement )
9139 {
9140 }
9141 }
9142 }
9143 }
9144 
9145 } else
9146 {
9147 {
9148 IkReal j3array[1], cj3array[1], sj3array[1];
9149 bool j3valid[1]={false};
9150 _nj3 = 1;
9151 CheckValue<IkReal> x699=IKPowWithIntegerCheck(new_r01,-1);
9152 if(!x699.valid){
9153 continue;
9154 }
9155 IkReal x698=x699.value;
9156 CheckValue<IkReal> x700=IKPowWithIntegerCheck(sj5,-1);
9157 if(!x700.valid){
9158 continue;
9159 }
9160 CheckValue<IkReal> x701=IKPowWithIntegerCheck(x698,-2);
9161 if(!x701.valid){
9162 continue;
9163 }
9164 if( IKabs(((-1.0)*cj5*x698)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*x698))+IKsqr((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))))-1) <= IKFAST_SINCOS_THRESH )
9165  continue;
9166 j3array[0]=IKatan2(((-1.0)*cj5*x698), (x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))));
9167 sj3array[0]=IKsin(j3array[0]);
9168 cj3array[0]=IKcos(j3array[0]);
9169 if( j3array[0] > IKPI )
9170 {
9171  j3array[0]-=IK2PI;
9172 }
9173 else if( j3array[0] < -IKPI )
9174 { j3array[0]+=IK2PI;
9175 }
9176 j3valid[0] = true;
9177 for(int ij3 = 0; ij3 < 1; ++ij3)
9178 {
9179 if( !j3valid[ij3] )
9180 {
9181  continue;
9182 }
9183 _ij3[0] = ij3; _ij3[1] = -1;
9184 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9185 {
9186 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9187 {
9188  j3valid[iij3]=false; _ij3[1] = iij3; break;
9189 }
9190 }
9191 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9192 {
9193 IkReal evalcond[7];
9194 IkReal x702=IKcos(j3);
9195 IkReal x703=IKsin(j3);
9196 IkReal x704=((1.0)*cj5);
9197 IkReal x705=(sj5*x702);
9198 evalcond[0]=(cj5+((new_r01*x703)));
9199 evalcond[1]=(sj5+((new_r01*x702)));
9200 evalcond[2]=(sj5+(((-1.0)*new_r10*x702)));
9201 evalcond[3]=((((-1.0)*x704))+((new_r10*x703)));
9202 evalcond[4]=(((cj5*x703))+x705+new_r01);
9203 evalcond[5]=((((-1.0)*x702*x704))+((sj5*x703)));
9204 evalcond[6]=((((-1.0)*x705))+(((-1.0)*x703*x704))+new_r10);
9205 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9206 {
9207 continue;
9208 }
9209 }
9210 
9211 {
9212 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9213 vinfos[0].jointtype = 1;
9214 vinfos[0].foffset = j0;
9215 vinfos[0].indices[0] = _ij0[0];
9216 vinfos[0].indices[1] = _ij0[1];
9217 vinfos[0].maxsolutions = _nj0;
9218 vinfos[1].jointtype = 1;
9219 vinfos[1].foffset = j1;
9220 vinfos[1].indices[0] = _ij1[0];
9221 vinfos[1].indices[1] = _ij1[1];
9222 vinfos[1].maxsolutions = _nj1;
9223 vinfos[2].jointtype = 1;
9224 vinfos[2].foffset = j2;
9225 vinfos[2].indices[0] = _ij2[0];
9226 vinfos[2].indices[1] = _ij2[1];
9227 vinfos[2].maxsolutions = _nj2;
9228 vinfos[3].jointtype = 1;
9229 vinfos[3].foffset = j3;
9230 vinfos[3].indices[0] = _ij3[0];
9231 vinfos[3].indices[1] = _ij3[1];
9232 vinfos[3].maxsolutions = _nj3;
9233 vinfos[4].jointtype = 1;
9234 vinfos[4].foffset = j4;
9235 vinfos[4].indices[0] = _ij4[0];
9236 vinfos[4].indices[1] = _ij4[1];
9237 vinfos[4].maxsolutions = _nj4;
9238 vinfos[5].jointtype = 1;
9239 vinfos[5].foffset = j5;
9240 vinfos[5].indices[0] = _ij5[0];
9241 vinfos[5].indices[1] = _ij5[1];
9242 vinfos[5].maxsolutions = _nj5;
9243 std::vector<int> vfree(0);
9244 solutions.AddSolution(vinfos,vfree);
9245 }
9246 }
9247 }
9248 
9249 }
9250 
9251 }
9252 
9253 } else
9254 {
9255 {
9256 IkReal j3array[1], cj3array[1], sj3array[1];
9257 bool j3valid[1]={false};
9258 _nj3 = 1;
9259 CheckValue<IkReal> x706=IKPowWithIntegerCheck(new_r01,-1);
9260 if(!x706.valid){
9261 continue;
9262 }
9263 CheckValue<IkReal> x707=IKPowWithIntegerCheck(new_r10,-1);
9264 if(!x707.valid){
9265 continue;
9266 }
9267 if( IKabs(((-1.0)*cj5*(x706.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x707.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x706.value)))+IKsqr((sj5*(x707.value)))-1) <= IKFAST_SINCOS_THRESH )
9268  continue;
9269 j3array[0]=IKatan2(((-1.0)*cj5*(x706.value)), (sj5*(x707.value)));
9270 sj3array[0]=IKsin(j3array[0]);
9271 cj3array[0]=IKcos(j3array[0]);
9272 if( j3array[0] > IKPI )
9273 {
9274  j3array[0]-=IK2PI;
9275 }
9276 else if( j3array[0] < -IKPI )
9277 { j3array[0]+=IK2PI;
9278 }
9279 j3valid[0] = true;
9280 for(int ij3 = 0; ij3 < 1; ++ij3)
9281 {
9282 if( !j3valid[ij3] )
9283 {
9284  continue;
9285 }
9286 _ij3[0] = ij3; _ij3[1] = -1;
9287 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9288 {
9289 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9290 {
9291  j3valid[iij3]=false; _ij3[1] = iij3; break;
9292 }
9293 }
9294 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9295 {
9296 IkReal evalcond[7];
9297 IkReal x708=IKcos(j3);
9298 IkReal x709=IKsin(j3);
9299 IkReal x710=((1.0)*cj5);
9300 IkReal x711=(sj5*x708);
9301 evalcond[0]=(cj5+((new_r01*x709)));
9302 evalcond[1]=(sj5+((new_r01*x708)));
9303 evalcond[2]=(sj5+(((-1.0)*new_r10*x708)));
9304 evalcond[3]=((((-1.0)*x710))+((new_r10*x709)));
9305 evalcond[4]=(((cj5*x709))+x711+new_r01);
9306 evalcond[5]=(((sj5*x709))+(((-1.0)*x708*x710)));
9307 evalcond[6]=((((-1.0)*x709*x710))+(((-1.0)*x711))+new_r10);
9308 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9309 {
9310 continue;
9311 }
9312 }
9313 
9314 {
9315 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9316 vinfos[0].jointtype = 1;
9317 vinfos[0].foffset = j0;
9318 vinfos[0].indices[0] = _ij0[0];
9319 vinfos[0].indices[1] = _ij0[1];
9320 vinfos[0].maxsolutions = _nj0;
9321 vinfos[1].jointtype = 1;
9322 vinfos[1].foffset = j1;
9323 vinfos[1].indices[0] = _ij1[0];
9324 vinfos[1].indices[1] = _ij1[1];
9325 vinfos[1].maxsolutions = _nj1;
9326 vinfos[2].jointtype = 1;
9327 vinfos[2].foffset = j2;
9328 vinfos[2].indices[0] = _ij2[0];
9329 vinfos[2].indices[1] = _ij2[1];
9330 vinfos[2].maxsolutions = _nj2;
9331 vinfos[3].jointtype = 1;
9332 vinfos[3].foffset = j3;
9333 vinfos[3].indices[0] = _ij3[0];
9334 vinfos[3].indices[1] = _ij3[1];
9335 vinfos[3].maxsolutions = _nj3;
9336 vinfos[4].jointtype = 1;
9337 vinfos[4].foffset = j4;
9338 vinfos[4].indices[0] = _ij4[0];
9339 vinfos[4].indices[1] = _ij4[1];
9340 vinfos[4].maxsolutions = _nj4;
9341 vinfos[5].jointtype = 1;
9342 vinfos[5].foffset = j5;
9343 vinfos[5].indices[0] = _ij5[0];
9344 vinfos[5].indices[1] = _ij5[1];
9345 vinfos[5].maxsolutions = _nj5;
9346 std::vector<int> vfree(0);
9347 solutions.AddSolution(vinfos,vfree);
9348 }
9349 }
9350 }
9351 
9352 }
9353 
9354 }
9355 
9356 } else
9357 {
9358 {
9359 IkReal j3array[1], cj3array[1], sj3array[1];
9360 bool j3valid[1]={false};
9361 _nj3 = 1;
9362 CheckValue<IkReal> x712=IKPowWithIntegerCheck(IKsign(new_r01),-1);
9363 if(!x712.valid){
9364 continue;
9365 }
9366 CheckValue<IkReal> x713 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(((-1.0)*sj5)),IKFAST_ATAN2_MAGTHRESH);
9367 if(!x713.valid){
9368 continue;
9369 }
9370 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x712.value)))+(x713.value));
9371 sj3array[0]=IKsin(j3array[0]);
9372 cj3array[0]=IKcos(j3array[0]);
9373 if( j3array[0] > IKPI )
9374 {
9375  j3array[0]-=IK2PI;
9376 }
9377 else if( j3array[0] < -IKPI )
9378 { j3array[0]+=IK2PI;
9379 }
9380 j3valid[0] = true;
9381 for(int ij3 = 0; ij3 < 1; ++ij3)
9382 {
9383 if( !j3valid[ij3] )
9384 {
9385  continue;
9386 }
9387 _ij3[0] = ij3; _ij3[1] = -1;
9388 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9389 {
9390 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9391 {
9392  j3valid[iij3]=false; _ij3[1] = iij3; break;
9393 }
9394 }
9395 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9396 {
9397 IkReal evalcond[7];
9398 IkReal x714=IKcos(j3);
9399 IkReal x715=IKsin(j3);
9400 IkReal x716=((1.0)*cj5);
9401 IkReal x717=(sj5*x714);
9402 evalcond[0]=(cj5+((new_r01*x715)));
9403 evalcond[1]=(sj5+((new_r01*x714)));
9404 evalcond[2]=(sj5+(((-1.0)*new_r10*x714)));
9405 evalcond[3]=((((-1.0)*x716))+((new_r10*x715)));
9406 evalcond[4]=(((cj5*x715))+x717+new_r01);
9407 evalcond[5]=((((-1.0)*x714*x716))+((sj5*x715)));
9408 evalcond[6]=((((-1.0)*x717))+new_r10+(((-1.0)*x715*x716)));
9409 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9410 {
9411 continue;
9412 }
9413 }
9414 
9415 {
9416 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9417 vinfos[0].jointtype = 1;
9418 vinfos[0].foffset = j0;
9419 vinfos[0].indices[0] = _ij0[0];
9420 vinfos[0].indices[1] = _ij0[1];
9421 vinfos[0].maxsolutions = _nj0;
9422 vinfos[1].jointtype = 1;
9423 vinfos[1].foffset = j1;
9424 vinfos[1].indices[0] = _ij1[0];
9425 vinfos[1].indices[1] = _ij1[1];
9426 vinfos[1].maxsolutions = _nj1;
9427 vinfos[2].jointtype = 1;
9428 vinfos[2].foffset = j2;
9429 vinfos[2].indices[0] = _ij2[0];
9430 vinfos[2].indices[1] = _ij2[1];
9431 vinfos[2].maxsolutions = _nj2;
9432 vinfos[3].jointtype = 1;
9433 vinfos[3].foffset = j3;
9434 vinfos[3].indices[0] = _ij3[0];
9435 vinfos[3].indices[1] = _ij3[1];
9436 vinfos[3].maxsolutions = _nj3;
9437 vinfos[4].jointtype = 1;
9438 vinfos[4].foffset = j4;
9439 vinfos[4].indices[0] = _ij4[0];
9440 vinfos[4].indices[1] = _ij4[1];
9441 vinfos[4].maxsolutions = _nj4;
9442 vinfos[5].jointtype = 1;
9443 vinfos[5].foffset = j5;
9444 vinfos[5].indices[0] = _ij5[0];
9445 vinfos[5].indices[1] = _ij5[1];
9446 vinfos[5].maxsolutions = _nj5;
9447 std::vector<int> vfree(0);
9448 solutions.AddSolution(vinfos,vfree);
9449 }
9450 }
9451 }
9452 
9453 }
9454 
9455 }
9456 
9457 }
9458 } while(0);
9459 if( bgotonextstatement )
9460 {
9461 bool bgotonextstatement = true;
9462 do
9463 {
9464 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9465 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9466 {
9467 bgotonextstatement=false;
9468 {
9469 IkReal j3eval[1];
9470 sj4=0;
9471 cj4=1.0;
9472 j4=0;
9473 new_r11=0;
9474 new_r01=0;
9475 new_r22=0;
9476 new_r20=0;
9477 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9478 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9479 {
9480 continue; // no branches [j3]
9481 
9482 } else
9483 {
9484 {
9485 IkReal j3array[2], cj3array[2], sj3array[2];
9486 bool j3valid[2]={false};
9487 _nj3 = 2;
9488 CheckValue<IkReal> x719 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9489 if(!x719.valid){
9490 continue;
9491 }
9492 IkReal x718=x719.value;
9493 j3array[0]=((-1.0)*x718);
9494 sj3array[0]=IKsin(j3array[0]);
9495 cj3array[0]=IKcos(j3array[0]);
9496 j3array[1]=((3.14159265358979)+(((-1.0)*x718)));
9497 sj3array[1]=IKsin(j3array[1]);
9498 cj3array[1]=IKcos(j3array[1]);
9499 if( j3array[0] > IKPI )
9500 {
9501  j3array[0]-=IK2PI;
9502 }
9503 else if( j3array[0] < -IKPI )
9504 { j3array[0]+=IK2PI;
9505 }
9506 j3valid[0] = true;
9507 if( j3array[1] > IKPI )
9508 {
9509  j3array[1]-=IK2PI;
9510 }
9511 else if( j3array[1] < -IKPI )
9512 { j3array[1]+=IK2PI;
9513 }
9514 j3valid[1] = true;
9515 for(int ij3 = 0; ij3 < 2; ++ij3)
9516 {
9517 if( !j3valid[ij3] )
9518 {
9519  continue;
9520 }
9521 _ij3[0] = ij3; _ij3[1] = -1;
9522 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9523 {
9524 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9525 {
9526  j3valid[iij3]=false; _ij3[1] = iij3; break;
9527 }
9528 }
9529 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9530 {
9531 IkReal evalcond[1];
9532 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
9533 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9534 {
9535 continue;
9536 }
9537 }
9538 
9539 {
9540 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9541 vinfos[0].jointtype = 1;
9542 vinfos[0].foffset = j0;
9543 vinfos[0].indices[0] = _ij0[0];
9544 vinfos[0].indices[1] = _ij0[1];
9545 vinfos[0].maxsolutions = _nj0;
9546 vinfos[1].jointtype = 1;
9547 vinfos[1].foffset = j1;
9548 vinfos[1].indices[0] = _ij1[0];
9549 vinfos[1].indices[1] = _ij1[1];
9550 vinfos[1].maxsolutions = _nj1;
9551 vinfos[2].jointtype = 1;
9552 vinfos[2].foffset = j2;
9553 vinfos[2].indices[0] = _ij2[0];
9554 vinfos[2].indices[1] = _ij2[1];
9555 vinfos[2].maxsolutions = _nj2;
9556 vinfos[3].jointtype = 1;
9557 vinfos[3].foffset = j3;
9558 vinfos[3].indices[0] = _ij3[0];
9559 vinfos[3].indices[1] = _ij3[1];
9560 vinfos[3].maxsolutions = _nj3;
9561 vinfos[4].jointtype = 1;
9562 vinfos[4].foffset = j4;
9563 vinfos[4].indices[0] = _ij4[0];
9564 vinfos[4].indices[1] = _ij4[1];
9565 vinfos[4].maxsolutions = _nj4;
9566 vinfos[5].jointtype = 1;
9567 vinfos[5].foffset = j5;
9568 vinfos[5].indices[0] = _ij5[0];
9569 vinfos[5].indices[1] = _ij5[1];
9570 vinfos[5].maxsolutions = _nj5;
9571 std::vector<int> vfree(0);
9572 solutions.AddSolution(vinfos,vfree);
9573 }
9574 }
9575 }
9576 
9577 }
9578 
9579 }
9580 
9581 }
9582 } while(0);
9583 if( bgotonextstatement )
9584 {
9585 bool bgotonextstatement = true;
9586 do
9587 {
9588 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9589 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9590 {
9591 bgotonextstatement=false;
9592 {
9593 IkReal j3eval[1];
9594 sj4=0;
9595 cj4=1.0;
9596 j4=0;
9597 new_r00=0;
9598 new_r10=0;
9599 new_r21=0;
9600 new_r22=0;
9601 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9602 if( IKabs(j3eval[0]) < 0.0000010000000000 )
9603 {
9604 continue; // no branches [j3]
9605 
9606 } else
9607 {
9608 {
9609 IkReal j3array[2], cj3array[2], sj3array[2];
9610 bool j3valid[2]={false};
9611 _nj3 = 2;
9612 CheckValue<IkReal> x721 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9613 if(!x721.valid){
9614 continue;
9615 }
9616 IkReal x720=x721.value;
9617 j3array[0]=((-1.0)*x720);
9618 sj3array[0]=IKsin(j3array[0]);
9619 cj3array[0]=IKcos(j3array[0]);
9620 j3array[1]=((3.14159265358979)+(((-1.0)*x720)));
9621 sj3array[1]=IKsin(j3array[1]);
9622 cj3array[1]=IKcos(j3array[1]);
9623 if( j3array[0] > IKPI )
9624 {
9625  j3array[0]-=IK2PI;
9626 }
9627 else if( j3array[0] < -IKPI )
9628 { j3array[0]+=IK2PI;
9629 }
9630 j3valid[0] = true;
9631 if( j3array[1] > IKPI )
9632 {
9633  j3array[1]-=IK2PI;
9634 }
9635 else if( j3array[1] < -IKPI )
9636 { j3array[1]+=IK2PI;
9637 }
9638 j3valid[1] = true;
9639 for(int ij3 = 0; ij3 < 2; ++ij3)
9640 {
9641 if( !j3valid[ij3] )
9642 {
9643  continue;
9644 }
9645 _ij3[0] = ij3; _ij3[1] = -1;
9646 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9647 {
9648 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9649 {
9650  j3valid[iij3]=false; _ij3[1] = iij3; break;
9651 }
9652 }
9653 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9654 {
9655 IkReal evalcond[1];
9656 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
9657 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9658 {
9659 continue;
9660 }
9661 }
9662 
9663 {
9664 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9665 vinfos[0].jointtype = 1;
9666 vinfos[0].foffset = j0;
9667 vinfos[0].indices[0] = _ij0[0];
9668 vinfos[0].indices[1] = _ij0[1];
9669 vinfos[0].maxsolutions = _nj0;
9670 vinfos[1].jointtype = 1;
9671 vinfos[1].foffset = j1;
9672 vinfos[1].indices[0] = _ij1[0];
9673 vinfos[1].indices[1] = _ij1[1];
9674 vinfos[1].maxsolutions = _nj1;
9675 vinfos[2].jointtype = 1;
9676 vinfos[2].foffset = j2;
9677 vinfos[2].indices[0] = _ij2[0];
9678 vinfos[2].indices[1] = _ij2[1];
9679 vinfos[2].maxsolutions = _nj2;
9680 vinfos[3].jointtype = 1;
9681 vinfos[3].foffset = j3;
9682 vinfos[3].indices[0] = _ij3[0];
9683 vinfos[3].indices[1] = _ij3[1];
9684 vinfos[3].maxsolutions = _nj3;
9685 vinfos[4].jointtype = 1;
9686 vinfos[4].foffset = j4;
9687 vinfos[4].indices[0] = _ij4[0];
9688 vinfos[4].indices[1] = _ij4[1];
9689 vinfos[4].maxsolutions = _nj4;
9690 vinfos[5].jointtype = 1;
9691 vinfos[5].foffset = j5;
9692 vinfos[5].indices[0] = _ij5[0];
9693 vinfos[5].indices[1] = _ij5[1];
9694 vinfos[5].maxsolutions = _nj5;
9695 std::vector<int> vfree(0);
9696 solutions.AddSolution(vinfos,vfree);
9697 }
9698 }
9699 }
9700 
9701 }
9702 
9703 }
9704 
9705 }
9706 } while(0);
9707 if( bgotonextstatement )
9708 {
9709 bool bgotonextstatement = true;
9710 do
9711 {
9712 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
9713 if( IKabs(evalcond[0]) < 0.0000050000000000 )
9714 {
9715 bgotonextstatement=false;
9716 {
9717 IkReal j3eval[3];
9718 sj4=0;
9719 cj4=1.0;
9720 j4=0;
9721 new_r01=0;
9722 new_r10=0;
9723 j3eval[0]=new_r11;
9724 j3eval[1]=IKsign(new_r11);
9725 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
9726 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9727 {
9728 {
9729 IkReal j3eval[3];
9730 sj4=0;
9731 cj4=1.0;
9732 j4=0;
9733 new_r01=0;
9734 new_r10=0;
9735 j3eval[0]=new_r00;
9736 j3eval[1]=((IKabs(cj5))+(IKabs(sj5)));
9737 j3eval[2]=IKsign(new_r00);
9738 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9739 {
9740 {
9741 IkReal j3eval[2];
9742 sj4=0;
9743 cj4=1.0;
9744 j4=0;
9745 new_r01=0;
9746 new_r10=0;
9747 j3eval[0]=new_r00;
9748 j3eval[1]=new_r11;
9749 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9750 {
9751 continue; // no branches [j3]
9752 
9753 } else
9754 {
9755 {
9756 IkReal j3array[1], cj3array[1], sj3array[1];
9757 bool j3valid[1]={false};
9758 _nj3 = 1;
9759 CheckValue<IkReal> x722=IKPowWithIntegerCheck(new_r00,-1);
9760 if(!x722.valid){
9761 continue;
9762 }
9763 CheckValue<IkReal> x723=IKPowWithIntegerCheck(new_r11,-1);
9764 if(!x723.valid){
9765 continue;
9766 }
9767 if( IKabs(((-1.0)*sj5*(x722.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x723.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x722.value)))+IKsqr((cj5*(x723.value)))-1) <= IKFAST_SINCOS_THRESH )
9768  continue;
9769 j3array[0]=IKatan2(((-1.0)*sj5*(x722.value)), (cj5*(x723.value)));
9770 sj3array[0]=IKsin(j3array[0]);
9771 cj3array[0]=IKcos(j3array[0]);
9772 if( j3array[0] > IKPI )
9773 {
9774  j3array[0]-=IK2PI;
9775 }
9776 else if( j3array[0] < -IKPI )
9777 { j3array[0]+=IK2PI;
9778 }
9779 j3valid[0] = true;
9780 for(int ij3 = 0; ij3 < 1; ++ij3)
9781 {
9782 if( !j3valid[ij3] )
9783 {
9784  continue;
9785 }
9786 _ij3[0] = ij3; _ij3[1] = -1;
9787 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9788 {
9789 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9790 {
9791  j3valid[iij3]=false; _ij3[1] = iij3; break;
9792 }
9793 }
9794 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9795 {
9796 IkReal evalcond[7];
9797 IkReal x724=IKsin(j3);
9798 IkReal x725=IKcos(j3);
9799 IkReal x726=(sj5*x724);
9800 IkReal x727=((1.0)*x725);
9801 IkReal x728=(cj5*x727);
9802 evalcond[0]=(sj5+((new_r00*x724)));
9803 evalcond[1]=(sj5+((new_r11*x724)));
9804 evalcond[2]=(cj5+(((-1.0)*new_r11*x727)));
9805 evalcond[3]=(((new_r00*x725))+(((-1.0)*cj5)));
9806 evalcond[4]=(((cj5*x724))+((sj5*x725)));
9807 evalcond[5]=((((-1.0)*x728))+x726+new_r00);
9808 evalcond[6]=((((-1.0)*x728))+x726+new_r11);
9809 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9810 {
9811 continue;
9812 }
9813 }
9814 
9815 {
9816 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9817 vinfos[0].jointtype = 1;
9818 vinfos[0].foffset = j0;
9819 vinfos[0].indices[0] = _ij0[0];
9820 vinfos[0].indices[1] = _ij0[1];
9821 vinfos[0].maxsolutions = _nj0;
9822 vinfos[1].jointtype = 1;
9823 vinfos[1].foffset = j1;
9824 vinfos[1].indices[0] = _ij1[0];
9825 vinfos[1].indices[1] = _ij1[1];
9826 vinfos[1].maxsolutions = _nj1;
9827 vinfos[2].jointtype = 1;
9828 vinfos[2].foffset = j2;
9829 vinfos[2].indices[0] = _ij2[0];
9830 vinfos[2].indices[1] = _ij2[1];
9831 vinfos[2].maxsolutions = _nj2;
9832 vinfos[3].jointtype = 1;
9833 vinfos[3].foffset = j3;
9834 vinfos[3].indices[0] = _ij3[0];
9835 vinfos[3].indices[1] = _ij3[1];
9836 vinfos[3].maxsolutions = _nj3;
9837 vinfos[4].jointtype = 1;
9838 vinfos[4].foffset = j4;
9839 vinfos[4].indices[0] = _ij4[0];
9840 vinfos[4].indices[1] = _ij4[1];
9841 vinfos[4].maxsolutions = _nj4;
9842 vinfos[5].jointtype = 1;
9843 vinfos[5].foffset = j5;
9844 vinfos[5].indices[0] = _ij5[0];
9845 vinfos[5].indices[1] = _ij5[1];
9846 vinfos[5].maxsolutions = _nj5;
9847 std::vector<int> vfree(0);
9848 solutions.AddSolution(vinfos,vfree);
9849 }
9850 }
9851 }
9852 
9853 }
9854 
9855 }
9856 
9857 } else
9858 {
9859 {
9860 IkReal j3array[1], cj3array[1], sj3array[1];
9861 bool j3valid[1]={false};
9862 _nj3 = 1;
9863 CheckValue<IkReal> x729=IKPowWithIntegerCheck(IKsign(new_r00),-1);
9864 if(!x729.valid){
9865 continue;
9866 }
9867 CheckValue<IkReal> x730 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9868 if(!x730.valid){
9869 continue;
9870 }
9871 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x729.value)))+(x730.value));
9872 sj3array[0]=IKsin(j3array[0]);
9873 cj3array[0]=IKcos(j3array[0]);
9874 if( j3array[0] > IKPI )
9875 {
9876  j3array[0]-=IK2PI;
9877 }
9878 else if( j3array[0] < -IKPI )
9879 { j3array[0]+=IK2PI;
9880 }
9881 j3valid[0] = true;
9882 for(int ij3 = 0; ij3 < 1; ++ij3)
9883 {
9884 if( !j3valid[ij3] )
9885 {
9886  continue;
9887 }
9888 _ij3[0] = ij3; _ij3[1] = -1;
9889 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9890 {
9891 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9892 {
9893  j3valid[iij3]=false; _ij3[1] = iij3; break;
9894 }
9895 }
9896 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9897 {
9898 IkReal evalcond[7];
9899 IkReal x731=IKsin(j3);
9900 IkReal x732=IKcos(j3);
9901 IkReal x733=(sj5*x731);
9902 IkReal x734=((1.0)*x732);
9903 IkReal x735=(cj5*x734);
9904 evalcond[0]=(sj5+((new_r00*x731)));
9905 evalcond[1]=(sj5+((new_r11*x731)));
9906 evalcond[2]=(cj5+(((-1.0)*new_r11*x734)));
9907 evalcond[3]=(((new_r00*x732))+(((-1.0)*cj5)));
9908 evalcond[4]=(((cj5*x731))+((sj5*x732)));
9909 evalcond[5]=((((-1.0)*x735))+x733+new_r00);
9910 evalcond[6]=((((-1.0)*x735))+x733+new_r11);
9911 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9912 {
9913 continue;
9914 }
9915 }
9916 
9917 {
9918 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9919 vinfos[0].jointtype = 1;
9920 vinfos[0].foffset = j0;
9921 vinfos[0].indices[0] = _ij0[0];
9922 vinfos[0].indices[1] = _ij0[1];
9923 vinfos[0].maxsolutions = _nj0;
9924 vinfos[1].jointtype = 1;
9925 vinfos[1].foffset = j1;
9926 vinfos[1].indices[0] = _ij1[0];
9927 vinfos[1].indices[1] = _ij1[1];
9928 vinfos[1].maxsolutions = _nj1;
9929 vinfos[2].jointtype = 1;
9930 vinfos[2].foffset = j2;
9931 vinfos[2].indices[0] = _ij2[0];
9932 vinfos[2].indices[1] = _ij2[1];
9933 vinfos[2].maxsolutions = _nj2;
9934 vinfos[3].jointtype = 1;
9935 vinfos[3].foffset = j3;
9936 vinfos[3].indices[0] = _ij3[0];
9937 vinfos[3].indices[1] = _ij3[1];
9938 vinfos[3].maxsolutions = _nj3;
9939 vinfos[4].jointtype = 1;
9940 vinfos[4].foffset = j4;
9941 vinfos[4].indices[0] = _ij4[0];
9942 vinfos[4].indices[1] = _ij4[1];
9943 vinfos[4].maxsolutions = _nj4;
9944 vinfos[5].jointtype = 1;
9945 vinfos[5].foffset = j5;
9946 vinfos[5].indices[0] = _ij5[0];
9947 vinfos[5].indices[1] = _ij5[1];
9948 vinfos[5].maxsolutions = _nj5;
9949 std::vector<int> vfree(0);
9950 solutions.AddSolution(vinfos,vfree);
9951 }
9952 }
9953 }
9954 
9955 }
9956 
9957 }
9958 
9959 } else
9960 {
9961 {
9962 IkReal j3array[1], cj3array[1], sj3array[1];
9963 bool j3valid[1]={false};
9964 _nj3 = 1;
9965 CheckValue<IkReal> x736=IKPowWithIntegerCheck(IKsign(new_r11),-1);
9966 if(!x736.valid){
9967 continue;
9968 }
9969 CheckValue<IkReal> x737 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9970 if(!x737.valid){
9971 continue;
9972 }
9973 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x736.value)))+(x737.value));
9974 sj3array[0]=IKsin(j3array[0]);
9975 cj3array[0]=IKcos(j3array[0]);
9976 if( j3array[0] > IKPI )
9977 {
9978  j3array[0]-=IK2PI;
9979 }
9980 else if( j3array[0] < -IKPI )
9981 { j3array[0]+=IK2PI;
9982 }
9983 j3valid[0] = true;
9984 for(int ij3 = 0; ij3 < 1; ++ij3)
9985 {
9986 if( !j3valid[ij3] )
9987 {
9988  continue;
9989 }
9990 _ij3[0] = ij3; _ij3[1] = -1;
9991 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9992 {
9993 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9994 {
9995  j3valid[iij3]=false; _ij3[1] = iij3; break;
9996 }
9997 }
9998 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9999 {
10000 IkReal evalcond[7];
10001 IkReal x738=IKsin(j3);
10002 IkReal x739=IKcos(j3);
10003 IkReal x740=(sj5*x738);
10004 IkReal x741=((1.0)*x739);
10005 IkReal x742=(cj5*x741);
10006 evalcond[0]=(sj5+((new_r00*x738)));
10007 evalcond[1]=(sj5+((new_r11*x738)));
10008 evalcond[2]=(cj5+(((-1.0)*new_r11*x741)));
10009 evalcond[3]=(((new_r00*x739))+(((-1.0)*cj5)));
10010 evalcond[4]=(((cj5*x738))+((sj5*x739)));
10011 evalcond[5]=(x740+new_r00+(((-1.0)*x742)));
10012 evalcond[6]=(x740+new_r11+(((-1.0)*x742)));
10013 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
10014 {
10015 continue;
10016 }
10017 }
10018 
10019 {
10020 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10021 vinfos[0].jointtype = 1;
10022 vinfos[0].foffset = j0;
10023 vinfos[0].indices[0] = _ij0[0];
10024 vinfos[0].indices[1] = _ij0[1];
10025 vinfos[0].maxsolutions = _nj0;
10026 vinfos[1].jointtype = 1;
10027 vinfos[1].foffset = j1;
10028 vinfos[1].indices[0] = _ij1[0];
10029 vinfos[1].indices[1] = _ij1[1];
10030 vinfos[1].maxsolutions = _nj1;
10031 vinfos[2].jointtype = 1;
10032 vinfos[2].foffset = j2;
10033 vinfos[2].indices[0] = _ij2[0];
10034 vinfos[2].indices[1] = _ij2[1];
10035 vinfos[2].maxsolutions = _nj2;
10036 vinfos[3].jointtype = 1;
10037 vinfos[3].foffset = j3;
10038 vinfos[3].indices[0] = _ij3[0];
10039 vinfos[3].indices[1] = _ij3[1];
10040 vinfos[3].maxsolutions = _nj3;
10041 vinfos[4].jointtype = 1;
10042 vinfos[4].foffset = j4;
10043 vinfos[4].indices[0] = _ij4[0];
10044 vinfos[4].indices[1] = _ij4[1];
10045 vinfos[4].maxsolutions = _nj4;
10046 vinfos[5].jointtype = 1;
10047 vinfos[5].foffset = j5;
10048 vinfos[5].indices[0] = _ij5[0];
10049 vinfos[5].indices[1] = _ij5[1];
10050 vinfos[5].maxsolutions = _nj5;
10051 std::vector<int> vfree(0);
10052 solutions.AddSolution(vinfos,vfree);
10053 }
10054 }
10055 }
10056 
10057 }
10058 
10059 }
10060 
10061 }
10062 } while(0);
10063 if( bgotonextstatement )
10064 {
10065 bool bgotonextstatement = true;
10066 do
10067 {
10068 if( 1 )
10069 {
10070 bgotonextstatement=false;
10071 continue; // branch miss [j3]
10072 
10073 }
10074 } while(0);
10075 if( bgotonextstatement )
10076 {
10077 }
10078 }
10079 }
10080 }
10081 }
10082 }
10083 }
10084 }
10085 }
10086 }
10087 
10088 } else
10089 {
10090 {
10091 IkReal j3array[1], cj3array[1], sj3array[1];
10092 bool j3valid[1]={false};
10093 _nj3 = 1;
10094 IkReal x743=((1.0)*new_r11);
10095 CheckValue<IkReal> x744 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((1.0)+(((-1.0)*new_r00*x743))+(((-1.0)*(cj5*cj5))))),IKFAST_ATAN2_MAGTHRESH);
10096 if(!x744.valid){
10097 continue;
10098 }
10099 CheckValue<IkReal> x745=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*x743)))),-1);
10100 if(!x745.valid){
10101 continue;
10102 }
10103 j3array[0]=((-1.5707963267949)+(x744.value)+(((1.5707963267949)*(x745.value))));
10104 sj3array[0]=IKsin(j3array[0]);
10105 cj3array[0]=IKcos(j3array[0]);
10106 if( j3array[0] > IKPI )
10107 {
10108  j3array[0]-=IK2PI;
10109 }
10110 else if( j3array[0] < -IKPI )
10111 { j3array[0]+=IK2PI;
10112 }
10113 j3valid[0] = true;
10114 for(int ij3 = 0; ij3 < 1; ++ij3)
10115 {
10116 if( !j3valid[ij3] )
10117 {
10118  continue;
10119 }
10120 _ij3[0] = ij3; _ij3[1] = -1;
10121 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10122 {
10123 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10124 {
10125  j3valid[iij3]=false; _ij3[1] = iij3; break;
10126 }
10127 }
10128 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10129 {
10130 IkReal evalcond[8];
10131 IkReal x746=IKcos(j3);
10132 IkReal x747=IKsin(j3);
10133 IkReal x748=(sj5*x747);
10134 IkReal x749=(cj5*x747);
10135 IkReal x750=(sj5*x746);
10136 IkReal x751=((1.0)*x746);
10137 IkReal x752=(cj5*x751);
10138 evalcond[0]=(sj5+((new_r01*x746))+((new_r11*x747)));
10139 evalcond[1]=(x750+x749+new_r01);
10140 evalcond[2]=(sj5+(((-1.0)*new_r10*x751))+((new_r00*x747)));
10141 evalcond[3]=(cj5+(((-1.0)*new_r11*x751))+((new_r01*x747)));
10142 evalcond[4]=(x748+new_r00+(((-1.0)*x752)));
10143 evalcond[5]=(x748+new_r11+(((-1.0)*x752)));
10144 evalcond[6]=(((new_r10*x747))+((new_r00*x746))+(((-1.0)*cj5)));
10145 evalcond[7]=((((-1.0)*x750))+(((-1.0)*x749))+new_r10);
10146 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10147 {
10148 continue;
10149 }
10150 }
10151 
10152 {
10153 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10154 vinfos[0].jointtype = 1;
10155 vinfos[0].foffset = j0;
10156 vinfos[0].indices[0] = _ij0[0];
10157 vinfos[0].indices[1] = _ij0[1];
10158 vinfos[0].maxsolutions = _nj0;
10159 vinfos[1].jointtype = 1;
10160 vinfos[1].foffset = j1;
10161 vinfos[1].indices[0] = _ij1[0];
10162 vinfos[1].indices[1] = _ij1[1];
10163 vinfos[1].maxsolutions = _nj1;
10164 vinfos[2].jointtype = 1;
10165 vinfos[2].foffset = j2;
10166 vinfos[2].indices[0] = _ij2[0];
10167 vinfos[2].indices[1] = _ij2[1];
10168 vinfos[2].maxsolutions = _nj2;
10169 vinfos[3].jointtype = 1;
10170 vinfos[3].foffset = j3;
10171 vinfos[3].indices[0] = _ij3[0];
10172 vinfos[3].indices[1] = _ij3[1];
10173 vinfos[3].maxsolutions = _nj3;
10174 vinfos[4].jointtype = 1;
10175 vinfos[4].foffset = j4;
10176 vinfos[4].indices[0] = _ij4[0];
10177 vinfos[4].indices[1] = _ij4[1];
10178 vinfos[4].maxsolutions = _nj4;
10179 vinfos[5].jointtype = 1;
10180 vinfos[5].foffset = j5;
10181 vinfos[5].indices[0] = _ij5[0];
10182 vinfos[5].indices[1] = _ij5[1];
10183 vinfos[5].maxsolutions = _nj5;
10184 std::vector<int> vfree(0);
10185 solutions.AddSolution(vinfos,vfree);
10186 }
10187 }
10188 }
10189 
10190 }
10191 
10192 }
10193 
10194 } else
10195 {
10196 {
10197 IkReal j3array[1], cj3array[1], sj3array[1];
10198 bool j3valid[1]={false};
10199 _nj3 = 1;
10200 CheckValue<IkReal> x753 = IKatan2WithCheck(IkReal((((new_r11*sj5))+((cj5*new_r01)))),IkReal((((new_r01*sj5))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10201 if(!x753.valid){
10202 continue;
10203 }
10204 CheckValue<IkReal> x754=IKPowWithIntegerCheck(IKsign(((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))))),-1);
10205 if(!x754.valid){
10206 continue;
10207 }
10208 j3array[0]=((-1.5707963267949)+(x753.value)+(((1.5707963267949)*(x754.value))));
10209 sj3array[0]=IKsin(j3array[0]);
10210 cj3array[0]=IKcos(j3array[0]);
10211 if( j3array[0] > IKPI )
10212 {
10213  j3array[0]-=IK2PI;
10214 }
10215 else if( j3array[0] < -IKPI )
10216 { j3array[0]+=IK2PI;
10217 }
10218 j3valid[0] = true;
10219 for(int ij3 = 0; ij3 < 1; ++ij3)
10220 {
10221 if( !j3valid[ij3] )
10222 {
10223  continue;
10224 }
10225 _ij3[0] = ij3; _ij3[1] = -1;
10226 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10227 {
10228 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10229 {
10230  j3valid[iij3]=false; _ij3[1] = iij3; break;
10231 }
10232 }
10233 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10234 {
10235 IkReal evalcond[8];
10236 IkReal x755=IKcos(j3);
10237 IkReal x756=IKsin(j3);
10238 IkReal x757=(sj5*x756);
10239 IkReal x758=(cj5*x756);
10240 IkReal x759=(sj5*x755);
10241 IkReal x760=((1.0)*x755);
10242 IkReal x761=(cj5*x760);
10243 evalcond[0]=(sj5+((new_r11*x756))+((new_r01*x755)));
10244 evalcond[1]=(x759+x758+new_r01);
10245 evalcond[2]=(sj5+(((-1.0)*new_r10*x760))+((new_r00*x756)));
10246 evalcond[3]=(cj5+(((-1.0)*new_r11*x760))+((new_r01*x756)));
10247 evalcond[4]=((((-1.0)*x761))+x757+new_r00);
10248 evalcond[5]=((((-1.0)*x761))+x757+new_r11);
10249 evalcond[6]=(((new_r00*x755))+((new_r10*x756))+(((-1.0)*cj5)));
10250 evalcond[7]=((((-1.0)*x759))+(((-1.0)*x758))+new_r10);
10251 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10252 {
10253 continue;
10254 }
10255 }
10256 
10257 {
10258 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10259 vinfos[0].jointtype = 1;
10260 vinfos[0].foffset = j0;
10261 vinfos[0].indices[0] = _ij0[0];
10262 vinfos[0].indices[1] = _ij0[1];
10263 vinfos[0].maxsolutions = _nj0;
10264 vinfos[1].jointtype = 1;
10265 vinfos[1].foffset = j1;
10266 vinfos[1].indices[0] = _ij1[0];
10267 vinfos[1].indices[1] = _ij1[1];
10268 vinfos[1].maxsolutions = _nj1;
10269 vinfos[2].jointtype = 1;
10270 vinfos[2].foffset = j2;
10271 vinfos[2].indices[0] = _ij2[0];
10272 vinfos[2].indices[1] = _ij2[1];
10273 vinfos[2].maxsolutions = _nj2;
10274 vinfos[3].jointtype = 1;
10275 vinfos[3].foffset = j3;
10276 vinfos[3].indices[0] = _ij3[0];
10277 vinfos[3].indices[1] = _ij3[1];
10278 vinfos[3].maxsolutions = _nj3;
10279 vinfos[4].jointtype = 1;
10280 vinfos[4].foffset = j4;
10281 vinfos[4].indices[0] = _ij4[0];
10282 vinfos[4].indices[1] = _ij4[1];
10283 vinfos[4].maxsolutions = _nj4;
10284 vinfos[5].jointtype = 1;
10285 vinfos[5].foffset = j5;
10286 vinfos[5].indices[0] = _ij5[0];
10287 vinfos[5].indices[1] = _ij5[1];
10288 vinfos[5].maxsolutions = _nj5;
10289 std::vector<int> vfree(0);
10290 solutions.AddSolution(vinfos,vfree);
10291 }
10292 }
10293 }
10294 
10295 }
10296 
10297 }
10298 
10299 } else
10300 {
10301 {
10302 IkReal j3array[1], cj3array[1], sj3array[1];
10303 bool j3valid[1]={false};
10304 _nj3 = 1;
10305 IkReal x762=((1.0)*new_r11);
10306 CheckValue<IkReal> x763 = IKatan2WithCheck(IkReal((((new_r10*sj5))+((new_r01*sj5)))),IkReal((((new_r00*sj5))+(((-1.0)*sj5*x762)))),IKFAST_ATAN2_MAGTHRESH);
10307 if(!x763.valid){
10308 continue;
10309 }
10310 CheckValue<IkReal> x764=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x762))+(((-1.0)*new_r00*new_r01)))),-1);
10311 if(!x764.valid){
10312 continue;
10313 }
10314 j3array[0]=((-1.5707963267949)+(x763.value)+(((1.5707963267949)*(x764.value))));
10315 sj3array[0]=IKsin(j3array[0]);
10316 cj3array[0]=IKcos(j3array[0]);
10317 if( j3array[0] > IKPI )
10318 {
10319  j3array[0]-=IK2PI;
10320 }
10321 else if( j3array[0] < -IKPI )
10322 { j3array[0]+=IK2PI;
10323 }
10324 j3valid[0] = true;
10325 for(int ij3 = 0; ij3 < 1; ++ij3)
10326 {
10327 if( !j3valid[ij3] )
10328 {
10329  continue;
10330 }
10331 _ij3[0] = ij3; _ij3[1] = -1;
10332 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10333 {
10334 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10335 {
10336  j3valid[iij3]=false; _ij3[1] = iij3; break;
10337 }
10338 }
10339 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10340 {
10341 IkReal evalcond[8];
10342 IkReal x765=IKcos(j3);
10343 IkReal x766=IKsin(j3);
10344 IkReal x767=(sj5*x766);
10345 IkReal x768=(cj5*x766);
10346 IkReal x769=(sj5*x765);
10347 IkReal x770=((1.0)*x765);
10348 IkReal x771=(cj5*x770);
10349 evalcond[0]=(sj5+((new_r11*x766))+((new_r01*x765)));
10350 evalcond[1]=(x768+x769+new_r01);
10351 evalcond[2]=(sj5+((new_r00*x766))+(((-1.0)*new_r10*x770)));
10352 evalcond[3]=(cj5+((new_r01*x766))+(((-1.0)*new_r11*x770)));
10353 evalcond[4]=(x767+(((-1.0)*x771))+new_r00);
10354 evalcond[5]=(x767+(((-1.0)*x771))+new_r11);
10355 evalcond[6]=(((new_r10*x766))+((new_r00*x765))+(((-1.0)*cj5)));
10356 evalcond[7]=((((-1.0)*x769))+(((-1.0)*x768))+new_r10);
10357 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10358 {
10359 continue;
10360 }
10361 }
10362 
10363 {
10364 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10365 vinfos[0].jointtype = 1;
10366 vinfos[0].foffset = j0;
10367 vinfos[0].indices[0] = _ij0[0];
10368 vinfos[0].indices[1] = _ij0[1];
10369 vinfos[0].maxsolutions = _nj0;
10370 vinfos[1].jointtype = 1;
10371 vinfos[1].foffset = j1;
10372 vinfos[1].indices[0] = _ij1[0];
10373 vinfos[1].indices[1] = _ij1[1];
10374 vinfos[1].maxsolutions = _nj1;
10375 vinfos[2].jointtype = 1;
10376 vinfos[2].foffset = j2;
10377 vinfos[2].indices[0] = _ij2[0];
10378 vinfos[2].indices[1] = _ij2[1];
10379 vinfos[2].maxsolutions = _nj2;
10380 vinfos[3].jointtype = 1;
10381 vinfos[3].foffset = j3;
10382 vinfos[3].indices[0] = _ij3[0];
10383 vinfos[3].indices[1] = _ij3[1];
10384 vinfos[3].maxsolutions = _nj3;
10385 vinfos[4].jointtype = 1;
10386 vinfos[4].foffset = j4;
10387 vinfos[4].indices[0] = _ij4[0];
10388 vinfos[4].indices[1] = _ij4[1];
10389 vinfos[4].maxsolutions = _nj4;
10390 vinfos[5].jointtype = 1;
10391 vinfos[5].foffset = j5;
10392 vinfos[5].indices[0] = _ij5[0];
10393 vinfos[5].indices[1] = _ij5[1];
10394 vinfos[5].maxsolutions = _nj5;
10395 std::vector<int> vfree(0);
10396 solutions.AddSolution(vinfos,vfree);
10397 }
10398 }
10399 }
10400 
10401 }
10402 
10403 }
10404 
10405 }
10406 } while(0);
10407 if( bgotonextstatement )
10408 {
10409 bool bgotonextstatement = true;
10410 do
10411 {
10412 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10413 evalcond[1]=new_r02;
10414 evalcond[2]=new_r12;
10415 evalcond[3]=new_r21;
10416 evalcond[4]=new_r20;
10417 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 )
10418 {
10419 bgotonextstatement=false;
10420 {
10421 IkReal j3eval[3];
10422 sj4=0;
10423 cj4=-1.0;
10424 j4=3.14159265358979;
10425 IkReal x772=((1.0)*new_r10);
10426 IkReal x773=((((-1.0)*new_r11*x772))+(((-1.0)*new_r00*new_r01)));
10427 j3eval[0]=x773;
10428 j3eval[1]=((IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(IKabs((((cj5*new_r01))+(((-1.0)*cj5*x772))))));
10429 j3eval[2]=IKsign(x773);
10430 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10431 {
10432 {
10433 IkReal j3eval[3];
10434 sj4=0;
10435 cj4=-1.0;
10436 j4=3.14159265358979;
10437 IkReal x774=((1.0)*new_r10);
10438 IkReal x775=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x774)));
10439 j3eval[0]=x775;
10440 j3eval[1]=IKsign(x775);
10441 j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((((-1.0)*new_r01*x774))+(cj5*cj5)))));
10442 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10443 {
10444 {
10445 IkReal j3eval[3];
10446 sj4=0;
10447 cj4=-1.0;
10448 j4=3.14159265358979;
10449 IkReal x776=((1.0)*new_r00);
10450 IkReal x777=((((-1.0)*sj5*x776))+((cj5*new_r10)));
10451 j3eval[0]=x777;
10452 j3eval[1]=((IKabs((((cj5*sj5))+(((-1.0)*new_r10*x776)))))+(IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00)))));
10453 j3eval[2]=IKsign(x777);
10454 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10455 {
10456 {
10457 IkReal evalcond[1];
10458 bool bgotonextstatement = true;
10459 do
10460 {
10461 IkReal x778=((-1.0)*new_r00);
10462 IkReal x780 = ((new_r10*new_r10)+(new_r00*new_r00));
10463 if(IKabs(x780)==0){
10464 continue;
10465 }
10466 IkReal x779=pow(x780,-0.5);
10467 CheckValue<IkReal> x781 = IKatan2WithCheck(IkReal(new_r10),IkReal(x778),IKFAST_ATAN2_MAGTHRESH);
10468 if(!x781.valid){
10469 continue;
10470 }
10471 IkReal gconst6=((-1.0)*(x781.value));
10472 IkReal gconst7=((-1.0)*new_r10*x779);
10473 IkReal gconst8=(x778*x779);
10474 CheckValue<IkReal> x782 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10475 if(!x782.valid){
10476 continue;
10477 }
10478 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j5+(x782.value))))), 6.28318530717959)));
10479 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10480 {
10481 bgotonextstatement=false;
10482 {
10483 IkReal j3eval[3];
10484 IkReal x783=((-1.0)*new_r00);
10485 CheckValue<IkReal> x786 = IKatan2WithCheck(IkReal(new_r10),IkReal(x783),IKFAST_ATAN2_MAGTHRESH);
10486 if(!x786.valid){
10487 continue;
10488 }
10489 IkReal x784=((-1.0)*(x786.value));
10490 IkReal x785=x779;
10491 sj4=0;
10492 cj4=-1.0;
10493 j4=3.14159265358979;
10494 sj5=gconst7;
10495 cj5=gconst8;
10496 j5=x784;
10497 IkReal gconst6=x784;
10498 IkReal gconst7=((-1.0)*new_r10*x785);
10499 IkReal gconst8=(x783*x785);
10500 IkReal x787=new_r00*new_r00;
10501 IkReal x788=((1.0)*new_r11);
10502 IkReal x789=((1.0)*new_r00*new_r01);
10503 IkReal x790=((((-1.0)*x789))+(((-1.0)*new_r10*x788)));
10504 IkReal x791=x779;
10505 IkReal x792=(new_r00*x791);
10506 j3eval[0]=x790;
10507 j3eval[1]=((IKabs(((((-1.0)*x788*x792))+(((-1.0)*x787*x791)))))+(IKabs((((new_r10*x792))+(((-1.0)*x789*x791))))));
10508 j3eval[2]=IKsign(x790);
10509 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10510 {
10511 {
10512 IkReal j3eval[1];
10513 IkReal x793=((-1.0)*new_r00);
10514 CheckValue<IkReal> x796 = IKatan2WithCheck(IkReal(new_r10),IkReal(x793),IKFAST_ATAN2_MAGTHRESH);
10515 if(!x796.valid){
10516 continue;
10517 }
10518 IkReal x794=((-1.0)*(x796.value));
10519 IkReal x795=x779;
10520 sj4=0;
10521 cj4=-1.0;
10522 j4=3.14159265358979;
10523 sj5=gconst7;
10524 cj5=gconst8;
10525 j5=x794;
10526 IkReal gconst6=x794;
10527 IkReal gconst7=((-1.0)*new_r10*x795);
10528 IkReal gconst8=(x793*x795);
10529 IkReal x797=new_r10*new_r10;
10530 IkReal x798=new_r00*new_r00;
10531 CheckValue<IkReal> x801=IKPowWithIntegerCheck((x797+x798),-1);
10532 if(!x801.valid){
10533 continue;
10534 }
10535 IkReal x799=x801.value;
10536 IkReal x800=(new_r00*x799);
10537 j3eval[0]=((IKabs((((new_r01*x797*x800))+((new_r10*x800))+((new_r01*x800*(new_r00*new_r00))))))+(IKabs((((x798*x799))+(((-1.0)*new_r01*new_r10))))));
10538 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10539 {
10540 {
10541 IkReal j3eval[1];
10542 IkReal x802=((-1.0)*new_r00);
10543 CheckValue<IkReal> x805 = IKatan2WithCheck(IkReal(new_r10),IkReal(x802),IKFAST_ATAN2_MAGTHRESH);
10544 if(!x805.valid){
10545 continue;
10546 }
10547 IkReal x803=((-1.0)*(x805.value));
10548 IkReal x804=x779;
10549 sj4=0;
10550 cj4=-1.0;
10551 j4=3.14159265358979;
10552 sj5=gconst7;
10553 cj5=gconst8;
10554 j5=x803;
10555 IkReal gconst6=x803;
10556 IkReal gconst7=((-1.0)*new_r10*x804);
10557 IkReal gconst8=(x802*x804);
10558 IkReal x806=new_r00*new_r00;
10559 IkReal x807=new_r10*new_r10;
10560 CheckValue<IkReal> x811=IKPowWithIntegerCheck((x807+x806),-1);
10561 if(!x811.valid){
10562 continue;
10563 }
10564 IkReal x808=x811.value;
10565 IkReal x809=(new_r10*x808);
10566 IkReal x810=((1.0)*x808);
10567 j3eval[0]=((IKabs((((new_r00*x809*(new_r10*new_r10)))+((new_r00*x809))+((x809*(new_r00*new_r00*new_r00))))))+(IKabs((((x806*x808))+(((-1.0)*x810*(x807*x807)))+(((-1.0)*x806*x807*x810))))));
10568 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10569 {
10570 {
10571 IkReal evalcond[2];
10572 bool bgotonextstatement = true;
10573 do
10574 {
10575 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10576 if( IKabs(evalcond[0]) < 0.0000050000000000 )
10577 {
10578 bgotonextstatement=false;
10579 {
10580 IkReal j3eval[1];
10581 CheckValue<IkReal> x813 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10582 if(!x813.valid){
10583 continue;
10584 }
10585 IkReal x812=((-1.0)*(x813.value));
10586 sj4=0;
10587 cj4=-1.0;
10588 j4=3.14159265358979;
10589 sj5=gconst7;
10590 cj5=gconst8;
10591 j5=x812;
10592 new_r11=0;
10593 new_r00=0;
10594 IkReal gconst6=x812;
10595 IkReal x814 = new_r10*new_r10;
10596 if(IKabs(x814)==0){
10597 continue;
10598 }
10599 IkReal gconst7=((-1.0)*new_r10*(pow(x814,-0.5)));
10600 IkReal gconst8=0;
10601 j3eval[0]=new_r10;
10602 if( IKabs(j3eval[0]) < 0.0000010000000000 )
10603 {
10604 {
10605 IkReal j3array[2], cj3array[2], sj3array[2];
10606 bool j3valid[2]={false};
10607 _nj3 = 2;
10608 CheckValue<IkReal> x815=IKPowWithIntegerCheck(gconst7,-1);
10609 if(!x815.valid){
10610 continue;
10611 }
10612 cj3array[0]=(new_r01*(x815.value));
10613 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10614 {
10615  j3valid[0] = j3valid[1] = true;
10616  j3array[0] = IKacos(cj3array[0]);
10617  sj3array[0] = IKsin(j3array[0]);
10618  cj3array[1] = cj3array[0];
10619  j3array[1] = -j3array[0];
10620  sj3array[1] = -sj3array[0];
10621 }
10622 else if( isnan(cj3array[0]) )
10623 {
10624  // probably any value will work
10625  j3valid[0] = true;
10626  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10627 }
10628 for(int ij3 = 0; ij3 < 2; ++ij3)
10629 {
10630 if( !j3valid[ij3] )
10631 {
10632  continue;
10633 }
10634 _ij3[0] = ij3; _ij3[1] = -1;
10635 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10636 {
10637 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10638 {
10639  j3valid[iij3]=false; _ij3[1] = iij3; break;
10640 }
10641 }
10642 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10643 {
10644 IkReal evalcond[6];
10645 IkReal x816=IKsin(j3);
10646 IkReal x817=IKcos(j3);
10647 IkReal x818=((1.0)*gconst7);
10648 evalcond[0]=(new_r01*x816);
10649 evalcond[1]=(new_r10*x816);
10650 evalcond[2]=(gconst7*x816);
10651 evalcond[3]=((((-1.0)*new_r10*x817))+gconst7);
10652 evalcond[4]=((((-1.0)*x817*x818))+new_r10);
10653 evalcond[5]=(((new_r01*x817))+(((-1.0)*x818)));
10654 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10655 {
10656 continue;
10657 }
10658 }
10659 
10660 {
10661 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10662 vinfos[0].jointtype = 1;
10663 vinfos[0].foffset = j0;
10664 vinfos[0].indices[0] = _ij0[0];
10665 vinfos[0].indices[1] = _ij0[1];
10666 vinfos[0].maxsolutions = _nj0;
10667 vinfos[1].jointtype = 1;
10668 vinfos[1].foffset = j1;
10669 vinfos[1].indices[0] = _ij1[0];
10670 vinfos[1].indices[1] = _ij1[1];
10671 vinfos[1].maxsolutions = _nj1;
10672 vinfos[2].jointtype = 1;
10673 vinfos[2].foffset = j2;
10674 vinfos[2].indices[0] = _ij2[0];
10675 vinfos[2].indices[1] = _ij2[1];
10676 vinfos[2].maxsolutions = _nj2;
10677 vinfos[3].jointtype = 1;
10678 vinfos[3].foffset = j3;
10679 vinfos[3].indices[0] = _ij3[0];
10680 vinfos[3].indices[1] = _ij3[1];
10681 vinfos[3].maxsolutions = _nj3;
10682 vinfos[4].jointtype = 1;
10683 vinfos[4].foffset = j4;
10684 vinfos[4].indices[0] = _ij4[0];
10685 vinfos[4].indices[1] = _ij4[1];
10686 vinfos[4].maxsolutions = _nj4;
10687 vinfos[5].jointtype = 1;
10688 vinfos[5].foffset = j5;
10689 vinfos[5].indices[0] = _ij5[0];
10690 vinfos[5].indices[1] = _ij5[1];
10691 vinfos[5].maxsolutions = _nj5;
10692 std::vector<int> vfree(0);
10693 solutions.AddSolution(vinfos,vfree);
10694 }
10695 }
10696 }
10697 
10698 } else
10699 {
10700 {
10701 IkReal j3array[2], cj3array[2], sj3array[2];
10702 bool j3valid[2]={false};
10703 _nj3 = 2;
10704 CheckValue<IkReal> x819=IKPowWithIntegerCheck(new_r10,-1);
10705 if(!x819.valid){
10706 continue;
10707 }
10708 cj3array[0]=(gconst7*(x819.value));
10709 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10710 {
10711  j3valid[0] = j3valid[1] = true;
10712  j3array[0] = IKacos(cj3array[0]);
10713  sj3array[0] = IKsin(j3array[0]);
10714  cj3array[1] = cj3array[0];
10715  j3array[1] = -j3array[0];
10716  sj3array[1] = -sj3array[0];
10717 }
10718 else if( isnan(cj3array[0]) )
10719 {
10720  // probably any value will work
10721  j3valid[0] = true;
10722  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10723 }
10724 for(int ij3 = 0; ij3 < 2; ++ij3)
10725 {
10726 if( !j3valid[ij3] )
10727 {
10728  continue;
10729 }
10730 _ij3[0] = ij3; _ij3[1] = -1;
10731 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10732 {
10733 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10734 {
10735  j3valid[iij3]=false; _ij3[1] = iij3; break;
10736 }
10737 }
10738 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10739 {
10740 IkReal evalcond[6];
10741 IkReal x820=IKsin(j3);
10742 IkReal x821=IKcos(j3);
10743 IkReal x822=((1.0)*gconst7);
10744 IkReal x823=(x821*x822);
10745 evalcond[0]=(new_r01*x820);
10746 evalcond[1]=(new_r10*x820);
10747 evalcond[2]=(gconst7*x820);
10748 evalcond[3]=((((-1.0)*x823))+new_r01);
10749 evalcond[4]=((((-1.0)*x823))+new_r10);
10750 evalcond[5]=(((new_r01*x821))+(((-1.0)*x822)));
10751 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10752 {
10753 continue;
10754 }
10755 }
10756 
10757 {
10758 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10759 vinfos[0].jointtype = 1;
10760 vinfos[0].foffset = j0;
10761 vinfos[0].indices[0] = _ij0[0];
10762 vinfos[0].indices[1] = _ij0[1];
10763 vinfos[0].maxsolutions = _nj0;
10764 vinfos[1].jointtype = 1;
10765 vinfos[1].foffset = j1;
10766 vinfos[1].indices[0] = _ij1[0];
10767 vinfos[1].indices[1] = _ij1[1];
10768 vinfos[1].maxsolutions = _nj1;
10769 vinfos[2].jointtype = 1;
10770 vinfos[2].foffset = j2;
10771 vinfos[2].indices[0] = _ij2[0];
10772 vinfos[2].indices[1] = _ij2[1];
10773 vinfos[2].maxsolutions = _nj2;
10774 vinfos[3].jointtype = 1;
10775 vinfos[3].foffset = j3;
10776 vinfos[3].indices[0] = _ij3[0];
10777 vinfos[3].indices[1] = _ij3[1];
10778 vinfos[3].maxsolutions = _nj3;
10779 vinfos[4].jointtype = 1;
10780 vinfos[4].foffset = j4;
10781 vinfos[4].indices[0] = _ij4[0];
10782 vinfos[4].indices[1] = _ij4[1];
10783 vinfos[4].maxsolutions = _nj4;
10784 vinfos[5].jointtype = 1;
10785 vinfos[5].foffset = j5;
10786 vinfos[5].indices[0] = _ij5[0];
10787 vinfos[5].indices[1] = _ij5[1];
10788 vinfos[5].maxsolutions = _nj5;
10789 std::vector<int> vfree(0);
10790 solutions.AddSolution(vinfos,vfree);
10791 }
10792 }
10793 }
10794 
10795 }
10796 
10797 }
10798 
10799 }
10800 } while(0);
10801 if( bgotonextstatement )
10802 {
10803 bool bgotonextstatement = true;
10804 do
10805 {
10806 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
10807 evalcond[1]=gconst8;
10808 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
10809 {
10810 bgotonextstatement=false;
10811 {
10812 IkReal j3eval[3];
10813 IkReal x824=((-1.0)*new_r00);
10814 CheckValue<IkReal> x826 = IKatan2WithCheck(IkReal(new_r10),IkReal(x824),IKFAST_ATAN2_MAGTHRESH);
10815 if(!x826.valid){
10816 continue;
10817 }
10818 IkReal x825=((-1.0)*(x826.value));
10819 sj4=0;
10820 cj4=-1.0;
10821 j4=3.14159265358979;
10822 sj5=gconst7;
10823 cj5=gconst8;
10824 j5=x825;
10825 new_r11=0;
10826 new_r01=0;
10827 new_r22=0;
10828 new_r20=0;
10829 IkReal gconst6=x825;
10830 IkReal gconst7=((-1.0)*new_r10);
10831 IkReal gconst8=x824;
10832 j3eval[0]=1.0;
10833 j3eval[1]=1.0;
10834 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
10835 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10836 {
10837 {
10838 IkReal j3eval[3];
10839 IkReal x827=((-1.0)*new_r00);
10840 CheckValue<IkReal> x829 = IKatan2WithCheck(IkReal(new_r10),IkReal(x827),IKFAST_ATAN2_MAGTHRESH);
10841 if(!x829.valid){
10842 continue;
10843 }
10844 IkReal x828=((-1.0)*(x829.value));
10845 sj4=0;
10846 cj4=-1.0;
10847 j4=3.14159265358979;
10848 sj5=gconst7;
10849 cj5=gconst8;
10850 j5=x828;
10851 new_r11=0;
10852 new_r01=0;
10853 new_r22=0;
10854 new_r20=0;
10855 IkReal gconst6=x828;
10856 IkReal gconst7=((-1.0)*new_r10);
10857 IkReal gconst8=x827;
10858 j3eval[0]=-1.0;
10859 j3eval[1]=((IKabs(((-1.0)+(new_r10*new_r10))))+(IKabs((new_r00*new_r10))));
10860 j3eval[2]=-1.0;
10861 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10862 {
10863 {
10864 IkReal j3eval[3];
10865 IkReal x830=((-1.0)*new_r00);
10866 CheckValue<IkReal> x832 = IKatan2WithCheck(IkReal(new_r10),IkReal(x830),IKFAST_ATAN2_MAGTHRESH);
10867 if(!x832.valid){
10868 continue;
10869 }
10870 IkReal x831=((-1.0)*(x832.value));
10871 sj4=0;
10872 cj4=-1.0;
10873 j4=3.14159265358979;
10874 sj5=gconst7;
10875 cj5=gconst8;
10876 j5=x831;
10877 new_r11=0;
10878 new_r01=0;
10879 new_r22=0;
10880 new_r20=0;
10881 IkReal gconst6=x831;
10882 IkReal gconst7=((-1.0)*new_r10);
10883 IkReal gconst8=x830;
10884 j3eval[0]=1.0;
10885 j3eval[1]=((((0.5)*(IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
10886 j3eval[2]=1.0;
10887 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10888 {
10889 continue; // 3 cases reached
10890 
10891 } else
10892 {
10893 {
10894 IkReal j3array[1], cj3array[1], sj3array[1];
10895 bool j3valid[1]={false};
10896 _nj3 = 1;
10897 CheckValue<IkReal> x833=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
10898 if(!x833.valid){
10899 continue;
10900 }
10901 CheckValue<IkReal> x834 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
10902 if(!x834.valid){
10903 continue;
10904 }
10905 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x833.value)))+(x834.value));
10906 sj3array[0]=IKsin(j3array[0]);
10907 cj3array[0]=IKcos(j3array[0]);
10908 if( j3array[0] > IKPI )
10909 {
10910  j3array[0]-=IK2PI;
10911 }
10912 else if( j3array[0] < -IKPI )
10913 { j3array[0]+=IK2PI;
10914 }
10915 j3valid[0] = true;
10916 for(int ij3 = 0; ij3 < 1; ++ij3)
10917 {
10918 if( !j3valid[ij3] )
10919 {
10920  continue;
10921 }
10922 _ij3[0] = ij3; _ij3[1] = -1;
10923 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10924 {
10925 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10926 {
10927  j3valid[iij3]=false; _ij3[1] = iij3; break;
10928 }
10929 }
10930 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10931 {
10932 IkReal evalcond[6];
10933 IkReal x835=IKsin(j3);
10934 IkReal x836=IKcos(j3);
10935 IkReal x837=(gconst8*x835);
10936 IkReal x838=(gconst7*x835);
10937 IkReal x839=(gconst8*x836);
10938 IkReal x840=((1.0)*x836);
10939 IkReal x841=(gconst7*x840);
10940 evalcond[0]=((((-1.0)*x841))+x837);
10941 evalcond[1]=(gconst8+((new_r00*x836))+((new_r10*x835)));
10942 evalcond[2]=(new_r00+x838+x839);
10943 evalcond[3]=((((-1.0)*new_r10*x840))+gconst7+((new_r00*x835)));
10944 evalcond[4]=((((-1.0)*x838))+(((-1.0)*x839)));
10945 evalcond[5]=((((-1.0)*x841))+new_r10+x837);
10946 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10947 {
10948 continue;
10949 }
10950 }
10951 
10952 {
10953 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10954 vinfos[0].jointtype = 1;
10955 vinfos[0].foffset = j0;
10956 vinfos[0].indices[0] = _ij0[0];
10957 vinfos[0].indices[1] = _ij0[1];
10958 vinfos[0].maxsolutions = _nj0;
10959 vinfos[1].jointtype = 1;
10960 vinfos[1].foffset = j1;
10961 vinfos[1].indices[0] = _ij1[0];
10962 vinfos[1].indices[1] = _ij1[1];
10963 vinfos[1].maxsolutions = _nj1;
10964 vinfos[2].jointtype = 1;
10965 vinfos[2].foffset = j2;
10966 vinfos[2].indices[0] = _ij2[0];
10967 vinfos[2].indices[1] = _ij2[1];
10968 vinfos[2].maxsolutions = _nj2;
10969 vinfos[3].jointtype = 1;
10970 vinfos[3].foffset = j3;
10971 vinfos[3].indices[0] = _ij3[0];
10972 vinfos[3].indices[1] = _ij3[1];
10973 vinfos[3].maxsolutions = _nj3;
10974 vinfos[4].jointtype = 1;
10975 vinfos[4].foffset = j4;
10976 vinfos[4].indices[0] = _ij4[0];
10977 vinfos[4].indices[1] = _ij4[1];
10978 vinfos[4].maxsolutions = _nj4;
10979 vinfos[5].jointtype = 1;
10980 vinfos[5].foffset = j5;
10981 vinfos[5].indices[0] = _ij5[0];
10982 vinfos[5].indices[1] = _ij5[1];
10983 vinfos[5].maxsolutions = _nj5;
10984 std::vector<int> vfree(0);
10985 solutions.AddSolution(vinfos,vfree);
10986 }
10987 }
10988 }
10989 
10990 }
10991 
10992 }
10993 
10994 } else
10995 {
10996 {
10997 IkReal j3array[1], cj3array[1], sj3array[1];
10998 bool j3valid[1]={false};
10999 _nj3 = 1;
11000 CheckValue<IkReal> x842 = IKatan2WithCheck(IkReal((gconst7*new_r00)),IkReal((gconst8*new_r00)),IKFAST_ATAN2_MAGTHRESH);
11001 if(!x842.valid){
11002 continue;
11003 }
11004 CheckValue<IkReal> x843=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
11005 if(!x843.valid){
11006 continue;
11007 }
11008 j3array[0]=((-1.5707963267949)+(x842.value)+(((1.5707963267949)*(x843.value))));
11009 sj3array[0]=IKsin(j3array[0]);
11010 cj3array[0]=IKcos(j3array[0]);
11011 if( j3array[0] > IKPI )
11012 {
11013  j3array[0]-=IK2PI;
11014 }
11015 else if( j3array[0] < -IKPI )
11016 { j3array[0]+=IK2PI;
11017 }
11018 j3valid[0] = true;
11019 for(int ij3 = 0; ij3 < 1; ++ij3)
11020 {
11021 if( !j3valid[ij3] )
11022 {
11023  continue;
11024 }
11025 _ij3[0] = ij3; _ij3[1] = -1;
11026 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11027 {
11028 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11029 {
11030  j3valid[iij3]=false; _ij3[1] = iij3; break;
11031 }
11032 }
11033 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11034 {
11035 IkReal evalcond[6];
11036 IkReal x844=IKsin(j3);
11037 IkReal x845=IKcos(j3);
11038 IkReal x846=(gconst8*x844);
11039 IkReal x847=(gconst7*x844);
11040 IkReal x848=(gconst8*x845);
11041 IkReal x849=((1.0)*x845);
11042 IkReal x850=(gconst7*x849);
11043 evalcond[0]=((((-1.0)*x850))+x846);
11044 evalcond[1]=(((new_r10*x844))+gconst8+((new_r00*x845)));
11045 evalcond[2]=(new_r00+x847+x848);
11046 evalcond[3]=((((-1.0)*new_r10*x849))+gconst7+((new_r00*x844)));
11047 evalcond[4]=((((-1.0)*x848))+(((-1.0)*x847)));
11048 evalcond[5]=((((-1.0)*x850))+new_r10+x846);
11049 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11050 {
11051 continue;
11052 }
11053 }
11054 
11055 {
11056 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11057 vinfos[0].jointtype = 1;
11058 vinfos[0].foffset = j0;
11059 vinfos[0].indices[0] = _ij0[0];
11060 vinfos[0].indices[1] = _ij0[1];
11061 vinfos[0].maxsolutions = _nj0;
11062 vinfos[1].jointtype = 1;
11063 vinfos[1].foffset = j1;
11064 vinfos[1].indices[0] = _ij1[0];
11065 vinfos[1].indices[1] = _ij1[1];
11066 vinfos[1].maxsolutions = _nj1;
11067 vinfos[2].jointtype = 1;
11068 vinfos[2].foffset = j2;
11069 vinfos[2].indices[0] = _ij2[0];
11070 vinfos[2].indices[1] = _ij2[1];
11071 vinfos[2].maxsolutions = _nj2;
11072 vinfos[3].jointtype = 1;
11073 vinfos[3].foffset = j3;
11074 vinfos[3].indices[0] = _ij3[0];
11075 vinfos[3].indices[1] = _ij3[1];
11076 vinfos[3].maxsolutions = _nj3;
11077 vinfos[4].jointtype = 1;
11078 vinfos[4].foffset = j4;
11079 vinfos[4].indices[0] = _ij4[0];
11080 vinfos[4].indices[1] = _ij4[1];
11081 vinfos[4].maxsolutions = _nj4;
11082 vinfos[5].jointtype = 1;
11083 vinfos[5].foffset = j5;
11084 vinfos[5].indices[0] = _ij5[0];
11085 vinfos[5].indices[1] = _ij5[1];
11086 vinfos[5].maxsolutions = _nj5;
11087 std::vector<int> vfree(0);
11088 solutions.AddSolution(vinfos,vfree);
11089 }
11090 }
11091 }
11092 
11093 }
11094 
11095 }
11096 
11097 } else
11098 {
11099 {
11100 IkReal j3array[1], cj3array[1], sj3array[1];
11101 bool j3valid[1]={false};
11102 _nj3 = 1;
11103 CheckValue<IkReal> x851 = IKatan2WithCheck(IkReal((gconst7*gconst8)),IkReal(gconst8*gconst8),IKFAST_ATAN2_MAGTHRESH);
11104 if(!x851.valid){
11105 continue;
11106 }
11107 CheckValue<IkReal> x852=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11108 if(!x852.valid){
11109 continue;
11110 }
11111 j3array[0]=((-1.5707963267949)+(x851.value)+(((1.5707963267949)*(x852.value))));
11112 sj3array[0]=IKsin(j3array[0]);
11113 cj3array[0]=IKcos(j3array[0]);
11114 if( j3array[0] > IKPI )
11115 {
11116  j3array[0]-=IK2PI;
11117 }
11118 else if( j3array[0] < -IKPI )
11119 { j3array[0]+=IK2PI;
11120 }
11121 j3valid[0] = true;
11122 for(int ij3 = 0; ij3 < 1; ++ij3)
11123 {
11124 if( !j3valid[ij3] )
11125 {
11126  continue;
11127 }
11128 _ij3[0] = ij3; _ij3[1] = -1;
11129 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11130 {
11131 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11132 {
11133  j3valid[iij3]=false; _ij3[1] = iij3; break;
11134 }
11135 }
11136 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11137 {
11138 IkReal evalcond[6];
11139 IkReal x853=IKsin(j3);
11140 IkReal x854=IKcos(j3);
11141 IkReal x855=(gconst8*x853);
11142 IkReal x856=(gconst7*x853);
11143 IkReal x857=(gconst8*x854);
11144 IkReal x858=((1.0)*x854);
11145 IkReal x859=(gconst7*x858);
11146 evalcond[0]=((((-1.0)*x859))+x855);
11147 evalcond[1]=(gconst8+((new_r10*x853))+((new_r00*x854)));
11148 evalcond[2]=(new_r00+x856+x857);
11149 evalcond[3]=(gconst7+(((-1.0)*new_r10*x858))+((new_r00*x853)));
11150 evalcond[4]=((((-1.0)*x856))+(((-1.0)*x857)));
11151 evalcond[5]=((((-1.0)*x859))+new_r10+x855);
11152 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11153 {
11154 continue;
11155 }
11156 }
11157 
11158 {
11159 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11160 vinfos[0].jointtype = 1;
11161 vinfos[0].foffset = j0;
11162 vinfos[0].indices[0] = _ij0[0];
11163 vinfos[0].indices[1] = _ij0[1];
11164 vinfos[0].maxsolutions = _nj0;
11165 vinfos[1].jointtype = 1;
11166 vinfos[1].foffset = j1;
11167 vinfos[1].indices[0] = _ij1[0];
11168 vinfos[1].indices[1] = _ij1[1];
11169 vinfos[1].maxsolutions = _nj1;
11170 vinfos[2].jointtype = 1;
11171 vinfos[2].foffset = j2;
11172 vinfos[2].indices[0] = _ij2[0];
11173 vinfos[2].indices[1] = _ij2[1];
11174 vinfos[2].maxsolutions = _nj2;
11175 vinfos[3].jointtype = 1;
11176 vinfos[3].foffset = j3;
11177 vinfos[3].indices[0] = _ij3[0];
11178 vinfos[3].indices[1] = _ij3[1];
11179 vinfos[3].maxsolutions = _nj3;
11180 vinfos[4].jointtype = 1;
11181 vinfos[4].foffset = j4;
11182 vinfos[4].indices[0] = _ij4[0];
11183 vinfos[4].indices[1] = _ij4[1];
11184 vinfos[4].maxsolutions = _nj4;
11185 vinfos[5].jointtype = 1;
11186 vinfos[5].foffset = j5;
11187 vinfos[5].indices[0] = _ij5[0];
11188 vinfos[5].indices[1] = _ij5[1];
11189 vinfos[5].maxsolutions = _nj5;
11190 std::vector<int> vfree(0);
11191 solutions.AddSolution(vinfos,vfree);
11192 }
11193 }
11194 }
11195 
11196 }
11197 
11198 }
11199 
11200 }
11201 } while(0);
11202 if( bgotonextstatement )
11203 {
11204 bool bgotonextstatement = true;
11205 do
11206 {
11207 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
11208 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11209 {
11210 bgotonextstatement=false;
11211 {
11212 IkReal j3eval[1];
11213 IkReal x860=((-1.0)*new_r00);
11214 CheckValue<IkReal> x862 = IKatan2WithCheck(IkReal(0),IkReal(x860),IKFAST_ATAN2_MAGTHRESH);
11215 if(!x862.valid){
11216 continue;
11217 }
11218 IkReal x861=((-1.0)*(x862.value));
11219 sj4=0;
11220 cj4=-1.0;
11221 j4=3.14159265358979;
11222 sj5=gconst7;
11223 cj5=gconst8;
11224 j5=x861;
11225 new_r01=0;
11226 new_r10=0;
11227 IkReal gconst6=x861;
11228 IkReal gconst7=0;
11229 IkReal x863 = new_r00*new_r00;
11230 if(IKabs(x863)==0){
11231 continue;
11232 }
11233 IkReal gconst8=(x860*(pow(x863,-0.5)));
11234 j3eval[0]=new_r11;
11235 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11236 {
11237 {
11238 IkReal j3array[2], cj3array[2], sj3array[2];
11239 bool j3valid[2]={false};
11240 _nj3 = 2;
11241 CheckValue<IkReal> x864=IKPowWithIntegerCheck(gconst8,-1);
11242 if(!x864.valid){
11243 continue;
11244 }
11245 cj3array[0]=(new_r11*(x864.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 x865=IKsin(j3);
11279 IkReal x866=IKcos(j3);
11280 evalcond[0]=(new_r00*x865);
11281 evalcond[1]=(new_r11*x865);
11282 evalcond[2]=(gconst8*x865);
11283 evalcond[3]=(((new_r00*x866))+gconst8);
11284 evalcond[4]=(((gconst8*x866))+new_r00);
11285 evalcond[5]=(gconst8+(((-1.0)*new_r11*x866)));
11286 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11287 {
11288 continue;
11289 }
11290 }
11291 
11292 {
11293 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11294 vinfos[0].jointtype = 1;
11295 vinfos[0].foffset = j0;
11296 vinfos[0].indices[0] = _ij0[0];
11297 vinfos[0].indices[1] = _ij0[1];
11298 vinfos[0].maxsolutions = _nj0;
11299 vinfos[1].jointtype = 1;
11300 vinfos[1].foffset = j1;
11301 vinfos[1].indices[0] = _ij1[0];
11302 vinfos[1].indices[1] = _ij1[1];
11303 vinfos[1].maxsolutions = _nj1;
11304 vinfos[2].jointtype = 1;
11305 vinfos[2].foffset = j2;
11306 vinfos[2].indices[0] = _ij2[0];
11307 vinfos[2].indices[1] = _ij2[1];
11308 vinfos[2].maxsolutions = _nj2;
11309 vinfos[3].jointtype = 1;
11310 vinfos[3].foffset = j3;
11311 vinfos[3].indices[0] = _ij3[0];
11312 vinfos[3].indices[1] = _ij3[1];
11313 vinfos[3].maxsolutions = _nj3;
11314 vinfos[4].jointtype = 1;
11315 vinfos[4].foffset = j4;
11316 vinfos[4].indices[0] = _ij4[0];
11317 vinfos[4].indices[1] = _ij4[1];
11318 vinfos[4].maxsolutions = _nj4;
11319 vinfos[5].jointtype = 1;
11320 vinfos[5].foffset = j5;
11321 vinfos[5].indices[0] = _ij5[0];
11322 vinfos[5].indices[1] = _ij5[1];
11323 vinfos[5].maxsolutions = _nj5;
11324 std::vector<int> vfree(0);
11325 solutions.AddSolution(vinfos,vfree);
11326 }
11327 }
11328 }
11329 
11330 } else
11331 {
11332 {
11333 IkReal j3array[2], cj3array[2], sj3array[2];
11334 bool j3valid[2]={false};
11335 _nj3 = 2;
11336 CheckValue<IkReal> x867=IKPowWithIntegerCheck(new_r11,-1);
11337 if(!x867.valid){
11338 continue;
11339 }
11340 cj3array[0]=(gconst8*(x867.value));
11341 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11342 {
11343  j3valid[0] = j3valid[1] = true;
11344  j3array[0] = IKacos(cj3array[0]);
11345  sj3array[0] = IKsin(j3array[0]);
11346  cj3array[1] = cj3array[0];
11347  j3array[1] = -j3array[0];
11348  sj3array[1] = -sj3array[0];
11349 }
11350 else if( isnan(cj3array[0]) )
11351 {
11352  // probably any value will work
11353  j3valid[0] = true;
11354  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11355 }
11356 for(int ij3 = 0; ij3 < 2; ++ij3)
11357 {
11358 if( !j3valid[ij3] )
11359 {
11360  continue;
11361 }
11362 _ij3[0] = ij3; _ij3[1] = -1;
11363 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11364 {
11365 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11366 {
11367  j3valid[iij3]=false; _ij3[1] = iij3; break;
11368 }
11369 }
11370 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11371 {
11372 IkReal evalcond[6];
11373 IkReal x868=IKsin(j3);
11374 IkReal x869=IKcos(j3);
11375 IkReal x870=(gconst8*x869);
11376 evalcond[0]=(new_r00*x868);
11377 evalcond[1]=(new_r11*x868);
11378 evalcond[2]=(gconst8*x868);
11379 evalcond[3]=(((new_r00*x869))+gconst8);
11380 evalcond[4]=(new_r00+x870);
11381 evalcond[5]=((((-1.0)*x870))+new_r11);
11382 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11383 {
11384 continue;
11385 }
11386 }
11387 
11388 {
11389 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11390 vinfos[0].jointtype = 1;
11391 vinfos[0].foffset = j0;
11392 vinfos[0].indices[0] = _ij0[0];
11393 vinfos[0].indices[1] = _ij0[1];
11394 vinfos[0].maxsolutions = _nj0;
11395 vinfos[1].jointtype = 1;
11396 vinfos[1].foffset = j1;
11397 vinfos[1].indices[0] = _ij1[0];
11398 vinfos[1].indices[1] = _ij1[1];
11399 vinfos[1].maxsolutions = _nj1;
11400 vinfos[2].jointtype = 1;
11401 vinfos[2].foffset = j2;
11402 vinfos[2].indices[0] = _ij2[0];
11403 vinfos[2].indices[1] = _ij2[1];
11404 vinfos[2].maxsolutions = _nj2;
11405 vinfos[3].jointtype = 1;
11406 vinfos[3].foffset = j3;
11407 vinfos[3].indices[0] = _ij3[0];
11408 vinfos[3].indices[1] = _ij3[1];
11409 vinfos[3].maxsolutions = _nj3;
11410 vinfos[4].jointtype = 1;
11411 vinfos[4].foffset = j4;
11412 vinfos[4].indices[0] = _ij4[0];
11413 vinfos[4].indices[1] = _ij4[1];
11414 vinfos[4].maxsolutions = _nj4;
11415 vinfos[5].jointtype = 1;
11416 vinfos[5].foffset = j5;
11417 vinfos[5].indices[0] = _ij5[0];
11418 vinfos[5].indices[1] = _ij5[1];
11419 vinfos[5].maxsolutions = _nj5;
11420 std::vector<int> vfree(0);
11421 solutions.AddSolution(vinfos,vfree);
11422 }
11423 }
11424 }
11425 
11426 }
11427 
11428 }
11429 
11430 }
11431 } while(0);
11432 if( bgotonextstatement )
11433 {
11434 bool bgotonextstatement = true;
11435 do
11436 {
11437 evalcond[0]=IKabs(new_r00);
11438 if( IKabs(evalcond[0]) < 0.0000050000000000 )
11439 {
11440 bgotonextstatement=false;
11441 {
11442 IkReal j3eval[1];
11443 CheckValue<IkReal> x872 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11444 if(!x872.valid){
11445 continue;
11446 }
11447 IkReal x871=((-1.0)*(x872.value));
11448 sj4=0;
11449 cj4=-1.0;
11450 j4=3.14159265358979;
11451 sj5=gconst7;
11452 cj5=gconst8;
11453 j5=x871;
11454 new_r00=0;
11455 IkReal gconst6=x871;
11456 IkReal x873 = new_r10*new_r10;
11457 if(IKabs(x873)==0){
11458 continue;
11459 }
11460 IkReal gconst7=((-1.0)*new_r10*(pow(x873,-0.5)));
11461 IkReal gconst8=0;
11462 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11463 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11464 {
11465 {
11466 IkReal j3eval[1];
11467 CheckValue<IkReal> x875 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11468 if(!x875.valid){
11469 continue;
11470 }
11471 IkReal x874=((-1.0)*(x875.value));
11472 sj4=0;
11473 cj4=-1.0;
11474 j4=3.14159265358979;
11475 sj5=gconst7;
11476 cj5=gconst8;
11477 j5=x874;
11478 new_r00=0;
11479 IkReal gconst6=x874;
11480 IkReal x876 = new_r10*new_r10;
11481 if(IKabs(x876)==0){
11482 continue;
11483 }
11484 IkReal gconst7=((-1.0)*new_r10*(pow(x876,-0.5)));
11485 IkReal gconst8=0;
11486 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11487 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11488 {
11489 {
11490 IkReal j3eval[1];
11491 CheckValue<IkReal> x878 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11492 if(!x878.valid){
11493 continue;
11494 }
11495 IkReal x877=((-1.0)*(x878.value));
11496 sj4=0;
11497 cj4=-1.0;
11498 j4=3.14159265358979;
11499 sj5=gconst7;
11500 cj5=gconst8;
11501 j5=x877;
11502 new_r00=0;
11503 IkReal gconst6=x877;
11504 IkReal x879 = new_r10*new_r10;
11505 if(IKabs(x879)==0){
11506 continue;
11507 }
11508 IkReal gconst7=((-1.0)*new_r10*(pow(x879,-0.5)));
11509 IkReal gconst8=0;
11510 j3eval[0]=new_r10;
11511 if( IKabs(j3eval[0]) < 0.0000010000000000 )
11512 {
11513 continue; // 3 cases reached
11514 
11515 } else
11516 {
11517 {
11518 IkReal j3array[1], cj3array[1], sj3array[1];
11519 bool j3valid[1]={false};
11520 _nj3 = 1;
11521 CheckValue<IkReal> x880=IKPowWithIntegerCheck(gconst7,-1);
11522 if(!x880.valid){
11523 continue;
11524 }
11525 CheckValue<IkReal> x881=IKPowWithIntegerCheck(new_r10,-1);
11526 if(!x881.valid){
11527 continue;
11528 }
11529 if( IKabs((new_r11*(x880.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst7*(x881.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x880.value)))+IKsqr((gconst7*(x881.value)))-1) <= IKFAST_SINCOS_THRESH )
11530  continue;
11531 j3array[0]=IKatan2((new_r11*(x880.value)), (gconst7*(x881.value)));
11532 sj3array[0]=IKsin(j3array[0]);
11533 cj3array[0]=IKcos(j3array[0]);
11534 if( j3array[0] > IKPI )
11535 {
11536  j3array[0]-=IK2PI;
11537 }
11538 else if( j3array[0] < -IKPI )
11539 { j3array[0]+=IK2PI;
11540 }
11541 j3valid[0] = true;
11542 for(int ij3 = 0; ij3 < 1; ++ij3)
11543 {
11544 if( !j3valid[ij3] )
11545 {
11546  continue;
11547 }
11548 _ij3[0] = ij3; _ij3[1] = -1;
11549 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11550 {
11551 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11552 {
11553  j3valid[iij3]=false; _ij3[1] = iij3; break;
11554 }
11555 }
11556 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11557 {
11558 IkReal evalcond[8];
11559 IkReal x882=IKsin(j3);
11560 IkReal x883=IKcos(j3);
11561 IkReal x884=((1.0)*gconst7);
11562 IkReal x885=((1.0)*x883);
11563 IkReal x886=(x883*x884);
11564 evalcond[0]=(new_r10*x882);
11565 evalcond[1]=(gconst7*x882);
11566 evalcond[2]=((((-1.0)*new_r10*x885))+gconst7);
11567 evalcond[3]=((((-1.0)*x886))+new_r01);
11568 evalcond[4]=((((-1.0)*x882*x884))+new_r11);
11569 evalcond[5]=((((-1.0)*x886))+new_r10);
11570 evalcond[6]=(((new_r01*x882))+(((-1.0)*new_r11*x885)));
11571 evalcond[7]=(((new_r01*x883))+((new_r11*x882))+(((-1.0)*x884)));
11572 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11573 {
11574 continue;
11575 }
11576 }
11577 
11578 {
11579 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11580 vinfos[0].jointtype = 1;
11581 vinfos[0].foffset = j0;
11582 vinfos[0].indices[0] = _ij0[0];
11583 vinfos[0].indices[1] = _ij0[1];
11584 vinfos[0].maxsolutions = _nj0;
11585 vinfos[1].jointtype = 1;
11586 vinfos[1].foffset = j1;
11587 vinfos[1].indices[0] = _ij1[0];
11588 vinfos[1].indices[1] = _ij1[1];
11589 vinfos[1].maxsolutions = _nj1;
11590 vinfos[2].jointtype = 1;
11591 vinfos[2].foffset = j2;
11592 vinfos[2].indices[0] = _ij2[0];
11593 vinfos[2].indices[1] = _ij2[1];
11594 vinfos[2].maxsolutions = _nj2;
11595 vinfos[3].jointtype = 1;
11596 vinfos[3].foffset = j3;
11597 vinfos[3].indices[0] = _ij3[0];
11598 vinfos[3].indices[1] = _ij3[1];
11599 vinfos[3].maxsolutions = _nj3;
11600 vinfos[4].jointtype = 1;
11601 vinfos[4].foffset = j4;
11602 vinfos[4].indices[0] = _ij4[0];
11603 vinfos[4].indices[1] = _ij4[1];
11604 vinfos[4].maxsolutions = _nj4;
11605 vinfos[5].jointtype = 1;
11606 vinfos[5].foffset = j5;
11607 vinfos[5].indices[0] = _ij5[0];
11608 vinfos[5].indices[1] = _ij5[1];
11609 vinfos[5].maxsolutions = _nj5;
11610 std::vector<int> vfree(0);
11611 solutions.AddSolution(vinfos,vfree);
11612 }
11613 }
11614 }
11615 
11616 }
11617 
11618 }
11619 
11620 } else
11621 {
11622 {
11623 IkReal j3array[1], cj3array[1], sj3array[1];
11624 bool j3valid[1]={false};
11625 _nj3 = 1;
11626 CheckValue<IkReal> x887=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11627 if(!x887.valid){
11628 continue;
11629 }
11630 CheckValue<IkReal> x888 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11631 if(!x888.valid){
11632 continue;
11633 }
11634 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x887.value)))+(x888.value));
11635 sj3array[0]=IKsin(j3array[0]);
11636 cj3array[0]=IKcos(j3array[0]);
11637 if( j3array[0] > IKPI )
11638 {
11639  j3array[0]-=IK2PI;
11640 }
11641 else if( j3array[0] < -IKPI )
11642 { j3array[0]+=IK2PI;
11643 }
11644 j3valid[0] = true;
11645 for(int ij3 = 0; ij3 < 1; ++ij3)
11646 {
11647 if( !j3valid[ij3] )
11648 {
11649  continue;
11650 }
11651 _ij3[0] = ij3; _ij3[1] = -1;
11652 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11653 {
11654 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11655 {
11656  j3valid[iij3]=false; _ij3[1] = iij3; break;
11657 }
11658 }
11659 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11660 {
11661 IkReal evalcond[8];
11662 IkReal x889=IKsin(j3);
11663 IkReal x890=IKcos(j3);
11664 IkReal x891=((1.0)*gconst7);
11665 IkReal x892=((1.0)*x890);
11666 IkReal x893=(x890*x891);
11667 evalcond[0]=(new_r10*x889);
11668 evalcond[1]=(gconst7*x889);
11669 evalcond[2]=(gconst7+(((-1.0)*new_r10*x892)));
11670 evalcond[3]=((((-1.0)*x893))+new_r01);
11671 evalcond[4]=((((-1.0)*x889*x891))+new_r11);
11672 evalcond[5]=((((-1.0)*x893))+new_r10);
11673 evalcond[6]=(((new_r01*x889))+(((-1.0)*new_r11*x892)));
11674 evalcond[7]=(((new_r11*x889))+((new_r01*x890))+(((-1.0)*x891)));
11675 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11676 {
11677 continue;
11678 }
11679 }
11680 
11681 {
11682 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11683 vinfos[0].jointtype = 1;
11684 vinfos[0].foffset = j0;
11685 vinfos[0].indices[0] = _ij0[0];
11686 vinfos[0].indices[1] = _ij0[1];
11687 vinfos[0].maxsolutions = _nj0;
11688 vinfos[1].jointtype = 1;
11689 vinfos[1].foffset = j1;
11690 vinfos[1].indices[0] = _ij1[0];
11691 vinfos[1].indices[1] = _ij1[1];
11692 vinfos[1].maxsolutions = _nj1;
11693 vinfos[2].jointtype = 1;
11694 vinfos[2].foffset = j2;
11695 vinfos[2].indices[0] = _ij2[0];
11696 vinfos[2].indices[1] = _ij2[1];
11697 vinfos[2].maxsolutions = _nj2;
11698 vinfos[3].jointtype = 1;
11699 vinfos[3].foffset = j3;
11700 vinfos[3].indices[0] = _ij3[0];
11701 vinfos[3].indices[1] = _ij3[1];
11702 vinfos[3].maxsolutions = _nj3;
11703 vinfos[4].jointtype = 1;
11704 vinfos[4].foffset = j4;
11705 vinfos[4].indices[0] = _ij4[0];
11706 vinfos[4].indices[1] = _ij4[1];
11707 vinfos[4].maxsolutions = _nj4;
11708 vinfos[5].jointtype = 1;
11709 vinfos[5].foffset = j5;
11710 vinfos[5].indices[0] = _ij5[0];
11711 vinfos[5].indices[1] = _ij5[1];
11712 vinfos[5].maxsolutions = _nj5;
11713 std::vector<int> vfree(0);
11714 solutions.AddSolution(vinfos,vfree);
11715 }
11716 }
11717 }
11718 
11719 }
11720 
11721 }
11722 
11723 } else
11724 {
11725 {
11726 IkReal j3array[1], cj3array[1], sj3array[1];
11727 bool j3valid[1]={false};
11728 _nj3 = 1;
11729 CheckValue<IkReal> x894=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11730 if(!x894.valid){
11731 continue;
11732 }
11733 CheckValue<IkReal> x895 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
11734 if(!x895.valid){
11735 continue;
11736 }
11737 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x894.value)))+(x895.value));
11738 sj3array[0]=IKsin(j3array[0]);
11739 cj3array[0]=IKcos(j3array[0]);
11740 if( j3array[0] > IKPI )
11741 {
11742  j3array[0]-=IK2PI;
11743 }
11744 else if( j3array[0] < -IKPI )
11745 { j3array[0]+=IK2PI;
11746 }
11747 j3valid[0] = true;
11748 for(int ij3 = 0; ij3 < 1; ++ij3)
11749 {
11750 if( !j3valid[ij3] )
11751 {
11752  continue;
11753 }
11754 _ij3[0] = ij3; _ij3[1] = -1;
11755 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11756 {
11757 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11758 {
11759  j3valid[iij3]=false; _ij3[1] = iij3; break;
11760 }
11761 }
11762 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11763 {
11764 IkReal evalcond[8];
11765 IkReal x896=IKsin(j3);
11766 IkReal x897=IKcos(j3);
11767 IkReal x898=((1.0)*gconst7);
11768 IkReal x899=((1.0)*x897);
11769 IkReal x900=(x897*x898);
11770 evalcond[0]=(new_r10*x896);
11771 evalcond[1]=(gconst7*x896);
11772 evalcond[2]=(gconst7+(((-1.0)*new_r10*x899)));
11773 evalcond[3]=((((-1.0)*x900))+new_r01);
11774 evalcond[4]=((((-1.0)*x896*x898))+new_r11);
11775 evalcond[5]=((((-1.0)*x900))+new_r10);
11776 evalcond[6]=(((new_r01*x896))+(((-1.0)*new_r11*x899)));
11777 evalcond[7]=(((new_r11*x896))+((new_r01*x897))+(((-1.0)*x898)));
11778 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11779 {
11780 continue;
11781 }
11782 }
11783 
11784 {
11785 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11786 vinfos[0].jointtype = 1;
11787 vinfos[0].foffset = j0;
11788 vinfos[0].indices[0] = _ij0[0];
11789 vinfos[0].indices[1] = _ij0[1];
11790 vinfos[0].maxsolutions = _nj0;
11791 vinfos[1].jointtype = 1;
11792 vinfos[1].foffset = j1;
11793 vinfos[1].indices[0] = _ij1[0];
11794 vinfos[1].indices[1] = _ij1[1];
11795 vinfos[1].maxsolutions = _nj1;
11796 vinfos[2].jointtype = 1;
11797 vinfos[2].foffset = j2;
11798 vinfos[2].indices[0] = _ij2[0];
11799 vinfos[2].indices[1] = _ij2[1];
11800 vinfos[2].maxsolutions = _nj2;
11801 vinfos[3].jointtype = 1;
11802 vinfos[3].foffset = j3;
11803 vinfos[3].indices[0] = _ij3[0];
11804 vinfos[3].indices[1] = _ij3[1];
11805 vinfos[3].maxsolutions = _nj3;
11806 vinfos[4].jointtype = 1;
11807 vinfos[4].foffset = j4;
11808 vinfos[4].indices[0] = _ij4[0];
11809 vinfos[4].indices[1] = _ij4[1];
11810 vinfos[4].maxsolutions = _nj4;
11811 vinfos[5].jointtype = 1;
11812 vinfos[5].foffset = j5;
11813 vinfos[5].indices[0] = _ij5[0];
11814 vinfos[5].indices[1] = _ij5[1];
11815 vinfos[5].maxsolutions = _nj5;
11816 std::vector<int> vfree(0);
11817 solutions.AddSolution(vinfos,vfree);
11818 }
11819 }
11820 }
11821 
11822 }
11823 
11824 }
11825 
11826 }
11827 } while(0);
11828 if( bgotonextstatement )
11829 {
11830 bool bgotonextstatement = true;
11831 do
11832 {
11833 if( 1 )
11834 {
11835 bgotonextstatement=false;
11836 continue; // branch miss [j3]
11837 
11838 }
11839 } while(0);
11840 if( bgotonextstatement )
11841 {
11842 }
11843 }
11844 }
11845 }
11846 }
11847 }
11848 
11849 } else
11850 {
11851 {
11852 IkReal j3array[1], cj3array[1], sj3array[1];
11853 bool j3valid[1]={false};
11854 _nj3 = 1;
11855 CheckValue<IkReal> x901=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11856 if(!x901.valid){
11857 continue;
11858 }
11859 CheckValue<IkReal> x902 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
11860 if(!x902.valid){
11861 continue;
11862 }
11863 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x901.value)))+(x902.value));
11864 sj3array[0]=IKsin(j3array[0]);
11865 cj3array[0]=IKcos(j3array[0]);
11866 if( j3array[0] > IKPI )
11867 {
11868  j3array[0]-=IK2PI;
11869 }
11870 else if( j3array[0] < -IKPI )
11871 { j3array[0]+=IK2PI;
11872 }
11873 j3valid[0] = true;
11874 for(int ij3 = 0; ij3 < 1; ++ij3)
11875 {
11876 if( !j3valid[ij3] )
11877 {
11878  continue;
11879 }
11880 _ij3[0] = ij3; _ij3[1] = -1;
11881 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11882 {
11883 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11884 {
11885  j3valid[iij3]=false; _ij3[1] = iij3; break;
11886 }
11887 }
11888 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11889 {
11890 IkReal evalcond[8];
11891 IkReal x903=IKcos(j3);
11892 IkReal x904=IKsin(j3);
11893 IkReal x905=(gconst8*x904);
11894 IkReal x906=(gconst7*x904);
11895 IkReal x907=((1.0)*x903);
11896 IkReal x908=(gconst7*x907);
11897 evalcond[0]=(gconst8+((new_r10*x904))+((new_r00*x903)));
11898 evalcond[1]=(new_r00+((gconst8*x903))+x906);
11899 evalcond[2]=(gconst7+((new_r00*x904))+(((-1.0)*new_r10*x907)));
11900 evalcond[3]=((((-1.0)*new_r11*x907))+gconst8+((new_r01*x904)));
11901 evalcond[4]=((((-1.0)*x908))+new_r01+x905);
11902 evalcond[5]=((((-1.0)*x908))+new_r10+x905);
11903 evalcond[6]=((((-1.0)*gconst7))+((new_r11*x904))+((new_r01*x903)));
11904 evalcond[7]=((((-1.0)*gconst8*x907))+(((-1.0)*x906))+new_r11);
11905 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11906 {
11907 continue;
11908 }
11909 }
11910 
11911 {
11912 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11913 vinfos[0].jointtype = 1;
11914 vinfos[0].foffset = j0;
11915 vinfos[0].indices[0] = _ij0[0];
11916 vinfos[0].indices[1] = _ij0[1];
11917 vinfos[0].maxsolutions = _nj0;
11918 vinfos[1].jointtype = 1;
11919 vinfos[1].foffset = j1;
11920 vinfos[1].indices[0] = _ij1[0];
11921 vinfos[1].indices[1] = _ij1[1];
11922 vinfos[1].maxsolutions = _nj1;
11923 vinfos[2].jointtype = 1;
11924 vinfos[2].foffset = j2;
11925 vinfos[2].indices[0] = _ij2[0];
11926 vinfos[2].indices[1] = _ij2[1];
11927 vinfos[2].maxsolutions = _nj2;
11928 vinfos[3].jointtype = 1;
11929 vinfos[3].foffset = j3;
11930 vinfos[3].indices[0] = _ij3[0];
11931 vinfos[3].indices[1] = _ij3[1];
11932 vinfos[3].maxsolutions = _nj3;
11933 vinfos[4].jointtype = 1;
11934 vinfos[4].foffset = j4;
11935 vinfos[4].indices[0] = _ij4[0];
11936 vinfos[4].indices[1] = _ij4[1];
11937 vinfos[4].maxsolutions = _nj4;
11938 vinfos[5].jointtype = 1;
11939 vinfos[5].foffset = j5;
11940 vinfos[5].indices[0] = _ij5[0];
11941 vinfos[5].indices[1] = _ij5[1];
11942 vinfos[5].maxsolutions = _nj5;
11943 std::vector<int> vfree(0);
11944 solutions.AddSolution(vinfos,vfree);
11945 }
11946 }
11947 }
11948 
11949 }
11950 
11951 }
11952 
11953 } else
11954 {
11955 {
11956 IkReal j3array[1], cj3array[1], sj3array[1];
11957 bool j3valid[1]={false};
11958 _nj3 = 1;
11959 IkReal x909=((1.0)*new_r10);
11960 CheckValue<IkReal> x910=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*x909))+(((-1.0)*gconst8*new_r00)))),-1);
11961 if(!x910.valid){
11962 continue;
11963 }
11964 CheckValue<IkReal> x911 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*new_r01*x909)))),IKFAST_ATAN2_MAGTHRESH);
11965 if(!x911.valid){
11966 continue;
11967 }
11968 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x910.value)))+(x911.value));
11969 sj3array[0]=IKsin(j3array[0]);
11970 cj3array[0]=IKcos(j3array[0]);
11971 if( j3array[0] > IKPI )
11972 {
11973  j3array[0]-=IK2PI;
11974 }
11975 else if( j3array[0] < -IKPI )
11976 { j3array[0]+=IK2PI;
11977 }
11978 j3valid[0] = true;
11979 for(int ij3 = 0; ij3 < 1; ++ij3)
11980 {
11981 if( !j3valid[ij3] )
11982 {
11983  continue;
11984 }
11985 _ij3[0] = ij3; _ij3[1] = -1;
11986 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11987 {
11988 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11989 {
11990  j3valid[iij3]=false; _ij3[1] = iij3; break;
11991 }
11992 }
11993 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11994 {
11995 IkReal evalcond[8];
11996 IkReal x912=IKcos(j3);
11997 IkReal x913=IKsin(j3);
11998 IkReal x914=(gconst8*x913);
11999 IkReal x915=(gconst7*x913);
12000 IkReal x916=((1.0)*x912);
12001 IkReal x917=(gconst7*x916);
12002 evalcond[0]=(gconst8+((new_r10*x913))+((new_r00*x912)));
12003 evalcond[1]=(((gconst8*x912))+new_r00+x915);
12004 evalcond[2]=(gconst7+((new_r00*x913))+(((-1.0)*new_r10*x916)));
12005 evalcond[3]=(gconst8+((new_r01*x913))+(((-1.0)*new_r11*x916)));
12006 evalcond[4]=((((-1.0)*x917))+new_r01+x914);
12007 evalcond[5]=((((-1.0)*x917))+new_r10+x914);
12008 evalcond[6]=(((new_r11*x913))+(((-1.0)*gconst7))+((new_r01*x912)));
12009 evalcond[7]=((((-1.0)*gconst8*x916))+(((-1.0)*x915))+new_r11);
12010 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12011 {
12012 continue;
12013 }
12014 }
12015 
12016 {
12017 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12018 vinfos[0].jointtype = 1;
12019 vinfos[0].foffset = j0;
12020 vinfos[0].indices[0] = _ij0[0];
12021 vinfos[0].indices[1] = _ij0[1];
12022 vinfos[0].maxsolutions = _nj0;
12023 vinfos[1].jointtype = 1;
12024 vinfos[1].foffset = j1;
12025 vinfos[1].indices[0] = _ij1[0];
12026 vinfos[1].indices[1] = _ij1[1];
12027 vinfos[1].maxsolutions = _nj1;
12028 vinfos[2].jointtype = 1;
12029 vinfos[2].foffset = j2;
12030 vinfos[2].indices[0] = _ij2[0];
12031 vinfos[2].indices[1] = _ij2[1];
12032 vinfos[2].maxsolutions = _nj2;
12033 vinfos[3].jointtype = 1;
12034 vinfos[3].foffset = j3;
12035 vinfos[3].indices[0] = _ij3[0];
12036 vinfos[3].indices[1] = _ij3[1];
12037 vinfos[3].maxsolutions = _nj3;
12038 vinfos[4].jointtype = 1;
12039 vinfos[4].foffset = j4;
12040 vinfos[4].indices[0] = _ij4[0];
12041 vinfos[4].indices[1] = _ij4[1];
12042 vinfos[4].maxsolutions = _nj4;
12043 vinfos[5].jointtype = 1;
12044 vinfos[5].foffset = j5;
12045 vinfos[5].indices[0] = _ij5[0];
12046 vinfos[5].indices[1] = _ij5[1];
12047 vinfos[5].maxsolutions = _nj5;
12048 std::vector<int> vfree(0);
12049 solutions.AddSolution(vinfos,vfree);
12050 }
12051 }
12052 }
12053 
12054 }
12055 
12056 }
12057 
12058 } else
12059 {
12060 {
12061 IkReal j3array[1], cj3array[1], sj3array[1];
12062 bool j3valid[1]={false};
12063 _nj3 = 1;
12064 IkReal x918=((1.0)*new_r10);
12065 CheckValue<IkReal> x919 = IKatan2WithCheck(IkReal((((gconst8*new_r11))+((gconst8*new_r00)))),IkReal((((gconst8*new_r01))+(((-1.0)*gconst8*x918)))),IKFAST_ATAN2_MAGTHRESH);
12066 if(!x919.valid){
12067 continue;
12068 }
12069 CheckValue<IkReal> x920=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x918)))),-1);
12070 if(!x920.valid){
12071 continue;
12072 }
12073 j3array[0]=((-1.5707963267949)+(x919.value)+(((1.5707963267949)*(x920.value))));
12074 sj3array[0]=IKsin(j3array[0]);
12075 cj3array[0]=IKcos(j3array[0]);
12076 if( j3array[0] > IKPI )
12077 {
12078  j3array[0]-=IK2PI;
12079 }
12080 else if( j3array[0] < -IKPI )
12081 { j3array[0]+=IK2PI;
12082 }
12083 j3valid[0] = true;
12084 for(int ij3 = 0; ij3 < 1; ++ij3)
12085 {
12086 if( !j3valid[ij3] )
12087 {
12088  continue;
12089 }
12090 _ij3[0] = ij3; _ij3[1] = -1;
12091 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12092 {
12093 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12094 {
12095  j3valid[iij3]=false; _ij3[1] = iij3; break;
12096 }
12097 }
12098 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12099 {
12100 IkReal evalcond[8];
12101 IkReal x921=IKcos(j3);
12102 IkReal x922=IKsin(j3);
12103 IkReal x923=(gconst8*x922);
12104 IkReal x924=(gconst7*x922);
12105 IkReal x925=((1.0)*x921);
12106 IkReal x926=(gconst7*x925);
12107 evalcond[0]=(gconst8+((new_r00*x921))+((new_r10*x922)));
12108 evalcond[1]=(((gconst8*x921))+new_r00+x924);
12109 evalcond[2]=(gconst7+(((-1.0)*new_r10*x925))+((new_r00*x922)));
12110 evalcond[3]=((((-1.0)*new_r11*x925))+gconst8+((new_r01*x922)));
12111 evalcond[4]=((((-1.0)*x926))+new_r01+x923);
12112 evalcond[5]=((((-1.0)*x926))+new_r10+x923);
12113 evalcond[6]=((((-1.0)*gconst7))+((new_r01*x921))+((new_r11*x922)));
12114 evalcond[7]=((((-1.0)*gconst8*x925))+(((-1.0)*x924))+new_r11);
12115 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12116 {
12117 continue;
12118 }
12119 }
12120 
12121 {
12122 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12123 vinfos[0].jointtype = 1;
12124 vinfos[0].foffset = j0;
12125 vinfos[0].indices[0] = _ij0[0];
12126 vinfos[0].indices[1] = _ij0[1];
12127 vinfos[0].maxsolutions = _nj0;
12128 vinfos[1].jointtype = 1;
12129 vinfos[1].foffset = j1;
12130 vinfos[1].indices[0] = _ij1[0];
12131 vinfos[1].indices[1] = _ij1[1];
12132 vinfos[1].maxsolutions = _nj1;
12133 vinfos[2].jointtype = 1;
12134 vinfos[2].foffset = j2;
12135 vinfos[2].indices[0] = _ij2[0];
12136 vinfos[2].indices[1] = _ij2[1];
12137 vinfos[2].maxsolutions = _nj2;
12138 vinfos[3].jointtype = 1;
12139 vinfos[3].foffset = j3;
12140 vinfos[3].indices[0] = _ij3[0];
12141 vinfos[3].indices[1] = _ij3[1];
12142 vinfos[3].maxsolutions = _nj3;
12143 vinfos[4].jointtype = 1;
12144 vinfos[4].foffset = j4;
12145 vinfos[4].indices[0] = _ij4[0];
12146 vinfos[4].indices[1] = _ij4[1];
12147 vinfos[4].maxsolutions = _nj4;
12148 vinfos[5].jointtype = 1;
12149 vinfos[5].foffset = j5;
12150 vinfos[5].indices[0] = _ij5[0];
12151 vinfos[5].indices[1] = _ij5[1];
12152 vinfos[5].maxsolutions = _nj5;
12153 std::vector<int> vfree(0);
12154 solutions.AddSolution(vinfos,vfree);
12155 }
12156 }
12157 }
12158 
12159 }
12160 
12161 }
12162 
12163 }
12164 } while(0);
12165 if( bgotonextstatement )
12166 {
12167 bool bgotonextstatement = true;
12168 do
12169 {
12170 IkReal x929 = ((new_r10*new_r10)+(new_r00*new_r00));
12171 if(IKabs(x929)==0){
12172 continue;
12173 }
12174 IkReal x927=pow(x929,-0.5);
12175 IkReal x928=((1.0)*x927);
12176 CheckValue<IkReal> x930 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12177 if(!x930.valid){
12178 continue;
12179 }
12180 IkReal gconst9=((3.14159265358979)+(((-1.0)*(x930.value))));
12181 IkReal gconst10=(new_r10*x928);
12182 IkReal gconst11=(new_r00*x928);
12183 CheckValue<IkReal> x931 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12184 if(!x931.valid){
12185 continue;
12186 }
12187 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5+(x931.value))))), 6.28318530717959)));
12188 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12189 {
12190 bgotonextstatement=false;
12191 {
12192 IkReal j3eval[3];
12193 CheckValue<IkReal> x935 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12194 if(!x935.valid){
12195 continue;
12196 }
12197 IkReal x932=((1.0)*(x935.value));
12198 IkReal x933=x927;
12199 IkReal x934=((1.0)*x933);
12200 sj4=0;
12201 cj4=-1.0;
12202 j4=3.14159265358979;
12203 sj5=gconst10;
12204 cj5=gconst11;
12205 j5=((3.14159265)+(((-1.0)*x932)));
12206 IkReal gconst9=((3.14159265358979)+(((-1.0)*x932)));
12207 IkReal gconst10=(new_r10*x934);
12208 IkReal gconst11=(new_r00*x934);
12209 IkReal x936=new_r00*new_r00;
12210 IkReal x937=((1.0)*new_r00);
12211 IkReal x938=((((-1.0)*new_r01*x937))+(((-1.0)*new_r10*new_r11)));
12212 IkReal x939=x927;
12213 IkReal x940=(new_r00*x939);
12214 j3eval[0]=x938;
12215 j3eval[1]=((IKabs((((new_r01*x940))+(((-1.0)*new_r10*x937*x939)))))+(IKabs((((x936*x939))+((new_r11*x940))))));
12216 j3eval[2]=IKsign(x938);
12217 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12218 {
12219 {
12220 IkReal j3eval[1];
12221 CheckValue<IkReal> x944 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12222 if(!x944.valid){
12223 continue;
12224 }
12225 IkReal x941=((1.0)*(x944.value));
12226 IkReal x942=x927;
12227 IkReal x943=((1.0)*x942);
12228 sj4=0;
12229 cj4=-1.0;
12230 j4=3.14159265358979;
12231 sj5=gconst10;
12232 cj5=gconst11;
12233 j5=((3.14159265)+(((-1.0)*x941)));
12234 IkReal gconst9=((3.14159265358979)+(((-1.0)*x941)));
12235 IkReal gconst10=(new_r10*x943);
12236 IkReal gconst11=(new_r00*x943);
12237 IkReal x945=new_r10*new_r10;
12238 IkReal x946=new_r00*new_r00;
12239 IkReal x947=((1.0)*new_r01);
12240 CheckValue<IkReal> x951=IKPowWithIntegerCheck((x945+x946),-1);
12241 if(!x951.valid){
12242 continue;
12243 }
12244 IkReal x948=x951.value;
12245 IkReal x949=(new_r10*x948);
12246 IkReal x950=(new_r01*x948);
12247 j3eval[0]=((IKabs((((new_r00*x949))+((new_r00*x945*x950))+((x950*(new_r00*new_r00*new_r00))))))+(IKabs((((x946*x948))+(((-1.0)*x947*x949*(new_r10*new_r10)))+(((-1.0)*x946*x947*x949))))));
12248 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12249 {
12250 {
12251 IkReal j3eval[1];
12252 CheckValue<IkReal> x955 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12253 if(!x955.valid){
12254 continue;
12255 }
12256 IkReal x952=((1.0)*(x955.value));
12257 IkReal x953=x927;
12258 IkReal x954=((1.0)*x953);
12259 sj4=0;
12260 cj4=-1.0;
12261 j4=3.14159265358979;
12262 sj5=gconst10;
12263 cj5=gconst11;
12264 j5=((3.14159265)+(((-1.0)*x952)));
12265 IkReal gconst9=((3.14159265358979)+(((-1.0)*x952)));
12266 IkReal gconst10=(new_r10*x954);
12267 IkReal gconst11=(new_r00*x954);
12268 IkReal x956=new_r00*new_r00;
12269 IkReal x957=new_r10*new_r10;
12270 CheckValue<IkReal> x961=IKPowWithIntegerCheck((x957+x956),-1);
12271 if(!x961.valid){
12272 continue;
12273 }
12274 IkReal x958=x961.value;
12275 IkReal x959=(new_r10*x958);
12276 IkReal x960=((1.0)*x958);
12277 j3eval[0]=((IKabs(((((-1.0)*x956*x957*x960))+((x956*x958))+(((-1.0)*x960*(x957*x957))))))+(IKabs((((new_r00*x959))+((x959*(new_r00*new_r00*new_r00)))+((new_r00*x959*(new_r10*new_r10)))))));
12278 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12279 {
12280 {
12281 IkReal evalcond[2];
12282 bool bgotonextstatement = true;
12283 do
12284 {
12285 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12286 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12287 {
12288 bgotonextstatement=false;
12289 {
12290 IkReal j3eval[1];
12291 CheckValue<IkReal> x963 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12292 if(!x963.valid){
12293 continue;
12294 }
12295 IkReal x962=((1.0)*(x963.value));
12296 sj4=0;
12297 cj4=-1.0;
12298 j4=3.14159265358979;
12299 sj5=gconst10;
12300 cj5=gconst11;
12301 j5=((3.14159265)+(((-1.0)*x962)));
12302 new_r11=0;
12303 new_r00=0;
12304 IkReal gconst9=((3.14159265358979)+(((-1.0)*x962)));
12305 IkReal x964 = new_r10*new_r10;
12306 if(IKabs(x964)==0){
12307 continue;
12308 }
12309 IkReal gconst10=((1.0)*new_r10*(pow(x964,-0.5)));
12310 IkReal gconst11=0;
12311 j3eval[0]=new_r10;
12312 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12313 {
12314 {
12315 IkReal j3array[2], cj3array[2], sj3array[2];
12316 bool j3valid[2]={false};
12317 _nj3 = 2;
12318 CheckValue<IkReal> x965=IKPowWithIntegerCheck(gconst10,-1);
12319 if(!x965.valid){
12320 continue;
12321 }
12322 cj3array[0]=(new_r01*(x965.value));
12323 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12324 {
12325  j3valid[0] = j3valid[1] = true;
12326  j3array[0] = IKacos(cj3array[0]);
12327  sj3array[0] = IKsin(j3array[0]);
12328  cj3array[1] = cj3array[0];
12329  j3array[1] = -j3array[0];
12330  sj3array[1] = -sj3array[0];
12331 }
12332 else if( isnan(cj3array[0]) )
12333 {
12334  // probably any value will work
12335  j3valid[0] = true;
12336  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12337 }
12338 for(int ij3 = 0; ij3 < 2; ++ij3)
12339 {
12340 if( !j3valid[ij3] )
12341 {
12342  continue;
12343 }
12344 _ij3[0] = ij3; _ij3[1] = -1;
12345 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12346 {
12347 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12348 {
12349  j3valid[iij3]=false; _ij3[1] = iij3; break;
12350 }
12351 }
12352 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12353 {
12354 IkReal evalcond[6];
12355 IkReal x966=IKsin(j3);
12356 IkReal x967=IKcos(j3);
12357 IkReal x968=((1.0)*gconst10);
12358 evalcond[0]=(new_r01*x966);
12359 evalcond[1]=(new_r10*x966);
12360 evalcond[2]=(gconst10*x966);
12361 evalcond[3]=(gconst10+(((-1.0)*new_r10*x967)));
12362 evalcond[4]=((((-1.0)*x967*x968))+new_r10);
12363 evalcond[5]=(((new_r01*x967))+(((-1.0)*x968)));
12364 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12365 {
12366 continue;
12367 }
12368 }
12369 
12370 {
12371 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12372 vinfos[0].jointtype = 1;
12373 vinfos[0].foffset = j0;
12374 vinfos[0].indices[0] = _ij0[0];
12375 vinfos[0].indices[1] = _ij0[1];
12376 vinfos[0].maxsolutions = _nj0;
12377 vinfos[1].jointtype = 1;
12378 vinfos[1].foffset = j1;
12379 vinfos[1].indices[0] = _ij1[0];
12380 vinfos[1].indices[1] = _ij1[1];
12381 vinfos[1].maxsolutions = _nj1;
12382 vinfos[2].jointtype = 1;
12383 vinfos[2].foffset = j2;
12384 vinfos[2].indices[0] = _ij2[0];
12385 vinfos[2].indices[1] = _ij2[1];
12386 vinfos[2].maxsolutions = _nj2;
12387 vinfos[3].jointtype = 1;
12388 vinfos[3].foffset = j3;
12389 vinfos[3].indices[0] = _ij3[0];
12390 vinfos[3].indices[1] = _ij3[1];
12391 vinfos[3].maxsolutions = _nj3;
12392 vinfos[4].jointtype = 1;
12393 vinfos[4].foffset = j4;
12394 vinfos[4].indices[0] = _ij4[0];
12395 vinfos[4].indices[1] = _ij4[1];
12396 vinfos[4].maxsolutions = _nj4;
12397 vinfos[5].jointtype = 1;
12398 vinfos[5].foffset = j5;
12399 vinfos[5].indices[0] = _ij5[0];
12400 vinfos[5].indices[1] = _ij5[1];
12401 vinfos[5].maxsolutions = _nj5;
12402 std::vector<int> vfree(0);
12403 solutions.AddSolution(vinfos,vfree);
12404 }
12405 }
12406 }
12407 
12408 } else
12409 {
12410 {
12411 IkReal j3array[2], cj3array[2], sj3array[2];
12412 bool j3valid[2]={false};
12413 _nj3 = 2;
12414 CheckValue<IkReal> x969=IKPowWithIntegerCheck(new_r10,-1);
12415 if(!x969.valid){
12416 continue;
12417 }
12418 cj3array[0]=(gconst10*(x969.value));
12419 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12420 {
12421  j3valid[0] = j3valid[1] = true;
12422  j3array[0] = IKacos(cj3array[0]);
12423  sj3array[0] = IKsin(j3array[0]);
12424  cj3array[1] = cj3array[0];
12425  j3array[1] = -j3array[0];
12426  sj3array[1] = -sj3array[0];
12427 }
12428 else if( isnan(cj3array[0]) )
12429 {
12430  // probably any value will work
12431  j3valid[0] = true;
12432  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12433 }
12434 for(int ij3 = 0; ij3 < 2; ++ij3)
12435 {
12436 if( !j3valid[ij3] )
12437 {
12438  continue;
12439 }
12440 _ij3[0] = ij3; _ij3[1] = -1;
12441 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12442 {
12443 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12444 {
12445  j3valid[iij3]=false; _ij3[1] = iij3; break;
12446 }
12447 }
12448 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12449 {
12450 IkReal evalcond[6];
12451 IkReal x970=IKsin(j3);
12452 IkReal x971=IKcos(j3);
12453 IkReal x972=((1.0)*gconst10);
12454 IkReal x973=(x971*x972);
12455 evalcond[0]=(new_r01*x970);
12456 evalcond[1]=(new_r10*x970);
12457 evalcond[2]=(gconst10*x970);
12458 evalcond[3]=(new_r01+(((-1.0)*x973)));
12459 evalcond[4]=(new_r10+(((-1.0)*x973)));
12460 evalcond[5]=(((new_r01*x971))+(((-1.0)*x972)));
12461 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12462 {
12463 continue;
12464 }
12465 }
12466 
12467 {
12468 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12469 vinfos[0].jointtype = 1;
12470 vinfos[0].foffset = j0;
12471 vinfos[0].indices[0] = _ij0[0];
12472 vinfos[0].indices[1] = _ij0[1];
12473 vinfos[0].maxsolutions = _nj0;
12474 vinfos[1].jointtype = 1;
12475 vinfos[1].foffset = j1;
12476 vinfos[1].indices[0] = _ij1[0];
12477 vinfos[1].indices[1] = _ij1[1];
12478 vinfos[1].maxsolutions = _nj1;
12479 vinfos[2].jointtype = 1;
12480 vinfos[2].foffset = j2;
12481 vinfos[2].indices[0] = _ij2[0];
12482 vinfos[2].indices[1] = _ij2[1];
12483 vinfos[2].maxsolutions = _nj2;
12484 vinfos[3].jointtype = 1;
12485 vinfos[3].foffset = j3;
12486 vinfos[3].indices[0] = _ij3[0];
12487 vinfos[3].indices[1] = _ij3[1];
12488 vinfos[3].maxsolutions = _nj3;
12489 vinfos[4].jointtype = 1;
12490 vinfos[4].foffset = j4;
12491 vinfos[4].indices[0] = _ij4[0];
12492 vinfos[4].indices[1] = _ij4[1];
12493 vinfos[4].maxsolutions = _nj4;
12494 vinfos[5].jointtype = 1;
12495 vinfos[5].foffset = j5;
12496 vinfos[5].indices[0] = _ij5[0];
12497 vinfos[5].indices[1] = _ij5[1];
12498 vinfos[5].maxsolutions = _nj5;
12499 std::vector<int> vfree(0);
12500 solutions.AddSolution(vinfos,vfree);
12501 }
12502 }
12503 }
12504 
12505 }
12506 
12507 }
12508 
12509 }
12510 } while(0);
12511 if( bgotonextstatement )
12512 {
12513 bool bgotonextstatement = true;
12514 do
12515 {
12516 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
12517 evalcond[1]=gconst11;
12518 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
12519 {
12520 bgotonextstatement=false;
12521 {
12522 IkReal j3eval[3];
12523 CheckValue<IkReal> x975 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12524 if(!x975.valid){
12525 continue;
12526 }
12527 IkReal x974=((1.0)*(x975.value));
12528 sj4=0;
12529 cj4=-1.0;
12530 j4=3.14159265358979;
12531 sj5=gconst10;
12532 cj5=gconst11;
12533 j5=((3.14159265)+(((-1.0)*x974)));
12534 new_r11=0;
12535 new_r01=0;
12536 new_r22=0;
12537 new_r20=0;
12538 IkReal gconst9=((3.14159265358979)+(((-1.0)*x974)));
12539 IkReal gconst10=((1.0)*new_r10);
12540 IkReal gconst11=((1.0)*new_r00);
12541 j3eval[0]=-1.0;
12542 j3eval[1]=-1.0;
12543 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12544 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12545 {
12546 {
12547 IkReal j3eval[3];
12548 CheckValue<IkReal> x977 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12549 if(!x977.valid){
12550 continue;
12551 }
12552 IkReal x976=((1.0)*(x977.value));
12553 sj4=0;
12554 cj4=-1.0;
12555 j4=3.14159265358979;
12556 sj5=gconst10;
12557 cj5=gconst11;
12558 j5=((3.14159265)+(((-1.0)*x976)));
12559 new_r11=0;
12560 new_r01=0;
12561 new_r22=0;
12562 new_r20=0;
12563 IkReal gconst9=((3.14159265358979)+(((-1.0)*x976)));
12564 IkReal gconst10=((1.0)*new_r10);
12565 IkReal gconst11=((1.0)*new_r00);
12566 j3eval[0]=-1.0;
12567 j3eval[1]=-1.0;
12568 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12569 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12570 {
12571 {
12572 IkReal j3eval[3];
12573 CheckValue<IkReal> x979 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12574 if(!x979.valid){
12575 continue;
12576 }
12577 IkReal x978=((1.0)*(x979.value));
12578 sj4=0;
12579 cj4=-1.0;
12580 j4=3.14159265358979;
12581 sj5=gconst10;
12582 cj5=gconst11;
12583 j5=((3.14159265)+(((-1.0)*x978)));
12584 new_r11=0;
12585 new_r01=0;
12586 new_r22=0;
12587 new_r20=0;
12588 IkReal gconst9=((3.14159265358979)+(((-1.0)*x978)));
12589 IkReal gconst10=((1.0)*new_r10);
12590 IkReal gconst11=((1.0)*new_r00);
12591 j3eval[0]=-1.0;
12592 j3eval[1]=-1.0;
12593 j3eval[2]=((IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
12594 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12595 {
12596 continue; // 3 cases reached
12597 
12598 } else
12599 {
12600 {
12601 IkReal j3array[1], cj3array[1], sj3array[1];
12602 bool j3valid[1]={false};
12603 _nj3 = 1;
12604 CheckValue<IkReal> x980 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
12605 if(!x980.valid){
12606 continue;
12607 }
12608 CheckValue<IkReal> x981=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12609 if(!x981.valid){
12610 continue;
12611 }
12612 j3array[0]=((-1.5707963267949)+(x980.value)+(((1.5707963267949)*(x981.value))));
12613 sj3array[0]=IKsin(j3array[0]);
12614 cj3array[0]=IKcos(j3array[0]);
12615 if( j3array[0] > IKPI )
12616 {
12617  j3array[0]-=IK2PI;
12618 }
12619 else if( j3array[0] < -IKPI )
12620 { j3array[0]+=IK2PI;
12621 }
12622 j3valid[0] = true;
12623 for(int ij3 = 0; ij3 < 1; ++ij3)
12624 {
12625 if( !j3valid[ij3] )
12626 {
12627  continue;
12628 }
12629 _ij3[0] = ij3; _ij3[1] = -1;
12630 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12631 {
12632 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12633 {
12634  j3valid[iij3]=false; _ij3[1] = iij3; break;
12635 }
12636 }
12637 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12638 {
12639 IkReal evalcond[6];
12640 IkReal x982=IKsin(j3);
12641 IkReal x983=IKcos(j3);
12642 IkReal x984=(gconst11*x982);
12643 IkReal x985=(gconst10*x982);
12644 IkReal x986=(gconst11*x983);
12645 IkReal x987=((1.0)*x983);
12646 IkReal x988=(gconst10*x987);
12647 evalcond[0]=((((-1.0)*x988))+x984);
12648 evalcond[1]=(((new_r00*x983))+gconst11+((new_r10*x982)));
12649 evalcond[2]=(new_r00+x985+x986);
12650 evalcond[3]=(((new_r00*x982))+gconst10+(((-1.0)*new_r10*x987)));
12651 evalcond[4]=((((-1.0)*x985))+(((-1.0)*x986)));
12652 evalcond[5]=((((-1.0)*x988))+new_r10+x984);
12653 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12654 {
12655 continue;
12656 }
12657 }
12658 
12659 {
12660 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12661 vinfos[0].jointtype = 1;
12662 vinfos[0].foffset = j0;
12663 vinfos[0].indices[0] = _ij0[0];
12664 vinfos[0].indices[1] = _ij0[1];
12665 vinfos[0].maxsolutions = _nj0;
12666 vinfos[1].jointtype = 1;
12667 vinfos[1].foffset = j1;
12668 vinfos[1].indices[0] = _ij1[0];
12669 vinfos[1].indices[1] = _ij1[1];
12670 vinfos[1].maxsolutions = _nj1;
12671 vinfos[2].jointtype = 1;
12672 vinfos[2].foffset = j2;
12673 vinfos[2].indices[0] = _ij2[0];
12674 vinfos[2].indices[1] = _ij2[1];
12675 vinfos[2].maxsolutions = _nj2;
12676 vinfos[3].jointtype = 1;
12677 vinfos[3].foffset = j3;
12678 vinfos[3].indices[0] = _ij3[0];
12679 vinfos[3].indices[1] = _ij3[1];
12680 vinfos[3].maxsolutions = _nj3;
12681 vinfos[4].jointtype = 1;
12682 vinfos[4].foffset = j4;
12683 vinfos[4].indices[0] = _ij4[0];
12684 vinfos[4].indices[1] = _ij4[1];
12685 vinfos[4].maxsolutions = _nj4;
12686 vinfos[5].jointtype = 1;
12687 vinfos[5].foffset = j5;
12688 vinfos[5].indices[0] = _ij5[0];
12689 vinfos[5].indices[1] = _ij5[1];
12690 vinfos[5].maxsolutions = _nj5;
12691 std::vector<int> vfree(0);
12692 solutions.AddSolution(vinfos,vfree);
12693 }
12694 }
12695 }
12696 
12697 }
12698 
12699 }
12700 
12701 } else
12702 {
12703 {
12704 IkReal j3array[1], cj3array[1], sj3array[1];
12705 bool j3valid[1]={false};
12706 _nj3 = 1;
12707 CheckValue<IkReal> x989=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
12708 if(!x989.valid){
12709 continue;
12710 }
12711 CheckValue<IkReal> x990 = IKatan2WithCheck(IkReal((gconst10*new_r00)),IkReal((gconst11*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12712 if(!x990.valid){
12713 continue;
12714 }
12715 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x989.value)))+(x990.value));
12716 sj3array[0]=IKsin(j3array[0]);
12717 cj3array[0]=IKcos(j3array[0]);
12718 if( j3array[0] > IKPI )
12719 {
12720  j3array[0]-=IK2PI;
12721 }
12722 else if( j3array[0] < -IKPI )
12723 { j3array[0]+=IK2PI;
12724 }
12725 j3valid[0] = true;
12726 for(int ij3 = 0; ij3 < 1; ++ij3)
12727 {
12728 if( !j3valid[ij3] )
12729 {
12730  continue;
12731 }
12732 _ij3[0] = ij3; _ij3[1] = -1;
12733 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12734 {
12735 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12736 {
12737  j3valid[iij3]=false; _ij3[1] = iij3; break;
12738 }
12739 }
12740 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12741 {
12742 IkReal evalcond[6];
12743 IkReal x991=IKsin(j3);
12744 IkReal x992=IKcos(j3);
12745 IkReal x993=(gconst11*x991);
12746 IkReal x994=(gconst10*x991);
12747 IkReal x995=(gconst11*x992);
12748 IkReal x996=((1.0)*x992);
12749 IkReal x997=(gconst10*x996);
12750 evalcond[0]=((((-1.0)*x997))+x993);
12751 evalcond[1]=(((new_r10*x991))+gconst11+((new_r00*x992)));
12752 evalcond[2]=(new_r00+x995+x994);
12753 evalcond[3]=((((-1.0)*new_r10*x996))+gconst10+((new_r00*x991)));
12754 evalcond[4]=((((-1.0)*x994))+(((-1.0)*x995)));
12755 evalcond[5]=((((-1.0)*x997))+new_r10+x993);
12756 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12757 {
12758 continue;
12759 }
12760 }
12761 
12762 {
12763 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12764 vinfos[0].jointtype = 1;
12765 vinfos[0].foffset = j0;
12766 vinfos[0].indices[0] = _ij0[0];
12767 vinfos[0].indices[1] = _ij0[1];
12768 vinfos[0].maxsolutions = _nj0;
12769 vinfos[1].jointtype = 1;
12770 vinfos[1].foffset = j1;
12771 vinfos[1].indices[0] = _ij1[0];
12772 vinfos[1].indices[1] = _ij1[1];
12773 vinfos[1].maxsolutions = _nj1;
12774 vinfos[2].jointtype = 1;
12775 vinfos[2].foffset = j2;
12776 vinfos[2].indices[0] = _ij2[0];
12777 vinfos[2].indices[1] = _ij2[1];
12778 vinfos[2].maxsolutions = _nj2;
12779 vinfos[3].jointtype = 1;
12780 vinfos[3].foffset = j3;
12781 vinfos[3].indices[0] = _ij3[0];
12782 vinfos[3].indices[1] = _ij3[1];
12783 vinfos[3].maxsolutions = _nj3;
12784 vinfos[4].jointtype = 1;
12785 vinfos[4].foffset = j4;
12786 vinfos[4].indices[0] = _ij4[0];
12787 vinfos[4].indices[1] = _ij4[1];
12788 vinfos[4].maxsolutions = _nj4;
12789 vinfos[5].jointtype = 1;
12790 vinfos[5].foffset = j5;
12791 vinfos[5].indices[0] = _ij5[0];
12792 vinfos[5].indices[1] = _ij5[1];
12793 vinfos[5].maxsolutions = _nj5;
12794 std::vector<int> vfree(0);
12795 solutions.AddSolution(vinfos,vfree);
12796 }
12797 }
12798 }
12799 
12800 }
12801 
12802 }
12803 
12804 } else
12805 {
12806 {
12807 IkReal j3array[1], cj3array[1], sj3array[1];
12808 bool j3valid[1]={false};
12809 _nj3 = 1;
12810 CheckValue<IkReal> x998 = IKatan2WithCheck(IkReal((gconst10*gconst11)),IkReal(gconst11*gconst11),IKFAST_ATAN2_MAGTHRESH);
12811 if(!x998.valid){
12812 continue;
12813 }
12814 CheckValue<IkReal> x999=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12815 if(!x999.valid){
12816 continue;
12817 }
12818 j3array[0]=((-1.5707963267949)+(x998.value)+(((1.5707963267949)*(x999.value))));
12819 sj3array[0]=IKsin(j3array[0]);
12820 cj3array[0]=IKcos(j3array[0]);
12821 if( j3array[0] > IKPI )
12822 {
12823  j3array[0]-=IK2PI;
12824 }
12825 else if( j3array[0] < -IKPI )
12826 { j3array[0]+=IK2PI;
12827 }
12828 j3valid[0] = true;
12829 for(int ij3 = 0; ij3 < 1; ++ij3)
12830 {
12831 if( !j3valid[ij3] )
12832 {
12833  continue;
12834 }
12835 _ij3[0] = ij3; _ij3[1] = -1;
12836 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12837 {
12838 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12839 {
12840  j3valid[iij3]=false; _ij3[1] = iij3; break;
12841 }
12842 }
12843 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12844 {
12845 IkReal evalcond[6];
12846 IkReal x1000=IKsin(j3);
12847 IkReal x1001=IKcos(j3);
12848 IkReal x1002=(gconst11*x1000);
12849 IkReal x1003=(gconst10*x1000);
12850 IkReal x1004=(gconst11*x1001);
12851 IkReal x1005=((1.0)*x1001);
12852 IkReal x1006=(gconst10*x1005);
12853 evalcond[0]=(x1002+(((-1.0)*x1006)));
12854 evalcond[1]=(gconst11+((new_r10*x1000))+((new_r00*x1001)));
12855 evalcond[2]=(x1004+x1003+new_r00);
12856 evalcond[3]=(gconst10+(((-1.0)*new_r10*x1005))+((new_r00*x1000)));
12857 evalcond[4]=((((-1.0)*x1003))+(((-1.0)*x1004)));
12858 evalcond[5]=(x1002+(((-1.0)*x1006))+new_r10);
12859 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12860 {
12861 continue;
12862 }
12863 }
12864 
12865 {
12866 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12867 vinfos[0].jointtype = 1;
12868 vinfos[0].foffset = j0;
12869 vinfos[0].indices[0] = _ij0[0];
12870 vinfos[0].indices[1] = _ij0[1];
12871 vinfos[0].maxsolutions = _nj0;
12872 vinfos[1].jointtype = 1;
12873 vinfos[1].foffset = j1;
12874 vinfos[1].indices[0] = _ij1[0];
12875 vinfos[1].indices[1] = _ij1[1];
12876 vinfos[1].maxsolutions = _nj1;
12877 vinfos[2].jointtype = 1;
12878 vinfos[2].foffset = j2;
12879 vinfos[2].indices[0] = _ij2[0];
12880 vinfos[2].indices[1] = _ij2[1];
12881 vinfos[2].maxsolutions = _nj2;
12882 vinfos[3].jointtype = 1;
12883 vinfos[3].foffset = j3;
12884 vinfos[3].indices[0] = _ij3[0];
12885 vinfos[3].indices[1] = _ij3[1];
12886 vinfos[3].maxsolutions = _nj3;
12887 vinfos[4].jointtype = 1;
12888 vinfos[4].foffset = j4;
12889 vinfos[4].indices[0] = _ij4[0];
12890 vinfos[4].indices[1] = _ij4[1];
12891 vinfos[4].maxsolutions = _nj4;
12892 vinfos[5].jointtype = 1;
12893 vinfos[5].foffset = j5;
12894 vinfos[5].indices[0] = _ij5[0];
12895 vinfos[5].indices[1] = _ij5[1];
12896 vinfos[5].maxsolutions = _nj5;
12897 std::vector<int> vfree(0);
12898 solutions.AddSolution(vinfos,vfree);
12899 }
12900 }
12901 }
12902 
12903 }
12904 
12905 }
12906 
12907 }
12908 } while(0);
12909 if( bgotonextstatement )
12910 {
12911 bool bgotonextstatement = true;
12912 do
12913 {
12914 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12915 if( IKabs(evalcond[0]) < 0.0000050000000000 )
12916 {
12917 bgotonextstatement=false;
12918 {
12919 IkReal j3eval[1];
12920 CheckValue<IkReal> x1008 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12921 if(!x1008.valid){
12922 continue;
12923 }
12924 IkReal x1007=((1.0)*(x1008.value));
12925 sj4=0;
12926 cj4=-1.0;
12927 j4=3.14159265358979;
12928 sj5=gconst10;
12929 cj5=gconst11;
12930 j5=((3.14159265)+(((-1.0)*x1007)));
12931 new_r01=0;
12932 new_r10=0;
12933 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1007)));
12934 IkReal gconst10=0;
12935 IkReal x1009 = new_r00*new_r00;
12936 if(IKabs(x1009)==0){
12937 continue;
12938 }
12939 IkReal gconst11=((1.0)*new_r00*(pow(x1009,-0.5)));
12940 j3eval[0]=new_r11;
12941 if( IKabs(j3eval[0]) < 0.0000010000000000 )
12942 {
12943 {
12944 IkReal j3array[2], cj3array[2], sj3array[2];
12945 bool j3valid[2]={false};
12946 _nj3 = 2;
12947 CheckValue<IkReal> x1010=IKPowWithIntegerCheck(gconst11,-1);
12948 if(!x1010.valid){
12949 continue;
12950 }
12951 cj3array[0]=(new_r11*(x1010.value));
12952 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12953 {
12954  j3valid[0] = j3valid[1] = true;
12955  j3array[0] = IKacos(cj3array[0]);
12956  sj3array[0] = IKsin(j3array[0]);
12957  cj3array[1] = cj3array[0];
12958  j3array[1] = -j3array[0];
12959  sj3array[1] = -sj3array[0];
12960 }
12961 else if( isnan(cj3array[0]) )
12962 {
12963  // probably any value will work
12964  j3valid[0] = true;
12965  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12966 }
12967 for(int ij3 = 0; ij3 < 2; ++ij3)
12968 {
12969 if( !j3valid[ij3] )
12970 {
12971  continue;
12972 }
12973 _ij3[0] = ij3; _ij3[1] = -1;
12974 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12975 {
12976 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12977 {
12978  j3valid[iij3]=false; _ij3[1] = iij3; break;
12979 }
12980 }
12981 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12982 {
12983 IkReal evalcond[6];
12984 IkReal x1011=IKsin(j3);
12985 IkReal x1012=IKcos(j3);
12986 evalcond[0]=(new_r00*x1011);
12987 evalcond[1]=(new_r11*x1011);
12988 evalcond[2]=(gconst11*x1011);
12989 evalcond[3]=(gconst11+((new_r00*x1012)));
12990 evalcond[4]=(((gconst11*x1012))+new_r00);
12991 evalcond[5]=((((-1.0)*new_r11*x1012))+gconst11);
12992 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12993 {
12994 continue;
12995 }
12996 }
12997 
12998 {
12999 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13000 vinfos[0].jointtype = 1;
13001 vinfos[0].foffset = j0;
13002 vinfos[0].indices[0] = _ij0[0];
13003 vinfos[0].indices[1] = _ij0[1];
13004 vinfos[0].maxsolutions = _nj0;
13005 vinfos[1].jointtype = 1;
13006 vinfos[1].foffset = j1;
13007 vinfos[1].indices[0] = _ij1[0];
13008 vinfos[1].indices[1] = _ij1[1];
13009 vinfos[1].maxsolutions = _nj1;
13010 vinfos[2].jointtype = 1;
13011 vinfos[2].foffset = j2;
13012 vinfos[2].indices[0] = _ij2[0];
13013 vinfos[2].indices[1] = _ij2[1];
13014 vinfos[2].maxsolutions = _nj2;
13015 vinfos[3].jointtype = 1;
13016 vinfos[3].foffset = j3;
13017 vinfos[3].indices[0] = _ij3[0];
13018 vinfos[3].indices[1] = _ij3[1];
13019 vinfos[3].maxsolutions = _nj3;
13020 vinfos[4].jointtype = 1;
13021 vinfos[4].foffset = j4;
13022 vinfos[4].indices[0] = _ij4[0];
13023 vinfos[4].indices[1] = _ij4[1];
13024 vinfos[4].maxsolutions = _nj4;
13025 vinfos[5].jointtype = 1;
13026 vinfos[5].foffset = j5;
13027 vinfos[5].indices[0] = _ij5[0];
13028 vinfos[5].indices[1] = _ij5[1];
13029 vinfos[5].maxsolutions = _nj5;
13030 std::vector<int> vfree(0);
13031 solutions.AddSolution(vinfos,vfree);
13032 }
13033 }
13034 }
13035 
13036 } else
13037 {
13038 {
13039 IkReal j3array[2], cj3array[2], sj3array[2];
13040 bool j3valid[2]={false};
13041 _nj3 = 2;
13042 CheckValue<IkReal> x1013=IKPowWithIntegerCheck(new_r11,-1);
13043 if(!x1013.valid){
13044 continue;
13045 }
13046 cj3array[0]=(gconst11*(x1013.value));
13047 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
13048 {
13049  j3valid[0] = j3valid[1] = true;
13050  j3array[0] = IKacos(cj3array[0]);
13051  sj3array[0] = IKsin(j3array[0]);
13052  cj3array[1] = cj3array[0];
13053  j3array[1] = -j3array[0];
13054  sj3array[1] = -sj3array[0];
13055 }
13056 else if( isnan(cj3array[0]) )
13057 {
13058  // probably any value will work
13059  j3valid[0] = true;
13060  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13061 }
13062 for(int ij3 = 0; ij3 < 2; ++ij3)
13063 {
13064 if( !j3valid[ij3] )
13065 {
13066  continue;
13067 }
13068 _ij3[0] = ij3; _ij3[1] = -1;
13069 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
13070 {
13071 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13072 {
13073  j3valid[iij3]=false; _ij3[1] = iij3; break;
13074 }
13075 }
13076 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13077 {
13078 IkReal evalcond[6];
13079 IkReal x1014=IKsin(j3);
13080 IkReal x1015=IKcos(j3);
13081 IkReal x1016=(gconst11*x1015);
13082 evalcond[0]=(new_r00*x1014);
13083 evalcond[1]=(new_r11*x1014);
13084 evalcond[2]=(gconst11*x1014);
13085 evalcond[3]=(gconst11+((new_r00*x1015)));
13086 evalcond[4]=(x1016+new_r00);
13087 evalcond[5]=(new_r11+(((-1.0)*x1016)));
13088 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
13089 {
13090 continue;
13091 }
13092 }
13093 
13094 {
13095 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13096 vinfos[0].jointtype = 1;
13097 vinfos[0].foffset = j0;
13098 vinfos[0].indices[0] = _ij0[0];
13099 vinfos[0].indices[1] = _ij0[1];
13100 vinfos[0].maxsolutions = _nj0;
13101 vinfos[1].jointtype = 1;
13102 vinfos[1].foffset = j1;
13103 vinfos[1].indices[0] = _ij1[0];
13104 vinfos[1].indices[1] = _ij1[1];
13105 vinfos[1].maxsolutions = _nj1;
13106 vinfos[2].jointtype = 1;
13107 vinfos[2].foffset = j2;
13108 vinfos[2].indices[0] = _ij2[0];
13109 vinfos[2].indices[1] = _ij2[1];
13110 vinfos[2].maxsolutions = _nj2;
13111 vinfos[3].jointtype = 1;
13112 vinfos[3].foffset = j3;
13113 vinfos[3].indices[0] = _ij3[0];
13114 vinfos[3].indices[1] = _ij3[1];
13115 vinfos[3].maxsolutions = _nj3;
13116 vinfos[4].jointtype = 1;
13117 vinfos[4].foffset = j4;
13118 vinfos[4].indices[0] = _ij4[0];
13119 vinfos[4].indices[1] = _ij4[1];
13120 vinfos[4].maxsolutions = _nj4;
13121 vinfos[5].jointtype = 1;
13122 vinfos[5].foffset = j5;
13123 vinfos[5].indices[0] = _ij5[0];
13124 vinfos[5].indices[1] = _ij5[1];
13125 vinfos[5].maxsolutions = _nj5;
13126 std::vector<int> vfree(0);
13127 solutions.AddSolution(vinfos,vfree);
13128 }
13129 }
13130 }
13131 
13132 }
13133 
13134 }
13135 
13136 }
13137 } while(0);
13138 if( bgotonextstatement )
13139 {
13140 bool bgotonextstatement = true;
13141 do
13142 {
13143 evalcond[0]=IKabs(new_r00);
13144 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13145 {
13146 bgotonextstatement=false;
13147 {
13148 IkReal j3eval[1];
13149 CheckValue<IkReal> x1018 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13150 if(!x1018.valid){
13151 continue;
13152 }
13153 IkReal x1017=((1.0)*(x1018.value));
13154 sj4=0;
13155 cj4=-1.0;
13156 j4=3.14159265358979;
13157 sj5=gconst10;
13158 cj5=gconst11;
13159 j5=((3.14159265)+(((-1.0)*x1017)));
13160 new_r00=0;
13161 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1017)));
13162 IkReal x1019 = new_r10*new_r10;
13163 if(IKabs(x1019)==0){
13164 continue;
13165 }
13166 IkReal gconst10=((1.0)*new_r10*(pow(x1019,-0.5)));
13167 IkReal gconst11=0;
13168 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13169 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13170 {
13171 {
13172 IkReal j3eval[1];
13173 CheckValue<IkReal> x1021 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13174 if(!x1021.valid){
13175 continue;
13176 }
13177 IkReal x1020=((1.0)*(x1021.value));
13178 sj4=0;
13179 cj4=-1.0;
13180 j4=3.14159265358979;
13181 sj5=gconst10;
13182 cj5=gconst11;
13183 j5=((3.14159265)+(((-1.0)*x1020)));
13184 new_r00=0;
13185 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1020)));
13186 IkReal x1022 = new_r10*new_r10;
13187 if(IKabs(x1022)==0){
13188 continue;
13189 }
13190 IkReal gconst10=((1.0)*new_r10*(pow(x1022,-0.5)));
13191 IkReal gconst11=0;
13192 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13193 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13194 {
13195 {
13196 IkReal j3eval[1];
13197 CheckValue<IkReal> x1024 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13198 if(!x1024.valid){
13199 continue;
13200 }
13201 IkReal x1023=((1.0)*(x1024.value));
13202 sj4=0;
13203 cj4=-1.0;
13204 j4=3.14159265358979;
13205 sj5=gconst10;
13206 cj5=gconst11;
13207 j5=((3.14159265)+(((-1.0)*x1023)));
13208 new_r00=0;
13209 IkReal gconst9=((3.14159265358979)+(((-1.0)*x1023)));
13210 IkReal x1025 = new_r10*new_r10;
13211 if(IKabs(x1025)==0){
13212 continue;
13213 }
13214 IkReal gconst10=((1.0)*new_r10*(pow(x1025,-0.5)));
13215 IkReal gconst11=0;
13216 j3eval[0]=new_r10;
13217 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13218 {
13219 continue; // 3 cases reached
13220 
13221 } else
13222 {
13223 {
13224 IkReal j3array[1], cj3array[1], sj3array[1];
13225 bool j3valid[1]={false};
13226 _nj3 = 1;
13227 CheckValue<IkReal> x1026=IKPowWithIntegerCheck(gconst10,-1);
13228 if(!x1026.valid){
13229 continue;
13230 }
13231 CheckValue<IkReal> x1027=IKPowWithIntegerCheck(new_r10,-1);
13232 if(!x1027.valid){
13233 continue;
13234 }
13235 if( IKabs((new_r11*(x1026.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst10*(x1027.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x1026.value)))+IKsqr((gconst10*(x1027.value)))-1) <= IKFAST_SINCOS_THRESH )
13236  continue;
13237 j3array[0]=IKatan2((new_r11*(x1026.value)), (gconst10*(x1027.value)));
13238 sj3array[0]=IKsin(j3array[0]);
13239 cj3array[0]=IKcos(j3array[0]);
13240 if( j3array[0] > IKPI )
13241 {
13242  j3array[0]-=IK2PI;
13243 }
13244 else if( j3array[0] < -IKPI )
13245 { j3array[0]+=IK2PI;
13246 }
13247 j3valid[0] = true;
13248 for(int ij3 = 0; ij3 < 1; ++ij3)
13249 {
13250 if( !j3valid[ij3] )
13251 {
13252  continue;
13253 }
13254 _ij3[0] = ij3; _ij3[1] = -1;
13255 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13256 {
13257 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13258 {
13259  j3valid[iij3]=false; _ij3[1] = iij3; break;
13260 }
13261 }
13262 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13263 {
13264 IkReal evalcond[8];
13265 IkReal x1028=IKsin(j3);
13266 IkReal x1029=IKcos(j3);
13267 IkReal x1030=(gconst10*x1028);
13268 IkReal x1031=((1.0)*x1029);
13269 IkReal x1032=(gconst10*x1031);
13270 evalcond[0]=(new_r10*x1028);
13271 evalcond[1]=x1030;
13272 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1031)));
13273 evalcond[3]=((((-1.0)*x1032))+new_r01);
13274 evalcond[4]=((((-1.0)*x1030))+new_r11);
13275 evalcond[5]=((((-1.0)*x1032))+new_r10);
13276 evalcond[6]=((((-1.0)*new_r11*x1031))+((new_r01*x1028)));
13277 evalcond[7]=(((new_r11*x1028))+(((-1.0)*gconst10))+((new_r01*x1029)));
13278 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13279 {
13280 continue;
13281 }
13282 }
13283 
13284 {
13285 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13286 vinfos[0].jointtype = 1;
13287 vinfos[0].foffset = j0;
13288 vinfos[0].indices[0] = _ij0[0];
13289 vinfos[0].indices[1] = _ij0[1];
13290 vinfos[0].maxsolutions = _nj0;
13291 vinfos[1].jointtype = 1;
13292 vinfos[1].foffset = j1;
13293 vinfos[1].indices[0] = _ij1[0];
13294 vinfos[1].indices[1] = _ij1[1];
13295 vinfos[1].maxsolutions = _nj1;
13296 vinfos[2].jointtype = 1;
13297 vinfos[2].foffset = j2;
13298 vinfos[2].indices[0] = _ij2[0];
13299 vinfos[2].indices[1] = _ij2[1];
13300 vinfos[2].maxsolutions = _nj2;
13301 vinfos[3].jointtype = 1;
13302 vinfos[3].foffset = j3;
13303 vinfos[3].indices[0] = _ij3[0];
13304 vinfos[3].indices[1] = _ij3[1];
13305 vinfos[3].maxsolutions = _nj3;
13306 vinfos[4].jointtype = 1;
13307 vinfos[4].foffset = j4;
13308 vinfos[4].indices[0] = _ij4[0];
13309 vinfos[4].indices[1] = _ij4[1];
13310 vinfos[4].maxsolutions = _nj4;
13311 vinfos[5].jointtype = 1;
13312 vinfos[5].foffset = j5;
13313 vinfos[5].indices[0] = _ij5[0];
13314 vinfos[5].indices[1] = _ij5[1];
13315 vinfos[5].maxsolutions = _nj5;
13316 std::vector<int> vfree(0);
13317 solutions.AddSolution(vinfos,vfree);
13318 }
13319 }
13320 }
13321 
13322 }
13323 
13324 }
13325 
13326 } else
13327 {
13328 {
13329 IkReal j3array[1], cj3array[1], sj3array[1];
13330 bool j3valid[1]={false};
13331 _nj3 = 1;
13332 CheckValue<IkReal> x1033=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13333 if(!x1033.valid){
13334 continue;
13335 }
13336 CheckValue<IkReal> x1034 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13337 if(!x1034.valid){
13338 continue;
13339 }
13340 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1033.value)))+(x1034.value));
13341 sj3array[0]=IKsin(j3array[0]);
13342 cj3array[0]=IKcos(j3array[0]);
13343 if( j3array[0] > IKPI )
13344 {
13345  j3array[0]-=IK2PI;
13346 }
13347 else if( j3array[0] < -IKPI )
13348 { j3array[0]+=IK2PI;
13349 }
13350 j3valid[0] = true;
13351 for(int ij3 = 0; ij3 < 1; ++ij3)
13352 {
13353 if( !j3valid[ij3] )
13354 {
13355  continue;
13356 }
13357 _ij3[0] = ij3; _ij3[1] = -1;
13358 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13359 {
13360 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13361 {
13362  j3valid[iij3]=false; _ij3[1] = iij3; break;
13363 }
13364 }
13365 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13366 {
13367 IkReal evalcond[8];
13368 IkReal x1035=IKsin(j3);
13369 IkReal x1036=IKcos(j3);
13370 IkReal x1037=(gconst10*x1035);
13371 IkReal x1038=((1.0)*x1036);
13372 IkReal x1039=(gconst10*x1038);
13373 evalcond[0]=(new_r10*x1035);
13374 evalcond[1]=x1037;
13375 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1038)));
13376 evalcond[3]=((((-1.0)*x1039))+new_r01);
13377 evalcond[4]=((((-1.0)*x1037))+new_r11);
13378 evalcond[5]=((((-1.0)*x1039))+new_r10);
13379 evalcond[6]=((((-1.0)*new_r11*x1038))+((new_r01*x1035)));
13380 evalcond[7]=(((new_r11*x1035))+(((-1.0)*gconst10))+((new_r01*x1036)));
13381 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13382 {
13383 continue;
13384 }
13385 }
13386 
13387 {
13388 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13389 vinfos[0].jointtype = 1;
13390 vinfos[0].foffset = j0;
13391 vinfos[0].indices[0] = _ij0[0];
13392 vinfos[0].indices[1] = _ij0[1];
13393 vinfos[0].maxsolutions = _nj0;
13394 vinfos[1].jointtype = 1;
13395 vinfos[1].foffset = j1;
13396 vinfos[1].indices[0] = _ij1[0];
13397 vinfos[1].indices[1] = _ij1[1];
13398 vinfos[1].maxsolutions = _nj1;
13399 vinfos[2].jointtype = 1;
13400 vinfos[2].foffset = j2;
13401 vinfos[2].indices[0] = _ij2[0];
13402 vinfos[2].indices[1] = _ij2[1];
13403 vinfos[2].maxsolutions = _nj2;
13404 vinfos[3].jointtype = 1;
13405 vinfos[3].foffset = j3;
13406 vinfos[3].indices[0] = _ij3[0];
13407 vinfos[3].indices[1] = _ij3[1];
13408 vinfos[3].maxsolutions = _nj3;
13409 vinfos[4].jointtype = 1;
13410 vinfos[4].foffset = j4;
13411 vinfos[4].indices[0] = _ij4[0];
13412 vinfos[4].indices[1] = _ij4[1];
13413 vinfos[4].maxsolutions = _nj4;
13414 vinfos[5].jointtype = 1;
13415 vinfos[5].foffset = j5;
13416 vinfos[5].indices[0] = _ij5[0];
13417 vinfos[5].indices[1] = _ij5[1];
13418 vinfos[5].maxsolutions = _nj5;
13419 std::vector<int> vfree(0);
13420 solutions.AddSolution(vinfos,vfree);
13421 }
13422 }
13423 }
13424 
13425 }
13426 
13427 }
13428 
13429 } else
13430 {
13431 {
13432 IkReal j3array[1], cj3array[1], sj3array[1];
13433 bool j3valid[1]={false};
13434 _nj3 = 1;
13435 CheckValue<IkReal> x1040=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13436 if(!x1040.valid){
13437 continue;
13438 }
13439 CheckValue<IkReal> x1041 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
13440 if(!x1041.valid){
13441 continue;
13442 }
13443 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1040.value)))+(x1041.value));
13444 sj3array[0]=IKsin(j3array[0]);
13445 cj3array[0]=IKcos(j3array[0]);
13446 if( j3array[0] > IKPI )
13447 {
13448  j3array[0]-=IK2PI;
13449 }
13450 else if( j3array[0] < -IKPI )
13451 { j3array[0]+=IK2PI;
13452 }
13453 j3valid[0] = true;
13454 for(int ij3 = 0; ij3 < 1; ++ij3)
13455 {
13456 if( !j3valid[ij3] )
13457 {
13458  continue;
13459 }
13460 _ij3[0] = ij3; _ij3[1] = -1;
13461 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13462 {
13463 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13464 {
13465  j3valid[iij3]=false; _ij3[1] = iij3; break;
13466 }
13467 }
13468 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13469 {
13470 IkReal evalcond[8];
13471 IkReal x1042=IKsin(j3);
13472 IkReal x1043=IKcos(j3);
13473 IkReal x1044=(gconst10*x1042);
13474 IkReal x1045=((1.0)*x1043);
13475 IkReal x1046=(gconst10*x1045);
13476 evalcond[0]=(new_r10*x1042);
13477 evalcond[1]=x1044;
13478 evalcond[2]=(gconst10+(((-1.0)*new_r10*x1045)));
13479 evalcond[3]=(new_r01+(((-1.0)*x1046)));
13480 evalcond[4]=((((-1.0)*x1044))+new_r11);
13481 evalcond[5]=(new_r10+(((-1.0)*x1046)));
13482 evalcond[6]=(((new_r01*x1042))+(((-1.0)*new_r11*x1045)));
13483 evalcond[7]=(((new_r11*x1042))+((new_r01*x1043))+(((-1.0)*gconst10)));
13484 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13485 {
13486 continue;
13487 }
13488 }
13489 
13490 {
13491 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13492 vinfos[0].jointtype = 1;
13493 vinfos[0].foffset = j0;
13494 vinfos[0].indices[0] = _ij0[0];
13495 vinfos[0].indices[1] = _ij0[1];
13496 vinfos[0].maxsolutions = _nj0;
13497 vinfos[1].jointtype = 1;
13498 vinfos[1].foffset = j1;
13499 vinfos[1].indices[0] = _ij1[0];
13500 vinfos[1].indices[1] = _ij1[1];
13501 vinfos[1].maxsolutions = _nj1;
13502 vinfos[2].jointtype = 1;
13503 vinfos[2].foffset = j2;
13504 vinfos[2].indices[0] = _ij2[0];
13505 vinfos[2].indices[1] = _ij2[1];
13506 vinfos[2].maxsolutions = _nj2;
13507 vinfos[3].jointtype = 1;
13508 vinfos[3].foffset = j3;
13509 vinfos[3].indices[0] = _ij3[0];
13510 vinfos[3].indices[1] = _ij3[1];
13511 vinfos[3].maxsolutions = _nj3;
13512 vinfos[4].jointtype = 1;
13513 vinfos[4].foffset = j4;
13514 vinfos[4].indices[0] = _ij4[0];
13515 vinfos[4].indices[1] = _ij4[1];
13516 vinfos[4].maxsolutions = _nj4;
13517 vinfos[5].jointtype = 1;
13518 vinfos[5].foffset = j5;
13519 vinfos[5].indices[0] = _ij5[0];
13520 vinfos[5].indices[1] = _ij5[1];
13521 vinfos[5].maxsolutions = _nj5;
13522 std::vector<int> vfree(0);
13523 solutions.AddSolution(vinfos,vfree);
13524 }
13525 }
13526 }
13527 
13528 }
13529 
13530 }
13531 
13532 }
13533 } while(0);
13534 if( bgotonextstatement )
13535 {
13536 bool bgotonextstatement = true;
13537 do
13538 {
13539 if( 1 )
13540 {
13541 bgotonextstatement=false;
13542 continue; // branch miss [j3]
13543 
13544 }
13545 } while(0);
13546 if( bgotonextstatement )
13547 {
13548 }
13549 }
13550 }
13551 }
13552 }
13553 }
13554 
13555 } else
13556 {
13557 {
13558 IkReal j3array[1], cj3array[1], sj3array[1];
13559 bool j3valid[1]={false};
13560 _nj3 = 1;
13561 CheckValue<IkReal> x1047 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13562 if(!x1047.valid){
13563 continue;
13564 }
13565 CheckValue<IkReal> x1048=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
13566 if(!x1048.valid){
13567 continue;
13568 }
13569 j3array[0]=((-1.5707963267949)+(x1047.value)+(((1.5707963267949)*(x1048.value))));
13570 sj3array[0]=IKsin(j3array[0]);
13571 cj3array[0]=IKcos(j3array[0]);
13572 if( j3array[0] > IKPI )
13573 {
13574  j3array[0]-=IK2PI;
13575 }
13576 else if( j3array[0] < -IKPI )
13577 { j3array[0]+=IK2PI;
13578 }
13579 j3valid[0] = true;
13580 for(int ij3 = 0; ij3 < 1; ++ij3)
13581 {
13582 if( !j3valid[ij3] )
13583 {
13584  continue;
13585 }
13586 _ij3[0] = ij3; _ij3[1] = -1;
13587 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13588 {
13589 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13590 {
13591  j3valid[iij3]=false; _ij3[1] = iij3; break;
13592 }
13593 }
13594 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13595 {
13596 IkReal evalcond[8];
13597 IkReal x1049=IKcos(j3);
13598 IkReal x1050=IKsin(j3);
13599 IkReal x1051=((1.0)*gconst10);
13600 IkReal x1052=(gconst11*x1050);
13601 IkReal x1053=(gconst10*x1050);
13602 IkReal x1054=(gconst11*x1049);
13603 IkReal x1055=((1.0)*x1049);
13604 IkReal x1056=(x1049*x1051);
13605 evalcond[0]=(gconst11+((new_r00*x1049))+((new_r10*x1050)));
13606 evalcond[1]=(x1053+x1054+new_r00);
13607 evalcond[2]=(gconst10+((new_r00*x1050))+(((-1.0)*new_r10*x1055)));
13608 evalcond[3]=(gconst11+((new_r01*x1050))+(((-1.0)*new_r11*x1055)));
13609 evalcond[4]=((((-1.0)*x1056))+x1052+new_r01);
13610 evalcond[5]=((((-1.0)*x1056))+x1052+new_r10);
13611 evalcond[6]=((((-1.0)*x1051))+((new_r01*x1049))+((new_r11*x1050)));
13612 evalcond[7]=((((-1.0)*x1054))+new_r11+(((-1.0)*x1050*x1051)));
13613 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13614 {
13615 continue;
13616 }
13617 }
13618 
13619 {
13620 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13621 vinfos[0].jointtype = 1;
13622 vinfos[0].foffset = j0;
13623 vinfos[0].indices[0] = _ij0[0];
13624 vinfos[0].indices[1] = _ij0[1];
13625 vinfos[0].maxsolutions = _nj0;
13626 vinfos[1].jointtype = 1;
13627 vinfos[1].foffset = j1;
13628 vinfos[1].indices[0] = _ij1[0];
13629 vinfos[1].indices[1] = _ij1[1];
13630 vinfos[1].maxsolutions = _nj1;
13631 vinfos[2].jointtype = 1;
13632 vinfos[2].foffset = j2;
13633 vinfos[2].indices[0] = _ij2[0];
13634 vinfos[2].indices[1] = _ij2[1];
13635 vinfos[2].maxsolutions = _nj2;
13636 vinfos[3].jointtype = 1;
13637 vinfos[3].foffset = j3;
13638 vinfos[3].indices[0] = _ij3[0];
13639 vinfos[3].indices[1] = _ij3[1];
13640 vinfos[3].maxsolutions = _nj3;
13641 vinfos[4].jointtype = 1;
13642 vinfos[4].foffset = j4;
13643 vinfos[4].indices[0] = _ij4[0];
13644 vinfos[4].indices[1] = _ij4[1];
13645 vinfos[4].maxsolutions = _nj4;
13646 vinfos[5].jointtype = 1;
13647 vinfos[5].foffset = j5;
13648 vinfos[5].indices[0] = _ij5[0];
13649 vinfos[5].indices[1] = _ij5[1];
13650 vinfos[5].maxsolutions = _nj5;
13651 std::vector<int> vfree(0);
13652 solutions.AddSolution(vinfos,vfree);
13653 }
13654 }
13655 }
13656 
13657 }
13658 
13659 }
13660 
13661 } else
13662 {
13663 {
13664 IkReal j3array[1], cj3array[1], sj3array[1];
13665 bool j3valid[1]={false};
13666 _nj3 = 1;
13667 IkReal x1057=((1.0)*new_r10);
13668 CheckValue<IkReal> x1058 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r01)))),IkReal(((((-1.0)*new_r01*x1057))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13669 if(!x1058.valid){
13670 continue;
13671 }
13672 CheckValue<IkReal> x1059=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*x1057))+(((-1.0)*gconst11*new_r00)))),-1);
13673 if(!x1059.valid){
13674 continue;
13675 }
13676 j3array[0]=((-1.5707963267949)+(x1058.value)+(((1.5707963267949)*(x1059.value))));
13677 sj3array[0]=IKsin(j3array[0]);
13678 cj3array[0]=IKcos(j3array[0]);
13679 if( j3array[0] > IKPI )
13680 {
13681  j3array[0]-=IK2PI;
13682 }
13683 else if( j3array[0] < -IKPI )
13684 { j3array[0]+=IK2PI;
13685 }
13686 j3valid[0] = true;
13687 for(int ij3 = 0; ij3 < 1; ++ij3)
13688 {
13689 if( !j3valid[ij3] )
13690 {
13691  continue;
13692 }
13693 _ij3[0] = ij3; _ij3[1] = -1;
13694 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13695 {
13696 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13697 {
13698  j3valid[iij3]=false; _ij3[1] = iij3; break;
13699 }
13700 }
13701 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13702 {
13703 IkReal evalcond[8];
13704 IkReal x1060=IKcos(j3);
13705 IkReal x1061=IKsin(j3);
13706 IkReal x1062=((1.0)*gconst10);
13707 IkReal x1063=(gconst11*x1061);
13708 IkReal x1064=(gconst10*x1061);
13709 IkReal x1065=(gconst11*x1060);
13710 IkReal x1066=((1.0)*x1060);
13711 IkReal x1067=(x1060*x1062);
13712 evalcond[0]=(gconst11+((new_r00*x1060))+((new_r10*x1061)));
13713 evalcond[1]=(x1065+x1064+new_r00);
13714 evalcond[2]=((((-1.0)*new_r10*x1066))+gconst10+((new_r00*x1061)));
13715 evalcond[3]=(gconst11+((new_r01*x1061))+(((-1.0)*new_r11*x1066)));
13716 evalcond[4]=(x1063+new_r01+(((-1.0)*x1067)));
13717 evalcond[5]=(x1063+new_r10+(((-1.0)*x1067)));
13718 evalcond[6]=(((new_r01*x1060))+((new_r11*x1061))+(((-1.0)*x1062)));
13719 evalcond[7]=((((-1.0)*x1061*x1062))+new_r11+(((-1.0)*x1065)));
13720 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13721 {
13722 continue;
13723 }
13724 }
13725 
13726 {
13727 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13728 vinfos[0].jointtype = 1;
13729 vinfos[0].foffset = j0;
13730 vinfos[0].indices[0] = _ij0[0];
13731 vinfos[0].indices[1] = _ij0[1];
13732 vinfos[0].maxsolutions = _nj0;
13733 vinfos[1].jointtype = 1;
13734 vinfos[1].foffset = j1;
13735 vinfos[1].indices[0] = _ij1[0];
13736 vinfos[1].indices[1] = _ij1[1];
13737 vinfos[1].maxsolutions = _nj1;
13738 vinfos[2].jointtype = 1;
13739 vinfos[2].foffset = j2;
13740 vinfos[2].indices[0] = _ij2[0];
13741 vinfos[2].indices[1] = _ij2[1];
13742 vinfos[2].maxsolutions = _nj2;
13743 vinfos[3].jointtype = 1;
13744 vinfos[3].foffset = j3;
13745 vinfos[3].indices[0] = _ij3[0];
13746 vinfos[3].indices[1] = _ij3[1];
13747 vinfos[3].maxsolutions = _nj3;
13748 vinfos[4].jointtype = 1;
13749 vinfos[4].foffset = j4;
13750 vinfos[4].indices[0] = _ij4[0];
13751 vinfos[4].indices[1] = _ij4[1];
13752 vinfos[4].maxsolutions = _nj4;
13753 vinfos[5].jointtype = 1;
13754 vinfos[5].foffset = j5;
13755 vinfos[5].indices[0] = _ij5[0];
13756 vinfos[5].indices[1] = _ij5[1];
13757 vinfos[5].maxsolutions = _nj5;
13758 std::vector<int> vfree(0);
13759 solutions.AddSolution(vinfos,vfree);
13760 }
13761 }
13762 }
13763 
13764 }
13765 
13766 }
13767 
13768 } else
13769 {
13770 {
13771 IkReal j3array[1], cj3array[1], sj3array[1];
13772 bool j3valid[1]={false};
13773 _nj3 = 1;
13774 IkReal x1068=((1.0)*new_r10);
13775 CheckValue<IkReal> x1069=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1068)))),-1);
13776 if(!x1069.valid){
13777 continue;
13778 }
13779 CheckValue<IkReal> x1070 = IKatan2WithCheck(IkReal((((gconst11*new_r00))+((gconst11*new_r11)))),IkReal((((gconst11*new_r01))+(((-1.0)*gconst11*x1068)))),IKFAST_ATAN2_MAGTHRESH);
13780 if(!x1070.valid){
13781 continue;
13782 }
13783 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1069.value)))+(x1070.value));
13784 sj3array[0]=IKsin(j3array[0]);
13785 cj3array[0]=IKcos(j3array[0]);
13786 if( j3array[0] > IKPI )
13787 {
13788  j3array[0]-=IK2PI;
13789 }
13790 else if( j3array[0] < -IKPI )
13791 { j3array[0]+=IK2PI;
13792 }
13793 j3valid[0] = true;
13794 for(int ij3 = 0; ij3 < 1; ++ij3)
13795 {
13796 if( !j3valid[ij3] )
13797 {
13798  continue;
13799 }
13800 _ij3[0] = ij3; _ij3[1] = -1;
13801 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13802 {
13803 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13804 {
13805  j3valid[iij3]=false; _ij3[1] = iij3; break;
13806 }
13807 }
13808 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13809 {
13810 IkReal evalcond[8];
13811 IkReal x1071=IKcos(j3);
13812 IkReal x1072=IKsin(j3);
13813 IkReal x1073=((1.0)*gconst10);
13814 IkReal x1074=(gconst11*x1072);
13815 IkReal x1075=(gconst10*x1072);
13816 IkReal x1076=(gconst11*x1071);
13817 IkReal x1077=((1.0)*x1071);
13818 IkReal x1078=(x1071*x1073);
13819 evalcond[0]=(gconst11+((new_r00*x1071))+((new_r10*x1072)));
13820 evalcond[1]=(x1076+x1075+new_r00);
13821 evalcond[2]=(gconst10+((new_r00*x1072))+(((-1.0)*new_r10*x1077)));
13822 evalcond[3]=((((-1.0)*new_r11*x1077))+gconst11+((new_r01*x1072)));
13823 evalcond[4]=(x1074+new_r01+(((-1.0)*x1078)));
13824 evalcond[5]=(x1074+new_r10+(((-1.0)*x1078)));
13825 evalcond[6]=(((new_r01*x1071))+((new_r11*x1072))+(((-1.0)*x1073)));
13826 evalcond[7]=((((-1.0)*x1076))+(((-1.0)*x1072*x1073))+new_r11);
13827 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13828 {
13829 continue;
13830 }
13831 }
13832 
13833 {
13834 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13835 vinfos[0].jointtype = 1;
13836 vinfos[0].foffset = j0;
13837 vinfos[0].indices[0] = _ij0[0];
13838 vinfos[0].indices[1] = _ij0[1];
13839 vinfos[0].maxsolutions = _nj0;
13840 vinfos[1].jointtype = 1;
13841 vinfos[1].foffset = j1;
13842 vinfos[1].indices[0] = _ij1[0];
13843 vinfos[1].indices[1] = _ij1[1];
13844 vinfos[1].maxsolutions = _nj1;
13845 vinfos[2].jointtype = 1;
13846 vinfos[2].foffset = j2;
13847 vinfos[2].indices[0] = _ij2[0];
13848 vinfos[2].indices[1] = _ij2[1];
13849 vinfos[2].maxsolutions = _nj2;
13850 vinfos[3].jointtype = 1;
13851 vinfos[3].foffset = j3;
13852 vinfos[3].indices[0] = _ij3[0];
13853 vinfos[3].indices[1] = _ij3[1];
13854 vinfos[3].maxsolutions = _nj3;
13855 vinfos[4].jointtype = 1;
13856 vinfos[4].foffset = j4;
13857 vinfos[4].indices[0] = _ij4[0];
13858 vinfos[4].indices[1] = _ij4[1];
13859 vinfos[4].maxsolutions = _nj4;
13860 vinfos[5].jointtype = 1;
13861 vinfos[5].foffset = j5;
13862 vinfos[5].indices[0] = _ij5[0];
13863 vinfos[5].indices[1] = _ij5[1];
13864 vinfos[5].maxsolutions = _nj5;
13865 std::vector<int> vfree(0);
13866 solutions.AddSolution(vinfos,vfree);
13867 }
13868 }
13869 }
13870 
13871 }
13872 
13873 }
13874 
13875 }
13876 } while(0);
13877 if( bgotonextstatement )
13878 {
13879 bool bgotonextstatement = true;
13880 do
13881 {
13882 IkReal x1079=((-1.0)*new_r10);
13883 IkReal x1081 = ((new_r10*new_r10)+(new_r00*new_r00));
13884 if(IKabs(x1081)==0){
13885 continue;
13886 }
13887 IkReal x1080=pow(x1081,-0.5);
13888 CheckValue<IkReal> x1082 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1079),IKFAST_ATAN2_MAGTHRESH);
13889 if(!x1082.valid){
13890 continue;
13891 }
13892 IkReal gconst12=((-1.0)*(x1082.value));
13893 IkReal gconst13=(new_r00*x1080);
13894 IkReal gconst14=(x1079*x1080);
13895 CheckValue<IkReal> x1083 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
13896 if(!x1083.valid){
13897 continue;
13898 }
13899 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x1083.value)+j5)))), 6.28318530717959)));
13900 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13901 {
13902 bgotonextstatement=false;
13903 {
13904 IkReal j3eval[3];
13905 IkReal x1084=((-1.0)*new_r10);
13906 CheckValue<IkReal> x1087 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1084),IKFAST_ATAN2_MAGTHRESH);
13907 if(!x1087.valid){
13908 continue;
13909 }
13910 IkReal x1085=((-1.0)*(x1087.value));
13911 IkReal x1086=x1080;
13912 sj4=0;
13913 cj4=-1.0;
13914 j4=3.14159265358979;
13915 sj5=gconst13;
13916 cj5=gconst14;
13917 j5=x1085;
13918 IkReal gconst12=x1085;
13919 IkReal gconst13=(new_r00*x1086);
13920 IkReal gconst14=(x1084*x1086);
13921 IkReal x1088=new_r10*new_r10;
13922 IkReal x1089=((1.0)*new_r00);
13923 IkReal x1090=((1.0)*new_r10*new_r11);
13924 IkReal x1091=((((-1.0)*x1090))+(((-1.0)*new_r01*x1089)));
13925 IkReal x1092=x1080;
13926 IkReal x1093=(new_r10*x1092);
13927 j3eval[0]=x1091;
13928 j3eval[1]=((IKabs((((x1088*x1092))+(((-1.0)*new_r01*x1093)))))+(IKabs(((((-1.0)*x1090*x1092))+(((-1.0)*x1089*x1093))))));
13929 j3eval[2]=IKsign(x1091);
13930 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
13931 {
13932 {
13933 IkReal j3eval[1];
13934 IkReal x1094=((-1.0)*new_r10);
13935 CheckValue<IkReal> x1097 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1094),IKFAST_ATAN2_MAGTHRESH);
13936 if(!x1097.valid){
13937 continue;
13938 }
13939 IkReal x1095=((-1.0)*(x1097.value));
13940 IkReal x1096=x1080;
13941 sj4=0;
13942 cj4=-1.0;
13943 j4=3.14159265358979;
13944 sj5=gconst13;
13945 cj5=gconst14;
13946 j5=x1095;
13947 IkReal gconst12=x1095;
13948 IkReal gconst13=(new_r00*x1096);
13949 IkReal gconst14=(x1094*x1096);
13950 IkReal x1098=new_r10*new_r10;
13951 CheckValue<IkReal> x1101=IKPowWithIntegerCheck((x1098+(new_r00*new_r00)),-1);
13952 if(!x1101.valid){
13953 continue;
13954 }
13955 IkReal x1099=x1101.value;
13956 IkReal x1100=(new_r00*x1099);
13957 j3eval[0]=((IKabs((((new_r00*new_r11))+((x1098*x1099)))))+(IKabs((((new_r01*x1098*x1100))+((new_r10*x1100))+((new_r01*x1100*(new_r00*new_r00)))))));
13958 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13959 {
13960 {
13961 IkReal j3eval[1];
13962 IkReal x1102=((-1.0)*new_r10);
13963 CheckValue<IkReal> x1105 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1102),IKFAST_ATAN2_MAGTHRESH);
13964 if(!x1105.valid){
13965 continue;
13966 }
13967 IkReal x1103=((-1.0)*(x1105.value));
13968 IkReal x1104=x1080;
13969 sj4=0;
13970 cj4=-1.0;
13971 j4=3.14159265358979;
13972 sj5=gconst13;
13973 cj5=gconst14;
13974 j5=x1103;
13975 IkReal gconst12=x1103;
13976 IkReal gconst13=(new_r00*x1104);
13977 IkReal gconst14=(x1102*x1104);
13978 IkReal x1106=new_r10*new_r10;
13979 IkReal x1107=new_r00*new_r00;
13980 CheckValue<IkReal> x1111=IKPowWithIntegerCheck((x1106+x1107),-1);
13981 if(!x1111.valid){
13982 continue;
13983 }
13984 IkReal x1108=x1111.value;
13985 IkReal x1109=(new_r10*x1108);
13986 IkReal x1110=(x1106*x1108);
13987 j3eval[0]=((IKabs((x1110+(((-1.0)*x1108*(x1107*x1107)))+(((-1.0)*x1107*x1110)))))+(IKabs((((x1109*(new_r00*new_r00*new_r00)))+((new_r00*x1109))+((new_r00*x1109*(new_r10*new_r10)))))));
13988 if( IKabs(j3eval[0]) < 0.0000010000000000 )
13989 {
13990 {
13991 IkReal evalcond[2];
13992 bool bgotonextstatement = true;
13993 do
13994 {
13995 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
13996 if( IKabs(evalcond[0]) < 0.0000050000000000 )
13997 {
13998 bgotonextstatement=false;
13999 {
14000 IkReal j3eval[1];
14001 IkReal x1112=((-1.0)*new_r10);
14002 CheckValue<IkReal> x1114 = IKatan2WithCheck(IkReal(0),IkReal(x1112),IKFAST_ATAN2_MAGTHRESH);
14003 if(!x1114.valid){
14004 continue;
14005 }
14006 IkReal x1113=((-1.0)*(x1114.value));
14007 sj4=0;
14008 cj4=-1.0;
14009 j4=3.14159265358979;
14010 sj5=gconst13;
14011 cj5=gconst14;
14012 j5=x1113;
14013 new_r11=0;
14014 new_r00=0;
14015 IkReal gconst12=x1113;
14016 IkReal gconst13=0;
14017 IkReal x1115 = new_r10*new_r10;
14018 if(IKabs(x1115)==0){
14019 continue;
14020 }
14021 IkReal gconst14=(x1112*(pow(x1115,-0.5)));
14022 j3eval[0]=new_r01;
14023 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14024 {
14025 {
14026 IkReal j3array[2], cj3array[2], sj3array[2];
14027 bool j3valid[2]={false};
14028 _nj3 = 2;
14029 CheckValue<IkReal> x1116=IKPowWithIntegerCheck(gconst14,-1);
14030 if(!x1116.valid){
14031 continue;
14032 }
14033 sj3array[0]=((-1.0)*new_r01*(x1116.value));
14034 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14035 {
14036  j3valid[0] = j3valid[1] = true;
14037  j3array[0] = IKasin(sj3array[0]);
14038  cj3array[0] = IKcos(j3array[0]);
14039  sj3array[1] = sj3array[0];
14040  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14041  cj3array[1] = -cj3array[0];
14042 }
14043 else if( isnan(sj3array[0]) )
14044 {
14045  // probably any value will work
14046  j3valid[0] = true;
14047  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14048 }
14049 for(int ij3 = 0; ij3 < 2; ++ij3)
14050 {
14051 if( !j3valid[ij3] )
14052 {
14053  continue;
14054 }
14055 _ij3[0] = ij3; _ij3[1] = -1;
14056 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14057 {
14058 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14059 {
14060  j3valid[iij3]=false; _ij3[1] = iij3; break;
14061 }
14062 }
14063 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14064 {
14065 IkReal evalcond[6];
14066 IkReal x1117=IKcos(j3);
14067 IkReal x1118=IKsin(j3);
14068 evalcond[0]=(new_r01*x1117);
14069 evalcond[1]=(gconst14*x1117);
14070 evalcond[2]=((-1.0)*new_r10*x1117);
14071 evalcond[3]=(gconst14+((new_r01*x1118)));
14072 evalcond[4]=(gconst14+((new_r10*x1118)));
14073 evalcond[5]=(((gconst14*x1118))+new_r10);
14074 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14075 {
14076 continue;
14077 }
14078 }
14079 
14080 {
14081 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14082 vinfos[0].jointtype = 1;
14083 vinfos[0].foffset = j0;
14084 vinfos[0].indices[0] = _ij0[0];
14085 vinfos[0].indices[1] = _ij0[1];
14086 vinfos[0].maxsolutions = _nj0;
14087 vinfos[1].jointtype = 1;
14088 vinfos[1].foffset = j1;
14089 vinfos[1].indices[0] = _ij1[0];
14090 vinfos[1].indices[1] = _ij1[1];
14091 vinfos[1].maxsolutions = _nj1;
14092 vinfos[2].jointtype = 1;
14093 vinfos[2].foffset = j2;
14094 vinfos[2].indices[0] = _ij2[0];
14095 vinfos[2].indices[1] = _ij2[1];
14096 vinfos[2].maxsolutions = _nj2;
14097 vinfos[3].jointtype = 1;
14098 vinfos[3].foffset = j3;
14099 vinfos[3].indices[0] = _ij3[0];
14100 vinfos[3].indices[1] = _ij3[1];
14101 vinfos[3].maxsolutions = _nj3;
14102 vinfos[4].jointtype = 1;
14103 vinfos[4].foffset = j4;
14104 vinfos[4].indices[0] = _ij4[0];
14105 vinfos[4].indices[1] = _ij4[1];
14106 vinfos[4].maxsolutions = _nj4;
14107 vinfos[5].jointtype = 1;
14108 vinfos[5].foffset = j5;
14109 vinfos[5].indices[0] = _ij5[0];
14110 vinfos[5].indices[1] = _ij5[1];
14111 vinfos[5].maxsolutions = _nj5;
14112 std::vector<int> vfree(0);
14113 solutions.AddSolution(vinfos,vfree);
14114 }
14115 }
14116 }
14117 
14118 } else
14119 {
14120 {
14121 IkReal j3array[2], cj3array[2], sj3array[2];
14122 bool j3valid[2]={false};
14123 _nj3 = 2;
14124 CheckValue<IkReal> x1119=IKPowWithIntegerCheck(new_r01,-1);
14125 if(!x1119.valid){
14126 continue;
14127 }
14128 sj3array[0]=((-1.0)*gconst14*(x1119.value));
14129 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14130 {
14131  j3valid[0] = j3valid[1] = true;
14132  j3array[0] = IKasin(sj3array[0]);
14133  cj3array[0] = IKcos(j3array[0]);
14134  sj3array[1] = sj3array[0];
14135  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14136  cj3array[1] = -cj3array[0];
14137 }
14138 else if( isnan(sj3array[0]) )
14139 {
14140  // probably any value will work
14141  j3valid[0] = true;
14142  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14143 }
14144 for(int ij3 = 0; ij3 < 2; ++ij3)
14145 {
14146 if( !j3valid[ij3] )
14147 {
14148  continue;
14149 }
14150 _ij3[0] = ij3; _ij3[1] = -1;
14151 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14152 {
14153 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14154 {
14155  j3valid[iij3]=false; _ij3[1] = iij3; break;
14156 }
14157 }
14158 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14159 {
14160 IkReal evalcond[6];
14161 IkReal x1120=IKcos(j3);
14162 IkReal x1121=IKsin(j3);
14163 IkReal x1122=(gconst14*x1121);
14164 evalcond[0]=(new_r01*x1120);
14165 evalcond[1]=(gconst14*x1120);
14166 evalcond[2]=((-1.0)*new_r10*x1120);
14167 evalcond[3]=(x1122+new_r01);
14168 evalcond[4]=(gconst14+((new_r10*x1121)));
14169 evalcond[5]=(x1122+new_r10);
14170 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14171 {
14172 continue;
14173 }
14174 }
14175 
14176 {
14177 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14178 vinfos[0].jointtype = 1;
14179 vinfos[0].foffset = j0;
14180 vinfos[0].indices[0] = _ij0[0];
14181 vinfos[0].indices[1] = _ij0[1];
14182 vinfos[0].maxsolutions = _nj0;
14183 vinfos[1].jointtype = 1;
14184 vinfos[1].foffset = j1;
14185 vinfos[1].indices[0] = _ij1[0];
14186 vinfos[1].indices[1] = _ij1[1];
14187 vinfos[1].maxsolutions = _nj1;
14188 vinfos[2].jointtype = 1;
14189 vinfos[2].foffset = j2;
14190 vinfos[2].indices[0] = _ij2[0];
14191 vinfos[2].indices[1] = _ij2[1];
14192 vinfos[2].maxsolutions = _nj2;
14193 vinfos[3].jointtype = 1;
14194 vinfos[3].foffset = j3;
14195 vinfos[3].indices[0] = _ij3[0];
14196 vinfos[3].indices[1] = _ij3[1];
14197 vinfos[3].maxsolutions = _nj3;
14198 vinfos[4].jointtype = 1;
14199 vinfos[4].foffset = j4;
14200 vinfos[4].indices[0] = _ij4[0];
14201 vinfos[4].indices[1] = _ij4[1];
14202 vinfos[4].maxsolutions = _nj4;
14203 vinfos[5].jointtype = 1;
14204 vinfos[5].foffset = j5;
14205 vinfos[5].indices[0] = _ij5[0];
14206 vinfos[5].indices[1] = _ij5[1];
14207 vinfos[5].maxsolutions = _nj5;
14208 std::vector<int> vfree(0);
14209 solutions.AddSolution(vinfos,vfree);
14210 }
14211 }
14212 }
14213 
14214 }
14215 
14216 }
14217 
14218 }
14219 } while(0);
14220 if( bgotonextstatement )
14221 {
14222 bool bgotonextstatement = true;
14223 do
14224 {
14225 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14226 evalcond[1]=gconst14;
14227 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
14228 {
14229 bgotonextstatement=false;
14230 {
14231 IkReal j3eval[3];
14232 IkReal x1123=((-1.0)*new_r10);
14233 CheckValue<IkReal> x1125 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1123),IKFAST_ATAN2_MAGTHRESH);
14234 if(!x1125.valid){
14235 continue;
14236 }
14237 IkReal x1124=((-1.0)*(x1125.value));
14238 sj4=0;
14239 cj4=-1.0;
14240 j4=3.14159265358979;
14241 sj5=gconst13;
14242 cj5=gconst14;
14243 j5=x1124;
14244 new_r11=0;
14245 new_r01=0;
14246 new_r22=0;
14247 new_r20=0;
14248 IkReal gconst12=x1124;
14249 IkReal gconst13=new_r00;
14250 IkReal gconst14=x1123;
14251 j3eval[0]=-1.0;
14252 j3eval[1]=-1.0;
14253 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14254 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14255 {
14256 {
14257 IkReal j3eval[3];
14258 IkReal x1126=((-1.0)*new_r10);
14259 CheckValue<IkReal> x1128 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1126),IKFAST_ATAN2_MAGTHRESH);
14260 if(!x1128.valid){
14261 continue;
14262 }
14263 IkReal x1127=((-1.0)*(x1128.value));
14264 sj4=0;
14265 cj4=-1.0;
14266 j4=3.14159265358979;
14267 sj5=gconst13;
14268 cj5=gconst14;
14269 j5=x1127;
14270 new_r11=0;
14271 new_r01=0;
14272 new_r22=0;
14273 new_r20=0;
14274 IkReal gconst12=x1127;
14275 IkReal gconst13=new_r00;
14276 IkReal gconst14=x1126;
14277 j3eval[0]=-1.0;
14278 j3eval[1]=-1.0;
14279 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14280 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14281 {
14282 {
14283 IkReal j3eval[3];
14284 IkReal x1129=((-1.0)*new_r10);
14285 CheckValue<IkReal> x1131 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1129),IKFAST_ATAN2_MAGTHRESH);
14286 if(!x1131.valid){
14287 continue;
14288 }
14289 IkReal x1130=((-1.0)*(x1131.value));
14290 sj4=0;
14291 cj4=-1.0;
14292 j4=3.14159265358979;
14293 sj5=gconst13;
14294 cj5=gconst14;
14295 j5=x1130;
14296 new_r11=0;
14297 new_r01=0;
14298 new_r22=0;
14299 new_r20=0;
14300 IkReal gconst12=x1130;
14301 IkReal gconst13=new_r00;
14302 IkReal gconst14=x1129;
14303 j3eval[0]=1.0;
14304 j3eval[1]=1.0;
14305 j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
14306 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14307 {
14308 continue; // 3 cases reached
14309 
14310 } else
14311 {
14312 {
14313 IkReal j3array[1], cj3array[1], sj3array[1];
14314 bool j3valid[1]={false};
14315 _nj3 = 1;
14316 IkReal x1132=((1.0)*gconst14);
14317 CheckValue<IkReal> x1133=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x1132))+((gconst13*new_r00)))),-1);
14318 if(!x1133.valid){
14319 continue;
14320 }
14321 CheckValue<IkReal> x1134 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal(((((-1.0)*gconst13*x1132))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
14322 if(!x1134.valid){
14323 continue;
14324 }
14325 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1133.value)))+(x1134.value));
14326 sj3array[0]=IKsin(j3array[0]);
14327 cj3array[0]=IKcos(j3array[0]);
14328 if( j3array[0] > IKPI )
14329 {
14330  j3array[0]-=IK2PI;
14331 }
14332 else if( j3array[0] < -IKPI )
14333 { j3array[0]+=IK2PI;
14334 }
14335 j3valid[0] = true;
14336 for(int ij3 = 0; ij3 < 1; ++ij3)
14337 {
14338 if( !j3valid[ij3] )
14339 {
14340  continue;
14341 }
14342 _ij3[0] = ij3; _ij3[1] = -1;
14343 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14344 {
14345 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14346 {
14347  j3valid[iij3]=false; _ij3[1] = iij3; break;
14348 }
14349 }
14350 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14351 {
14352 IkReal evalcond[6];
14353 IkReal x1135=IKsin(j3);
14354 IkReal x1136=IKcos(j3);
14355 IkReal x1137=((1.0)*gconst13);
14356 IkReal x1138=(gconst14*x1135);
14357 IkReal x1139=(gconst14*x1136);
14358 IkReal x1140=(x1136*x1137);
14359 evalcond[0]=(x1138+(((-1.0)*x1140)));
14360 evalcond[1]=(gconst14+((new_r00*x1136))+((new_r10*x1135)));
14361 evalcond[2]=(x1139+((gconst13*x1135))+new_r00);
14362 evalcond[3]=(gconst13+((new_r00*x1135))+(((-1.0)*new_r10*x1136)));
14363 evalcond[4]=((((-1.0)*x1139))+(((-1.0)*x1135*x1137)));
14364 evalcond[5]=(x1138+(((-1.0)*x1140))+new_r10);
14365 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14366 {
14367 continue;
14368 }
14369 }
14370 
14371 {
14372 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14373 vinfos[0].jointtype = 1;
14374 vinfos[0].foffset = j0;
14375 vinfos[0].indices[0] = _ij0[0];
14376 vinfos[0].indices[1] = _ij0[1];
14377 vinfos[0].maxsolutions = _nj0;
14378 vinfos[1].jointtype = 1;
14379 vinfos[1].foffset = j1;
14380 vinfos[1].indices[0] = _ij1[0];
14381 vinfos[1].indices[1] = _ij1[1];
14382 vinfos[1].maxsolutions = _nj1;
14383 vinfos[2].jointtype = 1;
14384 vinfos[2].foffset = j2;
14385 vinfos[2].indices[0] = _ij2[0];
14386 vinfos[2].indices[1] = _ij2[1];
14387 vinfos[2].maxsolutions = _nj2;
14388 vinfos[3].jointtype = 1;
14389 vinfos[3].foffset = j3;
14390 vinfos[3].indices[0] = _ij3[0];
14391 vinfos[3].indices[1] = _ij3[1];
14392 vinfos[3].maxsolutions = _nj3;
14393 vinfos[4].jointtype = 1;
14394 vinfos[4].foffset = j4;
14395 vinfos[4].indices[0] = _ij4[0];
14396 vinfos[4].indices[1] = _ij4[1];
14397 vinfos[4].maxsolutions = _nj4;
14398 vinfos[5].jointtype = 1;
14399 vinfos[5].foffset = j5;
14400 vinfos[5].indices[0] = _ij5[0];
14401 vinfos[5].indices[1] = _ij5[1];
14402 vinfos[5].maxsolutions = _nj5;
14403 std::vector<int> vfree(0);
14404 solutions.AddSolution(vinfos,vfree);
14405 }
14406 }
14407 }
14408 
14409 }
14410 
14411 }
14412 
14413 } else
14414 {
14415 {
14416 IkReal j3array[1], cj3array[1], sj3array[1];
14417 bool j3valid[1]={false};
14418 _nj3 = 1;
14419 CheckValue<IkReal> x1141=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst14*gconst14)))+(((-1.0)*(gconst13*gconst13))))),-1);
14420 if(!x1141.valid){
14421 continue;
14422 }
14423 CheckValue<IkReal> x1142 = IKatan2WithCheck(IkReal((gconst13*new_r00)),IkReal((gconst14*new_r00)),IKFAST_ATAN2_MAGTHRESH);
14424 if(!x1142.valid){
14425 continue;
14426 }
14427 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1141.value)))+(x1142.value));
14428 sj3array[0]=IKsin(j3array[0]);
14429 cj3array[0]=IKcos(j3array[0]);
14430 if( j3array[0] > IKPI )
14431 {
14432  j3array[0]-=IK2PI;
14433 }
14434 else if( j3array[0] < -IKPI )
14435 { j3array[0]+=IK2PI;
14436 }
14437 j3valid[0] = true;
14438 for(int ij3 = 0; ij3 < 1; ++ij3)
14439 {
14440 if( !j3valid[ij3] )
14441 {
14442  continue;
14443 }
14444 _ij3[0] = ij3; _ij3[1] = -1;
14445 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14446 {
14447 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14448 {
14449  j3valid[iij3]=false; _ij3[1] = iij3; break;
14450 }
14451 }
14452 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14453 {
14454 IkReal evalcond[6];
14455 IkReal x1143=IKsin(j3);
14456 IkReal x1144=IKcos(j3);
14457 IkReal x1145=((1.0)*gconst13);
14458 IkReal x1146=(gconst14*x1143);
14459 IkReal x1147=(gconst14*x1144);
14460 IkReal x1148=(x1144*x1145);
14461 evalcond[0]=(x1146+(((-1.0)*x1148)));
14462 evalcond[1]=(((new_r00*x1144))+gconst14+((new_r10*x1143)));
14463 evalcond[2]=(x1147+((gconst13*x1143))+new_r00);
14464 evalcond[3]=((((-1.0)*new_r10*x1144))+((new_r00*x1143))+gconst13);
14465 evalcond[4]=((((-1.0)*x1143*x1145))+(((-1.0)*x1147)));
14466 evalcond[5]=(x1146+(((-1.0)*x1148))+new_r10);
14467 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14468 {
14469 continue;
14470 }
14471 }
14472 
14473 {
14474 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14475 vinfos[0].jointtype = 1;
14476 vinfos[0].foffset = j0;
14477 vinfos[0].indices[0] = _ij0[0];
14478 vinfos[0].indices[1] = _ij0[1];
14479 vinfos[0].maxsolutions = _nj0;
14480 vinfos[1].jointtype = 1;
14481 vinfos[1].foffset = j1;
14482 vinfos[1].indices[0] = _ij1[0];
14483 vinfos[1].indices[1] = _ij1[1];
14484 vinfos[1].maxsolutions = _nj1;
14485 vinfos[2].jointtype = 1;
14486 vinfos[2].foffset = j2;
14487 vinfos[2].indices[0] = _ij2[0];
14488 vinfos[2].indices[1] = _ij2[1];
14489 vinfos[2].maxsolutions = _nj2;
14490 vinfos[3].jointtype = 1;
14491 vinfos[3].foffset = j3;
14492 vinfos[3].indices[0] = _ij3[0];
14493 vinfos[3].indices[1] = _ij3[1];
14494 vinfos[3].maxsolutions = _nj3;
14495 vinfos[4].jointtype = 1;
14496 vinfos[4].foffset = j4;
14497 vinfos[4].indices[0] = _ij4[0];
14498 vinfos[4].indices[1] = _ij4[1];
14499 vinfos[4].maxsolutions = _nj4;
14500 vinfos[5].jointtype = 1;
14501 vinfos[5].foffset = j5;
14502 vinfos[5].indices[0] = _ij5[0];
14503 vinfos[5].indices[1] = _ij5[1];
14504 vinfos[5].maxsolutions = _nj5;
14505 std::vector<int> vfree(0);
14506 solutions.AddSolution(vinfos,vfree);
14507 }
14508 }
14509 }
14510 
14511 }
14512 
14513 }
14514 
14515 } else
14516 {
14517 {
14518 IkReal j3array[1], cj3array[1], sj3array[1];
14519 bool j3valid[1]={false};
14520 _nj3 = 1;
14521 CheckValue<IkReal> x1149 = IKatan2WithCheck(IkReal(gconst13*gconst13),IkReal((gconst13*gconst14)),IKFAST_ATAN2_MAGTHRESH);
14522 if(!x1149.valid){
14523 continue;
14524 }
14525 CheckValue<IkReal> x1150=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r00))+((gconst14*new_r10)))),-1);
14526 if(!x1150.valid){
14527 continue;
14528 }
14529 j3array[0]=((-1.5707963267949)+(x1149.value)+(((1.5707963267949)*(x1150.value))));
14530 sj3array[0]=IKsin(j3array[0]);
14531 cj3array[0]=IKcos(j3array[0]);
14532 if( j3array[0] > IKPI )
14533 {
14534  j3array[0]-=IK2PI;
14535 }
14536 else if( j3array[0] < -IKPI )
14537 { j3array[0]+=IK2PI;
14538 }
14539 j3valid[0] = true;
14540 for(int ij3 = 0; ij3 < 1; ++ij3)
14541 {
14542 if( !j3valid[ij3] )
14543 {
14544  continue;
14545 }
14546 _ij3[0] = ij3; _ij3[1] = -1;
14547 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14548 {
14549 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14550 {
14551  j3valid[iij3]=false; _ij3[1] = iij3; break;
14552 }
14553 }
14554 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14555 {
14556 IkReal evalcond[6];
14557 IkReal x1151=IKsin(j3);
14558 IkReal x1152=IKcos(j3);
14559 IkReal x1153=((1.0)*gconst13);
14560 IkReal x1154=(gconst14*x1151);
14561 IkReal x1155=(gconst14*x1152);
14562 IkReal x1156=(x1152*x1153);
14563 evalcond[0]=(x1154+(((-1.0)*x1156)));
14564 evalcond[1]=(((new_r10*x1151))+gconst14+((new_r00*x1152)));
14565 evalcond[2]=(x1155+((gconst13*x1151))+new_r00);
14566 evalcond[3]=(gconst13+((new_r00*x1151))+(((-1.0)*new_r10*x1152)));
14567 evalcond[4]=((((-1.0)*x1155))+(((-1.0)*x1151*x1153)));
14568 evalcond[5]=(x1154+(((-1.0)*x1156))+new_r10);
14569 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14570 {
14571 continue;
14572 }
14573 }
14574 
14575 {
14576 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14577 vinfos[0].jointtype = 1;
14578 vinfos[0].foffset = j0;
14579 vinfos[0].indices[0] = _ij0[0];
14580 vinfos[0].indices[1] = _ij0[1];
14581 vinfos[0].maxsolutions = _nj0;
14582 vinfos[1].jointtype = 1;
14583 vinfos[1].foffset = j1;
14584 vinfos[1].indices[0] = _ij1[0];
14585 vinfos[1].indices[1] = _ij1[1];
14586 vinfos[1].maxsolutions = _nj1;
14587 vinfos[2].jointtype = 1;
14588 vinfos[2].foffset = j2;
14589 vinfos[2].indices[0] = _ij2[0];
14590 vinfos[2].indices[1] = _ij2[1];
14591 vinfos[2].maxsolutions = _nj2;
14592 vinfos[3].jointtype = 1;
14593 vinfos[3].foffset = j3;
14594 vinfos[3].indices[0] = _ij3[0];
14595 vinfos[3].indices[1] = _ij3[1];
14596 vinfos[3].maxsolutions = _nj3;
14597 vinfos[4].jointtype = 1;
14598 vinfos[4].foffset = j4;
14599 vinfos[4].indices[0] = _ij4[0];
14600 vinfos[4].indices[1] = _ij4[1];
14601 vinfos[4].maxsolutions = _nj4;
14602 vinfos[5].jointtype = 1;
14603 vinfos[5].foffset = j5;
14604 vinfos[5].indices[0] = _ij5[0];
14605 vinfos[5].indices[1] = _ij5[1];
14606 vinfos[5].maxsolutions = _nj5;
14607 std::vector<int> vfree(0);
14608 solutions.AddSolution(vinfos,vfree);
14609 }
14610 }
14611 }
14612 
14613 }
14614 
14615 }
14616 
14617 }
14618 } while(0);
14619 if( bgotonextstatement )
14620 {
14621 bool bgotonextstatement = true;
14622 do
14623 {
14624 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
14625 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14626 {
14627 bgotonextstatement=false;
14628 {
14629 IkReal j3array[2], cj3array[2], sj3array[2];
14630 bool j3valid[2]={false};
14631 _nj3 = 2;
14632 CheckValue<IkReal> x1157=IKPowWithIntegerCheck(gconst13,-1);
14633 if(!x1157.valid){
14634 continue;
14635 }
14636 sj3array[0]=(new_r11*(x1157.value));
14637 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14638 {
14639  j3valid[0] = j3valid[1] = true;
14640  j3array[0] = IKasin(sj3array[0]);
14641  cj3array[0] = IKcos(j3array[0]);
14642  sj3array[1] = sj3array[0];
14643  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14644  cj3array[1] = -cj3array[0];
14645 }
14646 else if( isnan(sj3array[0]) )
14647 {
14648  // probably any value will work
14649  j3valid[0] = true;
14650  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14651 }
14652 for(int ij3 = 0; ij3 < 2; ++ij3)
14653 {
14654 if( !j3valid[ij3] )
14655 {
14656  continue;
14657 }
14658 _ij3[0] = ij3; _ij3[1] = -1;
14659 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14660 {
14661 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14662 {
14663  j3valid[iij3]=false; _ij3[1] = iij3; break;
14664 }
14665 }
14666 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14667 {
14668 IkReal evalcond[6];
14669 IkReal x1158=IKcos(j3);
14670 IkReal x1159=IKsin(j3);
14671 IkReal x1160=((-1.0)*x1158);
14672 evalcond[0]=(new_r00*x1158);
14673 evalcond[1]=(new_r11*x1160);
14674 evalcond[2]=(gconst13*x1160);
14675 evalcond[3]=(gconst13+((new_r00*x1159)));
14676 evalcond[4]=(((gconst13*x1159))+new_r00);
14677 evalcond[5]=(((new_r11*x1159))+(((-1.0)*gconst13)));
14678 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14679 {
14680 continue;
14681 }
14682 }
14683 
14684 {
14685 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14686 vinfos[0].jointtype = 1;
14687 vinfos[0].foffset = j0;
14688 vinfos[0].indices[0] = _ij0[0];
14689 vinfos[0].indices[1] = _ij0[1];
14690 vinfos[0].maxsolutions = _nj0;
14691 vinfos[1].jointtype = 1;
14692 vinfos[1].foffset = j1;
14693 vinfos[1].indices[0] = _ij1[0];
14694 vinfos[1].indices[1] = _ij1[1];
14695 vinfos[1].maxsolutions = _nj1;
14696 vinfos[2].jointtype = 1;
14697 vinfos[2].foffset = j2;
14698 vinfos[2].indices[0] = _ij2[0];
14699 vinfos[2].indices[1] = _ij2[1];
14700 vinfos[2].maxsolutions = _nj2;
14701 vinfos[3].jointtype = 1;
14702 vinfos[3].foffset = j3;
14703 vinfos[3].indices[0] = _ij3[0];
14704 vinfos[3].indices[1] = _ij3[1];
14705 vinfos[3].maxsolutions = _nj3;
14706 vinfos[4].jointtype = 1;
14707 vinfos[4].foffset = j4;
14708 vinfos[4].indices[0] = _ij4[0];
14709 vinfos[4].indices[1] = _ij4[1];
14710 vinfos[4].maxsolutions = _nj4;
14711 vinfos[5].jointtype = 1;
14712 vinfos[5].foffset = j5;
14713 vinfos[5].indices[0] = _ij5[0];
14714 vinfos[5].indices[1] = _ij5[1];
14715 vinfos[5].maxsolutions = _nj5;
14716 std::vector<int> vfree(0);
14717 solutions.AddSolution(vinfos,vfree);
14718 }
14719 }
14720 }
14721 
14722 }
14723 } while(0);
14724 if( bgotonextstatement )
14725 {
14726 bool bgotonextstatement = true;
14727 do
14728 {
14729 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
14730 if( IKabs(evalcond[0]) < 0.0000050000000000 )
14731 {
14732 bgotonextstatement=false;
14733 {
14734 IkReal j3eval[1];
14735 CheckValue<IkReal> x1162 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14736 if(!x1162.valid){
14737 continue;
14738 }
14739 IkReal x1161=((-1.0)*(x1162.value));
14740 sj4=0;
14741 cj4=-1.0;
14742 j4=3.14159265358979;
14743 sj5=gconst13;
14744 cj5=gconst14;
14745 j5=x1161;
14746 new_r11=0;
14747 new_r10=0;
14748 new_r22=0;
14749 new_r02=0;
14750 IkReal gconst12=x1161;
14751 IkReal x1163 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14752 if(IKabs(x1163)==0){
14753 continue;
14754 }
14755 IkReal gconst13=(new_r00*(pow(x1163,-0.5)));
14756 IkReal gconst14=0;
14757 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
14758 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14759 {
14760 {
14761 IkReal j3eval[1];
14762 CheckValue<IkReal> x1165 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14763 if(!x1165.valid){
14764 continue;
14765 }
14766 IkReal x1164=((-1.0)*(x1165.value));
14767 sj4=0;
14768 cj4=-1.0;
14769 j4=3.14159265358979;
14770 sj5=gconst13;
14771 cj5=gconst14;
14772 j5=x1164;
14773 new_r11=0;
14774 new_r10=0;
14775 new_r22=0;
14776 new_r02=0;
14777 IkReal gconst12=x1164;
14778 IkReal x1166 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14779 if(IKabs(x1166)==0){
14780 continue;
14781 }
14782 IkReal gconst13=(new_r00*(pow(x1166,-0.5)));
14783 IkReal gconst14=0;
14784 j3eval[0]=new_r00;
14785 if( IKabs(j3eval[0]) < 0.0000010000000000 )
14786 {
14787 {
14788 IkReal j3eval[2];
14789 CheckValue<IkReal> x1168 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14790 if(!x1168.valid){
14791 continue;
14792 }
14793 IkReal x1167=((-1.0)*(x1168.value));
14794 sj4=0;
14795 cj4=-1.0;
14796 j4=3.14159265358979;
14797 sj5=gconst13;
14798 cj5=gconst14;
14799 j5=x1167;
14800 new_r11=0;
14801 new_r10=0;
14802 new_r22=0;
14803 new_r02=0;
14804 IkReal gconst12=x1167;
14805 IkReal x1169 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14806 if(IKabs(x1169)==0){
14807 continue;
14808 }
14809 IkReal gconst13=(new_r00*(pow(x1169,-0.5)));
14810 IkReal gconst14=0;
14811 j3eval[0]=new_r00;
14812 j3eval[1]=new_r01;
14813 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
14814 {
14815 continue; // 3 cases reached
14816 
14817 } else
14818 {
14819 {
14820 IkReal j3array[1], cj3array[1], sj3array[1];
14821 bool j3valid[1]={false};
14822 _nj3 = 1;
14823 CheckValue<IkReal> x1170=IKPowWithIntegerCheck(new_r00,-1);
14824 if(!x1170.valid){
14825 continue;
14826 }
14827 CheckValue<IkReal> x1171=IKPowWithIntegerCheck(new_r01,-1);
14828 if(!x1171.valid){
14829 continue;
14830 }
14831 if( IKabs(((-1.0)*gconst13*(x1170.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst13*(x1171.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1170.value)))+IKsqr((gconst13*(x1171.value)))-1) <= IKFAST_SINCOS_THRESH )
14832  continue;
14833 j3array[0]=IKatan2(((-1.0)*gconst13*(x1170.value)), (gconst13*(x1171.value)));
14834 sj3array[0]=IKsin(j3array[0]);
14835 cj3array[0]=IKcos(j3array[0]);
14836 if( j3array[0] > IKPI )
14837 {
14838  j3array[0]-=IK2PI;
14839 }
14840 else if( j3array[0] < -IKPI )
14841 { j3array[0]+=IK2PI;
14842 }
14843 j3valid[0] = true;
14844 for(int ij3 = 0; ij3 < 1; ++ij3)
14845 {
14846 if( !j3valid[ij3] )
14847 {
14848  continue;
14849 }
14850 _ij3[0] = ij3; _ij3[1] = -1;
14851 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14852 {
14853 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14854 {
14855  j3valid[iij3]=false; _ij3[1] = iij3; break;
14856 }
14857 }
14858 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14859 {
14860 IkReal evalcond[8];
14861 IkReal x1172=IKsin(j3);
14862 IkReal x1173=IKcos(j3);
14863 IkReal x1174=(gconst13*x1173);
14864 IkReal x1175=(gconst13*x1172);
14865 evalcond[0]=(new_r01*x1172);
14866 evalcond[1]=(new_r00*x1173);
14867 evalcond[2]=((-1.0)*x1175);
14868 evalcond[3]=((-1.0)*x1174);
14869 evalcond[4]=(gconst13+((new_r00*x1172)));
14870 evalcond[5]=(x1175+new_r00);
14871 evalcond[6]=(new_r01+(((-1.0)*x1174)));
14872 evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1173)));
14873 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14874 {
14875 continue;
14876 }
14877 }
14878 
14879 {
14880 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14881 vinfos[0].jointtype = 1;
14882 vinfos[0].foffset = j0;
14883 vinfos[0].indices[0] = _ij0[0];
14884 vinfos[0].indices[1] = _ij0[1];
14885 vinfos[0].maxsolutions = _nj0;
14886 vinfos[1].jointtype = 1;
14887 vinfos[1].foffset = j1;
14888 vinfos[1].indices[0] = _ij1[0];
14889 vinfos[1].indices[1] = _ij1[1];
14890 vinfos[1].maxsolutions = _nj1;
14891 vinfos[2].jointtype = 1;
14892 vinfos[2].foffset = j2;
14893 vinfos[2].indices[0] = _ij2[0];
14894 vinfos[2].indices[1] = _ij2[1];
14895 vinfos[2].maxsolutions = _nj2;
14896 vinfos[3].jointtype = 1;
14897 vinfos[3].foffset = j3;
14898 vinfos[3].indices[0] = _ij3[0];
14899 vinfos[3].indices[1] = _ij3[1];
14900 vinfos[3].maxsolutions = _nj3;
14901 vinfos[4].jointtype = 1;
14902 vinfos[4].foffset = j4;
14903 vinfos[4].indices[0] = _ij4[0];
14904 vinfos[4].indices[1] = _ij4[1];
14905 vinfos[4].maxsolutions = _nj4;
14906 vinfos[5].jointtype = 1;
14907 vinfos[5].foffset = j5;
14908 vinfos[5].indices[0] = _ij5[0];
14909 vinfos[5].indices[1] = _ij5[1];
14910 vinfos[5].maxsolutions = _nj5;
14911 std::vector<int> vfree(0);
14912 solutions.AddSolution(vinfos,vfree);
14913 }
14914 }
14915 }
14916 
14917 }
14918 
14919 }
14920 
14921 } else
14922 {
14923 {
14924 IkReal j3array[1], cj3array[1], sj3array[1];
14925 bool j3valid[1]={false};
14926 _nj3 = 1;
14927 CheckValue<IkReal> x1176=IKPowWithIntegerCheck(new_r00,-1);
14928 if(!x1176.valid){
14929 continue;
14930 }
14931 CheckValue<IkReal> x1177=IKPowWithIntegerCheck(gconst13,-1);
14932 if(!x1177.valid){
14933 continue;
14934 }
14935 if( IKabs(((-1.0)*gconst13*(x1176.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1177.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1176.value)))+IKsqr((new_r01*(x1177.value)))-1) <= IKFAST_SINCOS_THRESH )
14936  continue;
14937 j3array[0]=IKatan2(((-1.0)*gconst13*(x1176.value)), (new_r01*(x1177.value)));
14938 sj3array[0]=IKsin(j3array[0]);
14939 cj3array[0]=IKcos(j3array[0]);
14940 if( j3array[0] > IKPI )
14941 {
14942  j3array[0]-=IK2PI;
14943 }
14944 else if( j3array[0] < -IKPI )
14945 { j3array[0]+=IK2PI;
14946 }
14947 j3valid[0] = true;
14948 for(int ij3 = 0; ij3 < 1; ++ij3)
14949 {
14950 if( !j3valid[ij3] )
14951 {
14952  continue;
14953 }
14954 _ij3[0] = ij3; _ij3[1] = -1;
14955 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14956 {
14957 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14958 {
14959  j3valid[iij3]=false; _ij3[1] = iij3; break;
14960 }
14961 }
14962 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14963 {
14964 IkReal evalcond[8];
14965 IkReal x1178=IKsin(j3);
14966 IkReal x1179=IKcos(j3);
14967 IkReal x1180=(gconst13*x1179);
14968 IkReal x1181=(gconst13*x1178);
14969 evalcond[0]=(new_r01*x1178);
14970 evalcond[1]=(new_r00*x1179);
14971 evalcond[2]=((-1.0)*x1181);
14972 evalcond[3]=((-1.0)*x1180);
14973 evalcond[4]=(gconst13+((new_r00*x1178)));
14974 evalcond[5]=(x1181+new_r00);
14975 evalcond[6]=(new_r01+(((-1.0)*x1180)));
14976 evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1179)));
14977 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14978 {
14979 continue;
14980 }
14981 }
14982 
14983 {
14984 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14985 vinfos[0].jointtype = 1;
14986 vinfos[0].foffset = j0;
14987 vinfos[0].indices[0] = _ij0[0];
14988 vinfos[0].indices[1] = _ij0[1];
14989 vinfos[0].maxsolutions = _nj0;
14990 vinfos[1].jointtype = 1;
14991 vinfos[1].foffset = j1;
14992 vinfos[1].indices[0] = _ij1[0];
14993 vinfos[1].indices[1] = _ij1[1];
14994 vinfos[1].maxsolutions = _nj1;
14995 vinfos[2].jointtype = 1;
14996 vinfos[2].foffset = j2;
14997 vinfos[2].indices[0] = _ij2[0];
14998 vinfos[2].indices[1] = _ij2[1];
14999 vinfos[2].maxsolutions = _nj2;
15000 vinfos[3].jointtype = 1;
15001 vinfos[3].foffset = j3;
15002 vinfos[3].indices[0] = _ij3[0];
15003 vinfos[3].indices[1] = _ij3[1];
15004 vinfos[3].maxsolutions = _nj3;
15005 vinfos[4].jointtype = 1;
15006 vinfos[4].foffset = j4;
15007 vinfos[4].indices[0] = _ij4[0];
15008 vinfos[4].indices[1] = _ij4[1];
15009 vinfos[4].maxsolutions = _nj4;
15010 vinfos[5].jointtype = 1;
15011 vinfos[5].foffset = j5;
15012 vinfos[5].indices[0] = _ij5[0];
15013 vinfos[5].indices[1] = _ij5[1];
15014 vinfos[5].maxsolutions = _nj5;
15015 std::vector<int> vfree(0);
15016 solutions.AddSolution(vinfos,vfree);
15017 }
15018 }
15019 }
15020 
15021 }
15022 
15023 }
15024 
15025 } else
15026 {
15027 {
15028 IkReal j3array[1], cj3array[1], sj3array[1];
15029 bool j3valid[1]={false};
15030 _nj3 = 1;
15031 CheckValue<IkReal> x1182 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15032 if(!x1182.valid){
15033 continue;
15034 }
15035 CheckValue<IkReal> x1183=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15036 if(!x1183.valid){
15037 continue;
15038 }
15039 j3array[0]=((-1.5707963267949)+(x1182.value)+(((1.5707963267949)*(x1183.value))));
15040 sj3array[0]=IKsin(j3array[0]);
15041 cj3array[0]=IKcos(j3array[0]);
15042 if( j3array[0] > IKPI )
15043 {
15044  j3array[0]-=IK2PI;
15045 }
15046 else if( j3array[0] < -IKPI )
15047 { j3array[0]+=IK2PI;
15048 }
15049 j3valid[0] = true;
15050 for(int ij3 = 0; ij3 < 1; ++ij3)
15051 {
15052 if( !j3valid[ij3] )
15053 {
15054  continue;
15055 }
15056 _ij3[0] = ij3; _ij3[1] = -1;
15057 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15058 {
15059 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15060 {
15061  j3valid[iij3]=false; _ij3[1] = iij3; break;
15062 }
15063 }
15064 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15065 {
15066 IkReal evalcond[8];
15067 IkReal x1184=IKsin(j3);
15068 IkReal x1185=IKcos(j3);
15069 IkReal x1186=(gconst13*x1185);
15070 IkReal x1187=(gconst13*x1184);
15071 evalcond[0]=(new_r01*x1184);
15072 evalcond[1]=(new_r00*x1185);
15073 evalcond[2]=((-1.0)*x1187);
15074 evalcond[3]=((-1.0)*x1186);
15075 evalcond[4]=(gconst13+((new_r00*x1184)));
15076 evalcond[5]=(x1187+new_r00);
15077 evalcond[6]=(new_r01+(((-1.0)*x1186)));
15078 evalcond[7]=(((new_r01*x1185))+(((-1.0)*gconst13)));
15079 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15080 {
15081 continue;
15082 }
15083 }
15084 
15085 {
15086 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15087 vinfos[0].jointtype = 1;
15088 vinfos[0].foffset = j0;
15089 vinfos[0].indices[0] = _ij0[0];
15090 vinfos[0].indices[1] = _ij0[1];
15091 vinfos[0].maxsolutions = _nj0;
15092 vinfos[1].jointtype = 1;
15093 vinfos[1].foffset = j1;
15094 vinfos[1].indices[0] = _ij1[0];
15095 vinfos[1].indices[1] = _ij1[1];
15096 vinfos[1].maxsolutions = _nj1;
15097 vinfos[2].jointtype = 1;
15098 vinfos[2].foffset = j2;
15099 vinfos[2].indices[0] = _ij2[0];
15100 vinfos[2].indices[1] = _ij2[1];
15101 vinfos[2].maxsolutions = _nj2;
15102 vinfos[3].jointtype = 1;
15103 vinfos[3].foffset = j3;
15104 vinfos[3].indices[0] = _ij3[0];
15105 vinfos[3].indices[1] = _ij3[1];
15106 vinfos[3].maxsolutions = _nj3;
15107 vinfos[4].jointtype = 1;
15108 vinfos[4].foffset = j4;
15109 vinfos[4].indices[0] = _ij4[0];
15110 vinfos[4].indices[1] = _ij4[1];
15111 vinfos[4].maxsolutions = _nj4;
15112 vinfos[5].jointtype = 1;
15113 vinfos[5].foffset = j5;
15114 vinfos[5].indices[0] = _ij5[0];
15115 vinfos[5].indices[1] = _ij5[1];
15116 vinfos[5].maxsolutions = _nj5;
15117 std::vector<int> vfree(0);
15118 solutions.AddSolution(vinfos,vfree);
15119 }
15120 }
15121 }
15122 
15123 }
15124 
15125 }
15126 
15127 }
15128 } while(0);
15129 if( bgotonextstatement )
15130 {
15131 bool bgotonextstatement = true;
15132 do
15133 {
15134 evalcond[0]=IKabs(new_r10);
15135 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15136 {
15137 bgotonextstatement=false;
15138 {
15139 IkReal j3eval[1];
15140 CheckValue<IkReal> x1189 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15141 if(!x1189.valid){
15142 continue;
15143 }
15144 IkReal x1188=((-1.0)*(x1189.value));
15145 sj4=0;
15146 cj4=-1.0;
15147 j4=3.14159265358979;
15148 sj5=gconst13;
15149 cj5=gconst14;
15150 j5=x1188;
15151 new_r10=0;
15152 IkReal gconst12=x1188;
15153 IkReal x1190 = new_r00*new_r00;
15154 if(IKabs(x1190)==0){
15155 continue;
15156 }
15157 IkReal gconst13=(new_r00*(pow(x1190,-0.5)));
15158 IkReal gconst14=0;
15159 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
15160 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15161 {
15162 {
15163 IkReal j3eval[1];
15164 CheckValue<IkReal> x1192 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15165 if(!x1192.valid){
15166 continue;
15167 }
15168 IkReal x1191=((-1.0)*(x1192.value));
15169 sj4=0;
15170 cj4=-1.0;
15171 j4=3.14159265358979;
15172 sj5=gconst13;
15173 cj5=gconst14;
15174 j5=x1191;
15175 new_r10=0;
15176 IkReal gconst12=x1191;
15177 IkReal x1193 = new_r00*new_r00;
15178 if(IKabs(x1193)==0){
15179 continue;
15180 }
15181 IkReal gconst13=(new_r00*(pow(x1193,-0.5)));
15182 IkReal gconst14=0;
15183 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
15184 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15185 {
15186 {
15187 IkReal j3eval[1];
15188 CheckValue<IkReal> x1195 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15189 if(!x1195.valid){
15190 continue;
15191 }
15192 IkReal x1194=((-1.0)*(x1195.value));
15193 sj4=0;
15194 cj4=-1.0;
15195 j4=3.14159265358979;
15196 sj5=gconst13;
15197 cj5=gconst14;
15198 j5=x1194;
15199 new_r10=0;
15200 IkReal gconst12=x1194;
15201 IkReal x1196 = new_r00*new_r00;
15202 if(IKabs(x1196)==0){
15203 continue;
15204 }
15205 IkReal gconst13=(new_r00*(pow(x1196,-0.5)));
15206 IkReal gconst14=0;
15207 j3eval[0]=new_r00;
15208 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15209 {
15210 continue; // 3 cases reached
15211 
15212 } else
15213 {
15214 {
15215 IkReal j3array[1], cj3array[1], sj3array[1];
15216 bool j3valid[1]={false};
15217 _nj3 = 1;
15218 CheckValue<IkReal> x1197=IKPowWithIntegerCheck(new_r00,-1);
15219 if(!x1197.valid){
15220 continue;
15221 }
15222 CheckValue<IkReal> x1198=IKPowWithIntegerCheck(gconst13,-1);
15223 if(!x1198.valid){
15224 continue;
15225 }
15226 if( IKabs(((-1.0)*gconst13*(x1197.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1198.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1197.value)))+IKsqr((new_r01*(x1198.value)))-1) <= IKFAST_SINCOS_THRESH )
15227  continue;
15228 j3array[0]=IKatan2(((-1.0)*gconst13*(x1197.value)), (new_r01*(x1198.value)));
15229 sj3array[0]=IKsin(j3array[0]);
15230 cj3array[0]=IKcos(j3array[0]);
15231 if( j3array[0] > IKPI )
15232 {
15233  j3array[0]-=IK2PI;
15234 }
15235 else if( j3array[0] < -IKPI )
15236 { j3array[0]+=IK2PI;
15237 }
15238 j3valid[0] = true;
15239 for(int ij3 = 0; ij3 < 1; ++ij3)
15240 {
15241 if( !j3valid[ij3] )
15242 {
15243  continue;
15244 }
15245 _ij3[0] = ij3; _ij3[1] = -1;
15246 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15247 {
15248 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15249 {
15250  j3valid[iij3]=false; _ij3[1] = iij3; break;
15251 }
15252 }
15253 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15254 {
15255 IkReal evalcond[8];
15256 IkReal x1199=IKcos(j3);
15257 IkReal x1200=IKsin(j3);
15258 IkReal x1201=(gconst13*x1199);
15259 IkReal x1202=(gconst13*x1200);
15260 evalcond[0]=(new_r00*x1199);
15261 evalcond[1]=((-1.0)*x1201);
15262 evalcond[2]=(((new_r00*x1200))+gconst13);
15263 evalcond[3]=(x1202+new_r00);
15264 evalcond[4]=((((-1.0)*x1201))+new_r01);
15265 evalcond[5]=((((-1.0)*x1202))+new_r11);
15266 evalcond[6]=(((new_r01*x1200))+(((-1.0)*new_r11*x1199)));
15267 evalcond[7]=(((new_r11*x1200))+(((-1.0)*gconst13))+((new_r01*x1199)));
15268 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15269 {
15270 continue;
15271 }
15272 }
15273 
15274 {
15275 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15276 vinfos[0].jointtype = 1;
15277 vinfos[0].foffset = j0;
15278 vinfos[0].indices[0] = _ij0[0];
15279 vinfos[0].indices[1] = _ij0[1];
15280 vinfos[0].maxsolutions = _nj0;
15281 vinfos[1].jointtype = 1;
15282 vinfos[1].foffset = j1;
15283 vinfos[1].indices[0] = _ij1[0];
15284 vinfos[1].indices[1] = _ij1[1];
15285 vinfos[1].maxsolutions = _nj1;
15286 vinfos[2].jointtype = 1;
15287 vinfos[2].foffset = j2;
15288 vinfos[2].indices[0] = _ij2[0];
15289 vinfos[2].indices[1] = _ij2[1];
15290 vinfos[2].maxsolutions = _nj2;
15291 vinfos[3].jointtype = 1;
15292 vinfos[3].foffset = j3;
15293 vinfos[3].indices[0] = _ij3[0];
15294 vinfos[3].indices[1] = _ij3[1];
15295 vinfos[3].maxsolutions = _nj3;
15296 vinfos[4].jointtype = 1;
15297 vinfos[4].foffset = j4;
15298 vinfos[4].indices[0] = _ij4[0];
15299 vinfos[4].indices[1] = _ij4[1];
15300 vinfos[4].maxsolutions = _nj4;
15301 vinfos[5].jointtype = 1;
15302 vinfos[5].foffset = j5;
15303 vinfos[5].indices[0] = _ij5[0];
15304 vinfos[5].indices[1] = _ij5[1];
15305 vinfos[5].maxsolutions = _nj5;
15306 std::vector<int> vfree(0);
15307 solutions.AddSolution(vinfos,vfree);
15308 }
15309 }
15310 }
15311 
15312 }
15313 
15314 }
15315 
15316 } else
15317 {
15318 {
15319 IkReal j3array[1], cj3array[1], sj3array[1];
15320 bool j3valid[1]={false};
15321 _nj3 = 1;
15322 CheckValue<IkReal> x1203 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15323 if(!x1203.valid){
15324 continue;
15325 }
15326 CheckValue<IkReal> x1204=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15327 if(!x1204.valid){
15328 continue;
15329 }
15330 j3array[0]=((-1.5707963267949)+(x1203.value)+(((1.5707963267949)*(x1204.value))));
15331 sj3array[0]=IKsin(j3array[0]);
15332 cj3array[0]=IKcos(j3array[0]);
15333 if( j3array[0] > IKPI )
15334 {
15335  j3array[0]-=IK2PI;
15336 }
15337 else if( j3array[0] < -IKPI )
15338 { j3array[0]+=IK2PI;
15339 }
15340 j3valid[0] = true;
15341 for(int ij3 = 0; ij3 < 1; ++ij3)
15342 {
15343 if( !j3valid[ij3] )
15344 {
15345  continue;
15346 }
15347 _ij3[0] = ij3; _ij3[1] = -1;
15348 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15349 {
15350 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15351 {
15352  j3valid[iij3]=false; _ij3[1] = iij3; break;
15353 }
15354 }
15355 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15356 {
15357 IkReal evalcond[8];
15358 IkReal x1205=IKcos(j3);
15359 IkReal x1206=IKsin(j3);
15360 IkReal x1207=(gconst13*x1205);
15361 IkReal x1208=(gconst13*x1206);
15362 evalcond[0]=(new_r00*x1205);
15363 evalcond[1]=((-1.0)*x1207);
15364 evalcond[2]=(((new_r00*x1206))+gconst13);
15365 evalcond[3]=(x1208+new_r00);
15366 evalcond[4]=((((-1.0)*x1207))+new_r01);
15367 evalcond[5]=((((-1.0)*x1208))+new_r11);
15368 evalcond[6]=(((new_r01*x1206))+(((-1.0)*new_r11*x1205)));
15369 evalcond[7]=(((new_r11*x1206))+((new_r01*x1205))+(((-1.0)*gconst13)));
15370 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15371 {
15372 continue;
15373 }
15374 }
15375 
15376 {
15377 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15378 vinfos[0].jointtype = 1;
15379 vinfos[0].foffset = j0;
15380 vinfos[0].indices[0] = _ij0[0];
15381 vinfos[0].indices[1] = _ij0[1];
15382 vinfos[0].maxsolutions = _nj0;
15383 vinfos[1].jointtype = 1;
15384 vinfos[1].foffset = j1;
15385 vinfos[1].indices[0] = _ij1[0];
15386 vinfos[1].indices[1] = _ij1[1];
15387 vinfos[1].maxsolutions = _nj1;
15388 vinfos[2].jointtype = 1;
15389 vinfos[2].foffset = j2;
15390 vinfos[2].indices[0] = _ij2[0];
15391 vinfos[2].indices[1] = _ij2[1];
15392 vinfos[2].maxsolutions = _nj2;
15393 vinfos[3].jointtype = 1;
15394 vinfos[3].foffset = j3;
15395 vinfos[3].indices[0] = _ij3[0];
15396 vinfos[3].indices[1] = _ij3[1];
15397 vinfos[3].maxsolutions = _nj3;
15398 vinfos[4].jointtype = 1;
15399 vinfos[4].foffset = j4;
15400 vinfos[4].indices[0] = _ij4[0];
15401 vinfos[4].indices[1] = _ij4[1];
15402 vinfos[4].maxsolutions = _nj4;
15403 vinfos[5].jointtype = 1;
15404 vinfos[5].foffset = j5;
15405 vinfos[5].indices[0] = _ij5[0];
15406 vinfos[5].indices[1] = _ij5[1];
15407 vinfos[5].maxsolutions = _nj5;
15408 std::vector<int> vfree(0);
15409 solutions.AddSolution(vinfos,vfree);
15410 }
15411 }
15412 }
15413 
15414 }
15415 
15416 }
15417 
15418 } else
15419 {
15420 {
15421 IkReal j3array[1], cj3array[1], sj3array[1];
15422 bool j3valid[1]={false};
15423 _nj3 = 1;
15424 CheckValue<IkReal> x1209 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15425 if(!x1209.valid){
15426 continue;
15427 }
15428 CheckValue<IkReal> x1210=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15429 if(!x1210.valid){
15430 continue;
15431 }
15432 j3array[0]=((-1.5707963267949)+(x1209.value)+(((1.5707963267949)*(x1210.value))));
15433 sj3array[0]=IKsin(j3array[0]);
15434 cj3array[0]=IKcos(j3array[0]);
15435 if( j3array[0] > IKPI )
15436 {
15437  j3array[0]-=IK2PI;
15438 }
15439 else if( j3array[0] < -IKPI )
15440 { j3array[0]+=IK2PI;
15441 }
15442 j3valid[0] = true;
15443 for(int ij3 = 0; ij3 < 1; ++ij3)
15444 {
15445 if( !j3valid[ij3] )
15446 {
15447  continue;
15448 }
15449 _ij3[0] = ij3; _ij3[1] = -1;
15450 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15451 {
15452 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15453 {
15454  j3valid[iij3]=false; _ij3[1] = iij3; break;
15455 }
15456 }
15457 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15458 {
15459 IkReal evalcond[8];
15460 IkReal x1211=IKcos(j3);
15461 IkReal x1212=IKsin(j3);
15462 IkReal x1213=(gconst13*x1211);
15463 IkReal x1214=(gconst13*x1212);
15464 evalcond[0]=(new_r00*x1211);
15465 evalcond[1]=((-1.0)*x1213);
15466 evalcond[2]=(gconst13+((new_r00*x1212)));
15467 evalcond[3]=(x1214+new_r00);
15468 evalcond[4]=(new_r01+(((-1.0)*x1213)));
15469 evalcond[5]=(new_r11+(((-1.0)*x1214)));
15470 evalcond[6]=(((new_r01*x1212))+(((-1.0)*new_r11*x1211)));
15471 evalcond[7]=(((new_r11*x1212))+((new_r01*x1211))+(((-1.0)*gconst13)));
15472 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15473 {
15474 continue;
15475 }
15476 }
15477 
15478 {
15479 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15480 vinfos[0].jointtype = 1;
15481 vinfos[0].foffset = j0;
15482 vinfos[0].indices[0] = _ij0[0];
15483 vinfos[0].indices[1] = _ij0[1];
15484 vinfos[0].maxsolutions = _nj0;
15485 vinfos[1].jointtype = 1;
15486 vinfos[1].foffset = j1;
15487 vinfos[1].indices[0] = _ij1[0];
15488 vinfos[1].indices[1] = _ij1[1];
15489 vinfos[1].maxsolutions = _nj1;
15490 vinfos[2].jointtype = 1;
15491 vinfos[2].foffset = j2;
15492 vinfos[2].indices[0] = _ij2[0];
15493 vinfos[2].indices[1] = _ij2[1];
15494 vinfos[2].maxsolutions = _nj2;
15495 vinfos[3].jointtype = 1;
15496 vinfos[3].foffset = j3;
15497 vinfos[3].indices[0] = _ij3[0];
15498 vinfos[3].indices[1] = _ij3[1];
15499 vinfos[3].maxsolutions = _nj3;
15500 vinfos[4].jointtype = 1;
15501 vinfos[4].foffset = j4;
15502 vinfos[4].indices[0] = _ij4[0];
15503 vinfos[4].indices[1] = _ij4[1];
15504 vinfos[4].maxsolutions = _nj4;
15505 vinfos[5].jointtype = 1;
15506 vinfos[5].foffset = j5;
15507 vinfos[5].indices[0] = _ij5[0];
15508 vinfos[5].indices[1] = _ij5[1];
15509 vinfos[5].maxsolutions = _nj5;
15510 std::vector<int> vfree(0);
15511 solutions.AddSolution(vinfos,vfree);
15512 }
15513 }
15514 }
15515 
15516 }
15517 
15518 }
15519 
15520 }
15521 } while(0);
15522 if( bgotonextstatement )
15523 {
15524 bool bgotonextstatement = true;
15525 do
15526 {
15527 if( 1 )
15528 {
15529 bgotonextstatement=false;
15530 continue; // branch miss [j3]
15531 
15532 }
15533 } while(0);
15534 if( bgotonextstatement )
15535 {
15536 }
15537 }
15538 }
15539 }
15540 }
15541 }
15542 }
15543 
15544 } else
15545 {
15546 {
15547 IkReal j3array[1], cj3array[1], sj3array[1];
15548 bool j3valid[1]={false};
15549 _nj3 = 1;
15550 IkReal x1215=((1.0)*gconst14);
15551 CheckValue<IkReal> x1216=IKPowWithIntegerCheck(IKsign((((gconst13*new_r00))+(((-1.0)*new_r10*x1215)))),-1);
15552 if(!x1216.valid){
15553 continue;
15554 }
15555 CheckValue<IkReal> x1217 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst13*x1215)))),IKFAST_ATAN2_MAGTHRESH);
15556 if(!x1217.valid){
15557 continue;
15558 }
15559 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1216.value)))+(x1217.value));
15560 sj3array[0]=IKsin(j3array[0]);
15561 cj3array[0]=IKcos(j3array[0]);
15562 if( j3array[0] > IKPI )
15563 {
15564  j3array[0]-=IK2PI;
15565 }
15566 else if( j3array[0] < -IKPI )
15567 { j3array[0]+=IK2PI;
15568 }
15569 j3valid[0] = true;
15570 for(int ij3 = 0; ij3 < 1; ++ij3)
15571 {
15572 if( !j3valid[ij3] )
15573 {
15574  continue;
15575 }
15576 _ij3[0] = ij3; _ij3[1] = -1;
15577 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15578 {
15579 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15580 {
15581  j3valid[iij3]=false; _ij3[1] = iij3; break;
15582 }
15583 }
15584 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15585 {
15586 IkReal evalcond[8];
15587 IkReal x1218=IKsin(j3);
15588 IkReal x1219=IKcos(j3);
15589 IkReal x1220=(gconst14*x1218);
15590 IkReal x1221=((1.0)*x1219);
15591 IkReal x1222=(gconst13*x1218);
15592 IkReal x1223=(gconst13*x1221);
15593 evalcond[0]=(((new_r10*x1218))+gconst14+((new_r00*x1219)));
15594 evalcond[1]=(x1222+((gconst14*x1219))+new_r00);
15595 evalcond[2]=(gconst13+((new_r00*x1218))+(((-1.0)*new_r10*x1221)));
15596 evalcond[3]=(gconst14+((new_r01*x1218))+(((-1.0)*new_r11*x1221)));
15597 evalcond[4]=(x1220+(((-1.0)*x1223))+new_r01);
15598 evalcond[5]=(x1220+(((-1.0)*x1223))+new_r10);
15599 evalcond[6]=(((new_r11*x1218))+((new_r01*x1219))+(((-1.0)*gconst13)));
15600 evalcond[7]=((((-1.0)*x1222))+new_r11+(((-1.0)*gconst14*x1221)));
15601 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15602 {
15603 continue;
15604 }
15605 }
15606 
15607 {
15608 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15609 vinfos[0].jointtype = 1;
15610 vinfos[0].foffset = j0;
15611 vinfos[0].indices[0] = _ij0[0];
15612 vinfos[0].indices[1] = _ij0[1];
15613 vinfos[0].maxsolutions = _nj0;
15614 vinfos[1].jointtype = 1;
15615 vinfos[1].foffset = j1;
15616 vinfos[1].indices[0] = _ij1[0];
15617 vinfos[1].indices[1] = _ij1[1];
15618 vinfos[1].maxsolutions = _nj1;
15619 vinfos[2].jointtype = 1;
15620 vinfos[2].foffset = j2;
15621 vinfos[2].indices[0] = _ij2[0];
15622 vinfos[2].indices[1] = _ij2[1];
15623 vinfos[2].maxsolutions = _nj2;
15624 vinfos[3].jointtype = 1;
15625 vinfos[3].foffset = j3;
15626 vinfos[3].indices[0] = _ij3[0];
15627 vinfos[3].indices[1] = _ij3[1];
15628 vinfos[3].maxsolutions = _nj3;
15629 vinfos[4].jointtype = 1;
15630 vinfos[4].foffset = j4;
15631 vinfos[4].indices[0] = _ij4[0];
15632 vinfos[4].indices[1] = _ij4[1];
15633 vinfos[4].maxsolutions = _nj4;
15634 vinfos[5].jointtype = 1;
15635 vinfos[5].foffset = j5;
15636 vinfos[5].indices[0] = _ij5[0];
15637 vinfos[5].indices[1] = _ij5[1];
15638 vinfos[5].maxsolutions = _nj5;
15639 std::vector<int> vfree(0);
15640 solutions.AddSolution(vinfos,vfree);
15641 }
15642 }
15643 }
15644 
15645 }
15646 
15647 }
15648 
15649 } else
15650 {
15651 {
15652 IkReal j3array[1], cj3array[1], sj3array[1];
15653 bool j3valid[1]={false};
15654 _nj3 = 1;
15655 IkReal x1224=((1.0)*gconst14);
15656 CheckValue<IkReal> x1225 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+((new_r00*new_r11)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst13*x1224)))),IKFAST_ATAN2_MAGTHRESH);
15657 if(!x1225.valid){
15658 continue;
15659 }
15660 CheckValue<IkReal> x1226=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r11))+(((-1.0)*new_r01*x1224)))),-1);
15661 if(!x1226.valid){
15662 continue;
15663 }
15664 j3array[0]=((-1.5707963267949)+(x1225.value)+(((1.5707963267949)*(x1226.value))));
15665 sj3array[0]=IKsin(j3array[0]);
15666 cj3array[0]=IKcos(j3array[0]);
15667 if( j3array[0] > IKPI )
15668 {
15669  j3array[0]-=IK2PI;
15670 }
15671 else if( j3array[0] < -IKPI )
15672 { j3array[0]+=IK2PI;
15673 }
15674 j3valid[0] = true;
15675 for(int ij3 = 0; ij3 < 1; ++ij3)
15676 {
15677 if( !j3valid[ij3] )
15678 {
15679  continue;
15680 }
15681 _ij3[0] = ij3; _ij3[1] = -1;
15682 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15683 {
15684 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15685 {
15686  j3valid[iij3]=false; _ij3[1] = iij3; break;
15687 }
15688 }
15689 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15690 {
15691 IkReal evalcond[8];
15692 IkReal x1227=IKsin(j3);
15693 IkReal x1228=IKcos(j3);
15694 IkReal x1229=(gconst14*x1227);
15695 IkReal x1230=((1.0)*x1228);
15696 IkReal x1231=(gconst13*x1227);
15697 IkReal x1232=(gconst13*x1230);
15698 evalcond[0]=(gconst14+((new_r00*x1228))+((new_r10*x1227)));
15699 evalcond[1]=(x1231+new_r00+((gconst14*x1228)));
15700 evalcond[2]=(gconst13+((new_r00*x1227))+(((-1.0)*new_r10*x1230)));
15701 evalcond[3]=(gconst14+((new_r01*x1227))+(((-1.0)*new_r11*x1230)));
15702 evalcond[4]=(x1229+(((-1.0)*x1232))+new_r01);
15703 evalcond[5]=(x1229+(((-1.0)*x1232))+new_r10);
15704 evalcond[6]=(((new_r11*x1227))+((new_r01*x1228))+(((-1.0)*gconst13)));
15705 evalcond[7]=((((-1.0)*x1231))+new_r11+(((-1.0)*gconst14*x1230)));
15706 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15707 {
15708 continue;
15709 }
15710 }
15711 
15712 {
15713 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15714 vinfos[0].jointtype = 1;
15715 vinfos[0].foffset = j0;
15716 vinfos[0].indices[0] = _ij0[0];
15717 vinfos[0].indices[1] = _ij0[1];
15718 vinfos[0].maxsolutions = _nj0;
15719 vinfos[1].jointtype = 1;
15720 vinfos[1].foffset = j1;
15721 vinfos[1].indices[0] = _ij1[0];
15722 vinfos[1].indices[1] = _ij1[1];
15723 vinfos[1].maxsolutions = _nj1;
15724 vinfos[2].jointtype = 1;
15725 vinfos[2].foffset = j2;
15726 vinfos[2].indices[0] = _ij2[0];
15727 vinfos[2].indices[1] = _ij2[1];
15728 vinfos[2].maxsolutions = _nj2;
15729 vinfos[3].jointtype = 1;
15730 vinfos[3].foffset = j3;
15731 vinfos[3].indices[0] = _ij3[0];
15732 vinfos[3].indices[1] = _ij3[1];
15733 vinfos[3].maxsolutions = _nj3;
15734 vinfos[4].jointtype = 1;
15735 vinfos[4].foffset = j4;
15736 vinfos[4].indices[0] = _ij4[0];
15737 vinfos[4].indices[1] = _ij4[1];
15738 vinfos[4].maxsolutions = _nj4;
15739 vinfos[5].jointtype = 1;
15740 vinfos[5].foffset = j5;
15741 vinfos[5].indices[0] = _ij5[0];
15742 vinfos[5].indices[1] = _ij5[1];
15743 vinfos[5].maxsolutions = _nj5;
15744 std::vector<int> vfree(0);
15745 solutions.AddSolution(vinfos,vfree);
15746 }
15747 }
15748 }
15749 
15750 }
15751 
15752 }
15753 
15754 } else
15755 {
15756 {
15757 IkReal j3array[1], cj3array[1], sj3array[1];
15758 bool j3valid[1]={false};
15759 _nj3 = 1;
15760 IkReal x1233=((1.0)*new_r10);
15761 CheckValue<IkReal> x1234=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1233)))),-1);
15762 if(!x1234.valid){
15763 continue;
15764 }
15765 CheckValue<IkReal> x1235 = IKatan2WithCheck(IkReal((((gconst14*new_r00))+((gconst14*new_r11)))),IkReal((((gconst14*new_r01))+(((-1.0)*gconst14*x1233)))),IKFAST_ATAN2_MAGTHRESH);
15766 if(!x1235.valid){
15767 continue;
15768 }
15769 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1234.value)))+(x1235.value));
15770 sj3array[0]=IKsin(j3array[0]);
15771 cj3array[0]=IKcos(j3array[0]);
15772 if( j3array[0] > IKPI )
15773 {
15774  j3array[0]-=IK2PI;
15775 }
15776 else if( j3array[0] < -IKPI )
15777 { j3array[0]+=IK2PI;
15778 }
15779 j3valid[0] = true;
15780 for(int ij3 = 0; ij3 < 1; ++ij3)
15781 {
15782 if( !j3valid[ij3] )
15783 {
15784  continue;
15785 }
15786 _ij3[0] = ij3; _ij3[1] = -1;
15787 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15788 {
15789 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15790 {
15791  j3valid[iij3]=false; _ij3[1] = iij3; break;
15792 }
15793 }
15794 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15795 {
15796 IkReal evalcond[8];
15797 IkReal x1236=IKsin(j3);
15798 IkReal x1237=IKcos(j3);
15799 IkReal x1238=(gconst14*x1236);
15800 IkReal x1239=((1.0)*x1237);
15801 IkReal x1240=(gconst13*x1236);
15802 IkReal x1241=(gconst13*x1239);
15803 evalcond[0]=(gconst14+((new_r10*x1236))+((new_r00*x1237)));
15804 evalcond[1]=(((gconst14*x1237))+x1240+new_r00);
15805 evalcond[2]=(gconst13+((new_r00*x1236))+(((-1.0)*new_r10*x1239)));
15806 evalcond[3]=(gconst14+((new_r01*x1236))+(((-1.0)*new_r11*x1239)));
15807 evalcond[4]=(x1238+(((-1.0)*x1241))+new_r01);
15808 evalcond[5]=(x1238+(((-1.0)*x1241))+new_r10);
15809 evalcond[6]=(((new_r01*x1237))+((new_r11*x1236))+(((-1.0)*gconst13)));
15810 evalcond[7]=((((-1.0)*x1240))+new_r11+(((-1.0)*gconst14*x1239)));
15811 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15812 {
15813 continue;
15814 }
15815 }
15816 
15817 {
15818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15819 vinfos[0].jointtype = 1;
15820 vinfos[0].foffset = j0;
15821 vinfos[0].indices[0] = _ij0[0];
15822 vinfos[0].indices[1] = _ij0[1];
15823 vinfos[0].maxsolutions = _nj0;
15824 vinfos[1].jointtype = 1;
15825 vinfos[1].foffset = j1;
15826 vinfos[1].indices[0] = _ij1[0];
15827 vinfos[1].indices[1] = _ij1[1];
15828 vinfos[1].maxsolutions = _nj1;
15829 vinfos[2].jointtype = 1;
15830 vinfos[2].foffset = j2;
15831 vinfos[2].indices[0] = _ij2[0];
15832 vinfos[2].indices[1] = _ij2[1];
15833 vinfos[2].maxsolutions = _nj2;
15834 vinfos[3].jointtype = 1;
15835 vinfos[3].foffset = j3;
15836 vinfos[3].indices[0] = _ij3[0];
15837 vinfos[3].indices[1] = _ij3[1];
15838 vinfos[3].maxsolutions = _nj3;
15839 vinfos[4].jointtype = 1;
15840 vinfos[4].foffset = j4;
15841 vinfos[4].indices[0] = _ij4[0];
15842 vinfos[4].indices[1] = _ij4[1];
15843 vinfos[4].maxsolutions = _nj4;
15844 vinfos[5].jointtype = 1;
15845 vinfos[5].foffset = j5;
15846 vinfos[5].indices[0] = _ij5[0];
15847 vinfos[5].indices[1] = _ij5[1];
15848 vinfos[5].maxsolutions = _nj5;
15849 std::vector<int> vfree(0);
15850 solutions.AddSolution(vinfos,vfree);
15851 }
15852 }
15853 }
15854 
15855 }
15856 
15857 }
15858 
15859 }
15860 } while(0);
15861 if( bgotonextstatement )
15862 {
15863 bool bgotonextstatement = true;
15864 do
15865 {
15866 IkReal x1242=((-1.0)*new_r00);
15867 IkReal x1244 = ((new_r10*new_r10)+(new_r00*new_r00));
15868 if(IKabs(x1244)==0){
15869 continue;
15870 }
15871 IkReal x1243=pow(x1244,-0.5);
15872 CheckValue<IkReal> x1245 = IKatan2WithCheck(IkReal(x1242),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15873 if(!x1245.valid){
15874 continue;
15875 }
15876 IkReal gconst15=((3.14159265358979)+(((-1.0)*(x1245.value))));
15877 IkReal gconst16=(x1242*x1243);
15878 IkReal gconst17=((1.0)*new_r10*x1243);
15879 CheckValue<IkReal> x1246 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15880 if(!x1246.valid){
15881 continue;
15882 }
15883 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x1246.value)+j5)))), 6.28318530717959)));
15884 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15885 {
15886 bgotonextstatement=false;
15887 {
15888 IkReal j3eval[3];
15889 IkReal x1247=((-1.0)*new_r00);
15890 CheckValue<IkReal> x1250 = IKatan2WithCheck(IkReal(x1247),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15891 if(!x1250.valid){
15892 continue;
15893 }
15894 IkReal x1248=((1.0)*(x1250.value));
15895 IkReal x1249=x1243;
15896 sj4=0;
15897 cj4=-1.0;
15898 j4=3.14159265358979;
15899 sj5=gconst16;
15900 cj5=gconst17;
15901 j5=((3.14159265)+(((-1.0)*x1248)));
15902 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1248)));
15903 IkReal gconst16=(x1247*x1249);
15904 IkReal gconst17=((1.0)*new_r10*x1249);
15905 IkReal x1251=new_r10*new_r10;
15906 IkReal x1252=(new_r10*new_r11);
15907 IkReal x1253=((((-1.0)*new_r00*new_r01))+(((-1.0)*x1252)));
15908 IkReal x1254=x1243;
15909 IkReal x1255=(new_r10*x1254);
15910 j3eval[0]=x1253;
15911 j3eval[1]=((IKabs((((new_r01*x1255))+(((-1.0)*x1251*x1254)))))+(IKabs((((new_r00*x1255))+((x1252*x1254))))));
15912 j3eval[2]=IKsign(x1253);
15913 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
15914 {
15915 {
15916 IkReal j3eval[1];
15917 IkReal x1256=((-1.0)*new_r00);
15918 CheckValue<IkReal> x1259 = IKatan2WithCheck(IkReal(x1256),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15919 if(!x1259.valid){
15920 continue;
15921 }
15922 IkReal x1257=((1.0)*(x1259.value));
15923 IkReal x1258=x1243;
15924 sj4=0;
15925 cj4=-1.0;
15926 j4=3.14159265358979;
15927 sj5=gconst16;
15928 cj5=gconst17;
15929 j5=((3.14159265)+(((-1.0)*x1257)));
15930 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1257)));
15931 IkReal gconst16=(x1256*x1258);
15932 IkReal gconst17=((1.0)*new_r10*x1258);
15933 IkReal x1260=new_r10*new_r10;
15934 IkReal x1261=new_r00*new_r00*new_r00;
15935 CheckValue<IkReal> x1265=IKPowWithIntegerCheck((x1260+(new_r00*new_r00)),-1);
15936 if(!x1265.valid){
15937 continue;
15938 }
15939 IkReal x1262=x1265.value;
15940 IkReal x1263=(x1260*x1262);
15941 IkReal x1264=(x1261*x1262);
15942 j3eval[0]=((IKabs((((new_r00*new_r10*x1262))+((new_r01*x1264))+((new_r00*new_r01*x1263)))))+(IKabs((x1263+((new_r11*x1264))+((new_r00*new_r11*x1263))))));
15943 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15944 {
15945 {
15946 IkReal j3eval[1];
15947 IkReal x1266=((-1.0)*new_r00);
15948 CheckValue<IkReal> x1269 = IKatan2WithCheck(IkReal(x1266),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15949 if(!x1269.valid){
15950 continue;
15951 }
15952 IkReal x1267=((1.0)*(x1269.value));
15953 IkReal x1268=x1243;
15954 sj4=0;
15955 cj4=-1.0;
15956 j4=3.14159265358979;
15957 sj5=gconst16;
15958 cj5=gconst17;
15959 j5=((3.14159265)+(((-1.0)*x1267)));
15960 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1267)));
15961 IkReal gconst16=(x1266*x1268);
15962 IkReal gconst17=((1.0)*new_r10*x1268);
15963 IkReal x1270=new_r10*new_r10;
15964 IkReal x1271=new_r00*new_r00;
15965 CheckValue<IkReal> x1275=IKPowWithIntegerCheck((x1270+x1271),-1);
15966 if(!x1275.valid){
15967 continue;
15968 }
15969 IkReal x1272=x1275.value;
15970 IkReal x1273=(new_r10*x1272);
15971 IkReal x1274=(x1270*x1272);
15972 j3eval[0]=((IKabs(((((-1.0)*x1271*x1274))+x1274+(((-1.0)*x1272*(x1271*x1271))))))+(IKabs((((new_r00*x1273))+((x1273*(new_r00*new_r00*new_r00)))+((new_r00*x1273*(new_r10*new_r10)))))));
15973 if( IKabs(j3eval[0]) < 0.0000010000000000 )
15974 {
15975 {
15976 IkReal evalcond[2];
15977 bool bgotonextstatement = true;
15978 do
15979 {
15980 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
15981 if( IKabs(evalcond[0]) < 0.0000050000000000 )
15982 {
15983 bgotonextstatement=false;
15984 {
15985 IkReal j3eval[1];
15986 CheckValue<IkReal> x1277 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15987 if(!x1277.valid){
15988 continue;
15989 }
15990 IkReal x1276=((1.0)*(x1277.value));
15991 sj4=0;
15992 cj4=-1.0;
15993 j4=3.14159265358979;
15994 sj5=gconst16;
15995 cj5=gconst17;
15996 j5=((3.14159265)+(((-1.0)*x1276)));
15997 new_r11=0;
15998 new_r00=0;
15999 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1276)));
16000 IkReal gconst16=0;
16001 IkReal x1278 = new_r10*new_r10;
16002 if(IKabs(x1278)==0){
16003 continue;
16004 }
16005 IkReal gconst17=((1.0)*new_r10*(pow(x1278,-0.5)));
16006 j3eval[0]=new_r01;
16007 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16008 {
16009 {
16010 IkReal j3array[2], cj3array[2], sj3array[2];
16011 bool j3valid[2]={false};
16012 _nj3 = 2;
16013 CheckValue<IkReal> x1279=IKPowWithIntegerCheck(gconst17,-1);
16014 if(!x1279.valid){
16015 continue;
16016 }
16017 sj3array[0]=((-1.0)*new_r01*(x1279.value));
16018 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16019 {
16020  j3valid[0] = j3valid[1] = true;
16021  j3array[0] = IKasin(sj3array[0]);
16022  cj3array[0] = IKcos(j3array[0]);
16023  sj3array[1] = sj3array[0];
16024  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16025  cj3array[1] = -cj3array[0];
16026 }
16027 else if( isnan(sj3array[0]) )
16028 {
16029  // probably any value will work
16030  j3valid[0] = true;
16031  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16032 }
16033 for(int ij3 = 0; ij3 < 2; ++ij3)
16034 {
16035 if( !j3valid[ij3] )
16036 {
16037  continue;
16038 }
16039 _ij3[0] = ij3; _ij3[1] = -1;
16040 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16041 {
16042 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16043 {
16044  j3valid[iij3]=false; _ij3[1] = iij3; break;
16045 }
16046 }
16047 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16048 {
16049 IkReal evalcond[6];
16050 IkReal x1280=IKcos(j3);
16051 IkReal x1281=IKsin(j3);
16052 evalcond[0]=(new_r01*x1280);
16053 evalcond[1]=(gconst17*x1280);
16054 evalcond[2]=((-1.0)*new_r10*x1280);
16055 evalcond[3]=(gconst17+((new_r01*x1281)));
16056 evalcond[4]=(((new_r10*x1281))+gconst17);
16057 evalcond[5]=(new_r10+((gconst17*x1281)));
16058 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16059 {
16060 continue;
16061 }
16062 }
16063 
16064 {
16065 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16066 vinfos[0].jointtype = 1;
16067 vinfos[0].foffset = j0;
16068 vinfos[0].indices[0] = _ij0[0];
16069 vinfos[0].indices[1] = _ij0[1];
16070 vinfos[0].maxsolutions = _nj0;
16071 vinfos[1].jointtype = 1;
16072 vinfos[1].foffset = j1;
16073 vinfos[1].indices[0] = _ij1[0];
16074 vinfos[1].indices[1] = _ij1[1];
16075 vinfos[1].maxsolutions = _nj1;
16076 vinfos[2].jointtype = 1;
16077 vinfos[2].foffset = j2;
16078 vinfos[2].indices[0] = _ij2[0];
16079 vinfos[2].indices[1] = _ij2[1];
16080 vinfos[2].maxsolutions = _nj2;
16081 vinfos[3].jointtype = 1;
16082 vinfos[3].foffset = j3;
16083 vinfos[3].indices[0] = _ij3[0];
16084 vinfos[3].indices[1] = _ij3[1];
16085 vinfos[3].maxsolutions = _nj3;
16086 vinfos[4].jointtype = 1;
16087 vinfos[4].foffset = j4;
16088 vinfos[4].indices[0] = _ij4[0];
16089 vinfos[4].indices[1] = _ij4[1];
16090 vinfos[4].maxsolutions = _nj4;
16091 vinfos[5].jointtype = 1;
16092 vinfos[5].foffset = j5;
16093 vinfos[5].indices[0] = _ij5[0];
16094 vinfos[5].indices[1] = _ij5[1];
16095 vinfos[5].maxsolutions = _nj5;
16096 std::vector<int> vfree(0);
16097 solutions.AddSolution(vinfos,vfree);
16098 }
16099 }
16100 }
16101 
16102 } else
16103 {
16104 {
16105 IkReal j3array[2], cj3array[2], sj3array[2];
16106 bool j3valid[2]={false};
16107 _nj3 = 2;
16108 CheckValue<IkReal> x1282=IKPowWithIntegerCheck(new_r01,-1);
16109 if(!x1282.valid){
16110 continue;
16111 }
16112 sj3array[0]=((-1.0)*gconst17*(x1282.value));
16113 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16114 {
16115  j3valid[0] = j3valid[1] = true;
16116  j3array[0] = IKasin(sj3array[0]);
16117  cj3array[0] = IKcos(j3array[0]);
16118  sj3array[1] = sj3array[0];
16119  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16120  cj3array[1] = -cj3array[0];
16121 }
16122 else if( isnan(sj3array[0]) )
16123 {
16124  // probably any value will work
16125  j3valid[0] = true;
16126  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16127 }
16128 for(int ij3 = 0; ij3 < 2; ++ij3)
16129 {
16130 if( !j3valid[ij3] )
16131 {
16132  continue;
16133 }
16134 _ij3[0] = ij3; _ij3[1] = -1;
16135 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16136 {
16137 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16138 {
16139  j3valid[iij3]=false; _ij3[1] = iij3; break;
16140 }
16141 }
16142 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16143 {
16144 IkReal evalcond[6];
16145 IkReal x1283=IKcos(j3);
16146 IkReal x1284=IKsin(j3);
16147 IkReal x1285=(gconst17*x1284);
16148 evalcond[0]=(new_r01*x1283);
16149 evalcond[1]=(gconst17*x1283);
16150 evalcond[2]=((-1.0)*new_r10*x1283);
16151 evalcond[3]=(x1285+new_r01);
16152 evalcond[4]=(((new_r10*x1284))+gconst17);
16153 evalcond[5]=(x1285+new_r10);
16154 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16155 {
16156 continue;
16157 }
16158 }
16159 
16160 {
16161 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16162 vinfos[0].jointtype = 1;
16163 vinfos[0].foffset = j0;
16164 vinfos[0].indices[0] = _ij0[0];
16165 vinfos[0].indices[1] = _ij0[1];
16166 vinfos[0].maxsolutions = _nj0;
16167 vinfos[1].jointtype = 1;
16168 vinfos[1].foffset = j1;
16169 vinfos[1].indices[0] = _ij1[0];
16170 vinfos[1].indices[1] = _ij1[1];
16171 vinfos[1].maxsolutions = _nj1;
16172 vinfos[2].jointtype = 1;
16173 vinfos[2].foffset = j2;
16174 vinfos[2].indices[0] = _ij2[0];
16175 vinfos[2].indices[1] = _ij2[1];
16176 vinfos[2].maxsolutions = _nj2;
16177 vinfos[3].jointtype = 1;
16178 vinfos[3].foffset = j3;
16179 vinfos[3].indices[0] = _ij3[0];
16180 vinfos[3].indices[1] = _ij3[1];
16181 vinfos[3].maxsolutions = _nj3;
16182 vinfos[4].jointtype = 1;
16183 vinfos[4].foffset = j4;
16184 vinfos[4].indices[0] = _ij4[0];
16185 vinfos[4].indices[1] = _ij4[1];
16186 vinfos[4].maxsolutions = _nj4;
16187 vinfos[5].jointtype = 1;
16188 vinfos[5].foffset = j5;
16189 vinfos[5].indices[0] = _ij5[0];
16190 vinfos[5].indices[1] = _ij5[1];
16191 vinfos[5].maxsolutions = _nj5;
16192 std::vector<int> vfree(0);
16193 solutions.AddSolution(vinfos,vfree);
16194 }
16195 }
16196 }
16197 
16198 }
16199 
16200 }
16201 
16202 }
16203 } while(0);
16204 if( bgotonextstatement )
16205 {
16206 bool bgotonextstatement = true;
16207 do
16208 {
16209 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
16210 evalcond[1]=gconst17;
16211 if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
16212 {
16213 bgotonextstatement=false;
16214 {
16215 IkReal j3eval[3];
16216 IkReal x1286=((-1.0)*new_r00);
16217 CheckValue<IkReal> x1288 = IKatan2WithCheck(IkReal(x1286),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16218 if(!x1288.valid){
16219 continue;
16220 }
16221 IkReal x1287=((1.0)*(x1288.value));
16222 sj4=0;
16223 cj4=-1.0;
16224 j4=3.14159265358979;
16225 sj5=gconst16;
16226 cj5=gconst17;
16227 j5=((3.14159265)+(((-1.0)*x1287)));
16228 new_r11=0;
16229 new_r01=0;
16230 new_r22=0;
16231 new_r20=0;
16232 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1287)));
16233 IkReal gconst16=x1286;
16234 IkReal gconst17=((1.0)*new_r10);
16235 j3eval[0]=1.0;
16236 j3eval[1]=1.0;
16237 j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
16238 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16239 {
16240 {
16241 IkReal j3eval[3];
16242 IkReal x1289=((-1.0)*new_r00);
16243 CheckValue<IkReal> x1291 = IKatan2WithCheck(IkReal(x1289),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16244 if(!x1291.valid){
16245 continue;
16246 }
16247 IkReal x1290=((1.0)*(x1291.value));
16248 sj4=0;
16249 cj4=-1.0;
16250 j4=3.14159265358979;
16251 sj5=gconst16;
16252 cj5=gconst17;
16253 j5=((3.14159265)+(((-1.0)*x1290)));
16254 new_r11=0;
16255 new_r01=0;
16256 new_r22=0;
16257 new_r20=0;
16258 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1290)));
16259 IkReal gconst16=x1289;
16260 IkReal gconst17=((1.0)*new_r10);
16261 j3eval[0]=-1.0;
16262 j3eval[1]=-1.0;
16263 j3eval[2]=((IKabs(((1.0)*new_r00*new_r10)))+(IKabs(((-1.0)+(new_r10*new_r10)))));
16264 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16265 {
16266 {
16267 IkReal j3eval[3];
16268 IkReal x1292=((-1.0)*new_r00);
16269 CheckValue<IkReal> x1294 = IKatan2WithCheck(IkReal(x1292),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16270 if(!x1294.valid){
16271 continue;
16272 }
16273 IkReal x1293=((1.0)*(x1294.value));
16274 sj4=0;
16275 cj4=-1.0;
16276 j4=3.14159265358979;
16277 sj5=gconst16;
16278 cj5=gconst17;
16279 j5=((3.14159265)+(((-1.0)*x1293)));
16280 new_r11=0;
16281 new_r01=0;
16282 new_r22=0;
16283 new_r20=0;
16284 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1293)));
16285 IkReal gconst16=x1292;
16286 IkReal gconst17=((1.0)*new_r10);
16287 j3eval[0]=-1.0;
16288 j3eval[1]=-1.0;
16289 j3eval[2]=((IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
16290 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16291 {
16292 continue; // 3 cases reached
16293 
16294 } else
16295 {
16296 {
16297 IkReal j3array[1], cj3array[1], sj3array[1];
16298 bool j3valid[1]={false};
16299 _nj3 = 1;
16300 IkReal x1295=((1.0)*gconst17);
16301 CheckValue<IkReal> x1296 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1295))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
16302 if(!x1296.valid){
16303 continue;
16304 }
16305 CheckValue<IkReal> x1297=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1295)))),-1);
16306 if(!x1297.valid){
16307 continue;
16308 }
16309 j3array[0]=((-1.5707963267949)+(x1296.value)+(((1.5707963267949)*(x1297.value))));
16310 sj3array[0]=IKsin(j3array[0]);
16311 cj3array[0]=IKcos(j3array[0]);
16312 if( j3array[0] > IKPI )
16313 {
16314  j3array[0]-=IK2PI;
16315 }
16316 else if( j3array[0] < -IKPI )
16317 { j3array[0]+=IK2PI;
16318 }
16319 j3valid[0] = true;
16320 for(int ij3 = 0; ij3 < 1; ++ij3)
16321 {
16322 if( !j3valid[ij3] )
16323 {
16324  continue;
16325 }
16326 _ij3[0] = ij3; _ij3[1] = -1;
16327 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16328 {
16329 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16330 {
16331  j3valid[iij3]=false; _ij3[1] = iij3; break;
16332 }
16333 }
16334 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16335 {
16336 IkReal evalcond[6];
16337 IkReal x1298=IKsin(j3);
16338 IkReal x1299=IKcos(j3);
16339 IkReal x1300=(gconst17*x1298);
16340 IkReal x1301=(gconst16*x1298);
16341 IkReal x1302=(gconst17*x1299);
16342 IkReal x1303=((1.0)*x1299);
16343 IkReal x1304=(gconst16*x1303);
16344 evalcond[0]=(x1300+(((-1.0)*x1304)));
16345 evalcond[1]=(gconst17+((new_r10*x1298))+((new_r00*x1299)));
16346 evalcond[2]=(x1301+x1302+new_r00);
16347 evalcond[3]=((((-1.0)*new_r10*x1303))+gconst16+((new_r00*x1298)));
16348 evalcond[4]=((((-1.0)*x1301))+(((-1.0)*x1302)));
16349 evalcond[5]=(x1300+(((-1.0)*x1304))+new_r10);
16350 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16351 {
16352 continue;
16353 }
16354 }
16355 
16356 {
16357 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16358 vinfos[0].jointtype = 1;
16359 vinfos[0].foffset = j0;
16360 vinfos[0].indices[0] = _ij0[0];
16361 vinfos[0].indices[1] = _ij0[1];
16362 vinfos[0].maxsolutions = _nj0;
16363 vinfos[1].jointtype = 1;
16364 vinfos[1].foffset = j1;
16365 vinfos[1].indices[0] = _ij1[0];
16366 vinfos[1].indices[1] = _ij1[1];
16367 vinfos[1].maxsolutions = _nj1;
16368 vinfos[2].jointtype = 1;
16369 vinfos[2].foffset = j2;
16370 vinfos[2].indices[0] = _ij2[0];
16371 vinfos[2].indices[1] = _ij2[1];
16372 vinfos[2].maxsolutions = _nj2;
16373 vinfos[3].jointtype = 1;
16374 vinfos[3].foffset = j3;
16375 vinfos[3].indices[0] = _ij3[0];
16376 vinfos[3].indices[1] = _ij3[1];
16377 vinfos[3].maxsolutions = _nj3;
16378 vinfos[4].jointtype = 1;
16379 vinfos[4].foffset = j4;
16380 vinfos[4].indices[0] = _ij4[0];
16381 vinfos[4].indices[1] = _ij4[1];
16382 vinfos[4].maxsolutions = _nj4;
16383 vinfos[5].jointtype = 1;
16384 vinfos[5].foffset = j5;
16385 vinfos[5].indices[0] = _ij5[0];
16386 vinfos[5].indices[1] = _ij5[1];
16387 vinfos[5].maxsolutions = _nj5;
16388 std::vector<int> vfree(0);
16389 solutions.AddSolution(vinfos,vfree);
16390 }
16391 }
16392 }
16393 
16394 }
16395 
16396 }
16397 
16398 } else
16399 {
16400 {
16401 IkReal j3array[1], cj3array[1], sj3array[1];
16402 bool j3valid[1]={false};
16403 _nj3 = 1;
16404 CheckValue<IkReal> x1305=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst17*gconst17)))+(((-1.0)*(gconst16*gconst16))))),-1);
16405 if(!x1305.valid){
16406 continue;
16407 }
16408 CheckValue<IkReal> x1306 = IKatan2WithCheck(IkReal((gconst16*new_r00)),IkReal((gconst17*new_r00)),IKFAST_ATAN2_MAGTHRESH);
16409 if(!x1306.valid){
16410 continue;
16411 }
16412 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1305.value)))+(x1306.value));
16413 sj3array[0]=IKsin(j3array[0]);
16414 cj3array[0]=IKcos(j3array[0]);
16415 if( j3array[0] > IKPI )
16416 {
16417  j3array[0]-=IK2PI;
16418 }
16419 else if( j3array[0] < -IKPI )
16420 { j3array[0]+=IK2PI;
16421 }
16422 j3valid[0] = true;
16423 for(int ij3 = 0; ij3 < 1; ++ij3)
16424 {
16425 if( !j3valid[ij3] )
16426 {
16427  continue;
16428 }
16429 _ij3[0] = ij3; _ij3[1] = -1;
16430 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16431 {
16432 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16433 {
16434  j3valid[iij3]=false; _ij3[1] = iij3; break;
16435 }
16436 }
16437 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16438 {
16439 IkReal evalcond[6];
16440 IkReal x1307=IKsin(j3);
16441 IkReal x1308=IKcos(j3);
16442 IkReal x1309=(gconst17*x1307);
16443 IkReal x1310=(gconst16*x1307);
16444 IkReal x1311=(gconst17*x1308);
16445 IkReal x1312=((1.0)*x1308);
16446 IkReal x1313=(gconst16*x1312);
16447 evalcond[0]=(x1309+(((-1.0)*x1313)));
16448 evalcond[1]=(gconst17+((new_r10*x1307))+((new_r00*x1308)));
16449 evalcond[2]=(x1311+x1310+new_r00);
16450 evalcond[3]=((((-1.0)*new_r10*x1312))+gconst16+((new_r00*x1307)));
16451 evalcond[4]=((((-1.0)*x1310))+(((-1.0)*x1311)));
16452 evalcond[5]=(x1309+(((-1.0)*x1313))+new_r10);
16453 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16454 {
16455 continue;
16456 }
16457 }
16458 
16459 {
16460 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16461 vinfos[0].jointtype = 1;
16462 vinfos[0].foffset = j0;
16463 vinfos[0].indices[0] = _ij0[0];
16464 vinfos[0].indices[1] = _ij0[1];
16465 vinfos[0].maxsolutions = _nj0;
16466 vinfos[1].jointtype = 1;
16467 vinfos[1].foffset = j1;
16468 vinfos[1].indices[0] = _ij1[0];
16469 vinfos[1].indices[1] = _ij1[1];
16470 vinfos[1].maxsolutions = _nj1;
16471 vinfos[2].jointtype = 1;
16472 vinfos[2].foffset = j2;
16473 vinfos[2].indices[0] = _ij2[0];
16474 vinfos[2].indices[1] = _ij2[1];
16475 vinfos[2].maxsolutions = _nj2;
16476 vinfos[3].jointtype = 1;
16477 vinfos[3].foffset = j3;
16478 vinfos[3].indices[0] = _ij3[0];
16479 vinfos[3].indices[1] = _ij3[1];
16480 vinfos[3].maxsolutions = _nj3;
16481 vinfos[4].jointtype = 1;
16482 vinfos[4].foffset = j4;
16483 vinfos[4].indices[0] = _ij4[0];
16484 vinfos[4].indices[1] = _ij4[1];
16485 vinfos[4].maxsolutions = _nj4;
16486 vinfos[5].jointtype = 1;
16487 vinfos[5].foffset = j5;
16488 vinfos[5].indices[0] = _ij5[0];
16489 vinfos[5].indices[1] = _ij5[1];
16490 vinfos[5].maxsolutions = _nj5;
16491 std::vector<int> vfree(0);
16492 solutions.AddSolution(vinfos,vfree);
16493 }
16494 }
16495 }
16496 
16497 }
16498 
16499 }
16500 
16501 } else
16502 {
16503 {
16504 IkReal j3array[1], cj3array[1], sj3array[1];
16505 bool j3valid[1]={false};
16506 _nj3 = 1;
16507 CheckValue<IkReal> x1314=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r00))+((gconst17*new_r10)))),-1);
16508 if(!x1314.valid){
16509 continue;
16510 }
16511 CheckValue<IkReal> x1315 = IKatan2WithCheck(IkReal(gconst16*gconst16),IkReal((gconst16*gconst17)),IKFAST_ATAN2_MAGTHRESH);
16512 if(!x1315.valid){
16513 continue;
16514 }
16515 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1314.value)))+(x1315.value));
16516 sj3array[0]=IKsin(j3array[0]);
16517 cj3array[0]=IKcos(j3array[0]);
16518 if( j3array[0] > IKPI )
16519 {
16520  j3array[0]-=IK2PI;
16521 }
16522 else if( j3array[0] < -IKPI )
16523 { j3array[0]+=IK2PI;
16524 }
16525 j3valid[0] = true;
16526 for(int ij3 = 0; ij3 < 1; ++ij3)
16527 {
16528 if( !j3valid[ij3] )
16529 {
16530  continue;
16531 }
16532 _ij3[0] = ij3; _ij3[1] = -1;
16533 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16534 {
16535 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16536 {
16537  j3valid[iij3]=false; _ij3[1] = iij3; break;
16538 }
16539 }
16540 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16541 {
16542 IkReal evalcond[6];
16543 IkReal x1316=IKsin(j3);
16544 IkReal x1317=IKcos(j3);
16545 IkReal x1318=(gconst17*x1316);
16546 IkReal x1319=(gconst16*x1316);
16547 IkReal x1320=(gconst17*x1317);
16548 IkReal x1321=((1.0)*x1317);
16549 IkReal x1322=(gconst16*x1321);
16550 evalcond[0]=(x1318+(((-1.0)*x1322)));
16551 evalcond[1]=(((new_r00*x1317))+gconst17+((new_r10*x1316)));
16552 evalcond[2]=(x1319+x1320+new_r00);
16553 evalcond[3]=(((new_r00*x1316))+(((-1.0)*new_r10*x1321))+gconst16);
16554 evalcond[4]=((((-1.0)*x1319))+(((-1.0)*x1320)));
16555 evalcond[5]=(x1318+(((-1.0)*x1322))+new_r10);
16556 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16557 {
16558 continue;
16559 }
16560 }
16561 
16562 {
16563 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16564 vinfos[0].jointtype = 1;
16565 vinfos[0].foffset = j0;
16566 vinfos[0].indices[0] = _ij0[0];
16567 vinfos[0].indices[1] = _ij0[1];
16568 vinfos[0].maxsolutions = _nj0;
16569 vinfos[1].jointtype = 1;
16570 vinfos[1].foffset = j1;
16571 vinfos[1].indices[0] = _ij1[0];
16572 vinfos[1].indices[1] = _ij1[1];
16573 vinfos[1].maxsolutions = _nj1;
16574 vinfos[2].jointtype = 1;
16575 vinfos[2].foffset = j2;
16576 vinfos[2].indices[0] = _ij2[0];
16577 vinfos[2].indices[1] = _ij2[1];
16578 vinfos[2].maxsolutions = _nj2;
16579 vinfos[3].jointtype = 1;
16580 vinfos[3].foffset = j3;
16581 vinfos[3].indices[0] = _ij3[0];
16582 vinfos[3].indices[1] = _ij3[1];
16583 vinfos[3].maxsolutions = _nj3;
16584 vinfos[4].jointtype = 1;
16585 vinfos[4].foffset = j4;
16586 vinfos[4].indices[0] = _ij4[0];
16587 vinfos[4].indices[1] = _ij4[1];
16588 vinfos[4].maxsolutions = _nj4;
16589 vinfos[5].jointtype = 1;
16590 vinfos[5].foffset = j5;
16591 vinfos[5].indices[0] = _ij5[0];
16592 vinfos[5].indices[1] = _ij5[1];
16593 vinfos[5].maxsolutions = _nj5;
16594 std::vector<int> vfree(0);
16595 solutions.AddSolution(vinfos,vfree);
16596 }
16597 }
16598 }
16599 
16600 }
16601 
16602 }
16603 
16604 }
16605 } while(0);
16606 if( bgotonextstatement )
16607 {
16608 bool bgotonextstatement = true;
16609 do
16610 {
16611 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
16612 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16613 {
16614 bgotonextstatement=false;
16615 {
16616 IkReal j3array[2], cj3array[2], sj3array[2];
16617 bool j3valid[2]={false};
16618 _nj3 = 2;
16619 CheckValue<IkReal> x1323=IKPowWithIntegerCheck(gconst16,-1);
16620 if(!x1323.valid){
16621 continue;
16622 }
16623 sj3array[0]=(new_r11*(x1323.value));
16624 if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16625 {
16626  j3valid[0] = j3valid[1] = true;
16627  j3array[0] = IKasin(sj3array[0]);
16628  cj3array[0] = IKcos(j3array[0]);
16629  sj3array[1] = sj3array[0];
16630  j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16631  cj3array[1] = -cj3array[0];
16632 }
16633 else if( isnan(sj3array[0]) )
16634 {
16635  // probably any value will work
16636  j3valid[0] = true;
16637  cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16638 }
16639 for(int ij3 = 0; ij3 < 2; ++ij3)
16640 {
16641 if( !j3valid[ij3] )
16642 {
16643  continue;
16644 }
16645 _ij3[0] = ij3; _ij3[1] = -1;
16646 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16647 {
16648 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16649 {
16650  j3valid[iij3]=false; _ij3[1] = iij3; break;
16651 }
16652 }
16653 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16654 {
16655 IkReal evalcond[6];
16656 IkReal x1324=IKcos(j3);
16657 IkReal x1325=IKsin(j3);
16658 IkReal x1326=((-1.0)*x1324);
16659 evalcond[0]=(new_r00*x1324);
16660 evalcond[1]=(new_r11*x1326);
16661 evalcond[2]=(gconst16*x1326);
16662 evalcond[3]=(((new_r00*x1325))+gconst16);
16663 evalcond[4]=(((gconst16*x1325))+new_r00);
16664 evalcond[5]=(((new_r11*x1325))+(((-1.0)*gconst16)));
16665 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16666 {
16667 continue;
16668 }
16669 }
16670 
16671 {
16672 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16673 vinfos[0].jointtype = 1;
16674 vinfos[0].foffset = j0;
16675 vinfos[0].indices[0] = _ij0[0];
16676 vinfos[0].indices[1] = _ij0[1];
16677 vinfos[0].maxsolutions = _nj0;
16678 vinfos[1].jointtype = 1;
16679 vinfos[1].foffset = j1;
16680 vinfos[1].indices[0] = _ij1[0];
16681 vinfos[1].indices[1] = _ij1[1];
16682 vinfos[1].maxsolutions = _nj1;
16683 vinfos[2].jointtype = 1;
16684 vinfos[2].foffset = j2;
16685 vinfos[2].indices[0] = _ij2[0];
16686 vinfos[2].indices[1] = _ij2[1];
16687 vinfos[2].maxsolutions = _nj2;
16688 vinfos[3].jointtype = 1;
16689 vinfos[3].foffset = j3;
16690 vinfos[3].indices[0] = _ij3[0];
16691 vinfos[3].indices[1] = _ij3[1];
16692 vinfos[3].maxsolutions = _nj3;
16693 vinfos[4].jointtype = 1;
16694 vinfos[4].foffset = j4;
16695 vinfos[4].indices[0] = _ij4[0];
16696 vinfos[4].indices[1] = _ij4[1];
16697 vinfos[4].maxsolutions = _nj4;
16698 vinfos[5].jointtype = 1;
16699 vinfos[5].foffset = j5;
16700 vinfos[5].indices[0] = _ij5[0];
16701 vinfos[5].indices[1] = _ij5[1];
16702 vinfos[5].maxsolutions = _nj5;
16703 std::vector<int> vfree(0);
16704 solutions.AddSolution(vinfos,vfree);
16705 }
16706 }
16707 }
16708 
16709 }
16710 } while(0);
16711 if( bgotonextstatement )
16712 {
16713 bool bgotonextstatement = true;
16714 do
16715 {
16716 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
16717 if( IKabs(evalcond[0]) < 0.0000050000000000 )
16718 {
16719 bgotonextstatement=false;
16720 {
16721 IkReal j3eval[1];
16722 IkReal x1327=((-1.0)*new_r00);
16723 CheckValue<IkReal> x1329 = IKatan2WithCheck(IkReal(x1327),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16724 if(!x1329.valid){
16725 continue;
16726 }
16727 IkReal x1328=((1.0)*(x1329.value));
16728 sj4=0;
16729 cj4=-1.0;
16730 j4=3.14159265358979;
16731 sj5=gconst16;
16732 cj5=gconst17;
16733 j5=((3.14159265)+(((-1.0)*x1328)));
16734 new_r11=0;
16735 new_r10=0;
16736 new_r22=0;
16737 new_r02=0;
16738 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1328)));
16739 IkReal x1330 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16740 if(IKabs(x1330)==0){
16741 continue;
16742 }
16743 IkReal gconst16=(x1327*(pow(x1330,-0.5)));
16744 IkReal gconst17=0;
16745 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
16746 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16747 {
16748 {
16749 IkReal j3eval[1];
16750 IkReal x1331=((-1.0)*new_r00);
16751 CheckValue<IkReal> x1333 = IKatan2WithCheck(IkReal(x1331),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16752 if(!x1333.valid){
16753 continue;
16754 }
16755 IkReal x1332=((1.0)*(x1333.value));
16756 sj4=0;
16757 cj4=-1.0;
16758 j4=3.14159265358979;
16759 sj5=gconst16;
16760 cj5=gconst17;
16761 j5=((3.14159265)+(((-1.0)*x1332)));
16762 new_r11=0;
16763 new_r10=0;
16764 new_r22=0;
16765 new_r02=0;
16766 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1332)));
16767 IkReal x1334 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16768 if(IKabs(x1334)==0){
16769 continue;
16770 }
16771 IkReal gconst16=(x1331*(pow(x1334,-0.5)));
16772 IkReal gconst17=0;
16773 j3eval[0]=new_r00;
16774 if( IKabs(j3eval[0]) < 0.0000010000000000 )
16775 {
16776 {
16777 IkReal j3eval[2];
16778 IkReal x1335=((-1.0)*new_r00);
16779 CheckValue<IkReal> x1337 = IKatan2WithCheck(IkReal(x1335),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16780 if(!x1337.valid){
16781 continue;
16782 }
16783 IkReal x1336=((1.0)*(x1337.value));
16784 sj4=0;
16785 cj4=-1.0;
16786 j4=3.14159265358979;
16787 sj5=gconst16;
16788 cj5=gconst17;
16789 j5=((3.14159265)+(((-1.0)*x1336)));
16790 new_r11=0;
16791 new_r10=0;
16792 new_r22=0;
16793 new_r02=0;
16794 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1336)));
16795 IkReal x1338 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16796 if(IKabs(x1338)==0){
16797 continue;
16798 }
16799 IkReal gconst16=(x1335*(pow(x1338,-0.5)));
16800 IkReal gconst17=0;
16801 j3eval[0]=new_r00;
16802 j3eval[1]=new_r01;
16803 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
16804 {
16805 continue; // 3 cases reached
16806 
16807 } else
16808 {
16809 {
16810 IkReal j3array[1], cj3array[1], sj3array[1];
16811 bool j3valid[1]={false};
16812 _nj3 = 1;
16813 CheckValue<IkReal> x1339=IKPowWithIntegerCheck(new_r00,-1);
16814 if(!x1339.valid){
16815 continue;
16816 }
16817 CheckValue<IkReal> x1340=IKPowWithIntegerCheck(new_r01,-1);
16818 if(!x1340.valid){
16819 continue;
16820 }
16821 if( IKabs(((-1.0)*gconst16*(x1339.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst16*(x1340.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1339.value)))+IKsqr((gconst16*(x1340.value)))-1) <= IKFAST_SINCOS_THRESH )
16822  continue;
16823 j3array[0]=IKatan2(((-1.0)*gconst16*(x1339.value)), (gconst16*(x1340.value)));
16824 sj3array[0]=IKsin(j3array[0]);
16825 cj3array[0]=IKcos(j3array[0]);
16826 if( j3array[0] > IKPI )
16827 {
16828  j3array[0]-=IK2PI;
16829 }
16830 else if( j3array[0] < -IKPI )
16831 { j3array[0]+=IK2PI;
16832 }
16833 j3valid[0] = true;
16834 for(int ij3 = 0; ij3 < 1; ++ij3)
16835 {
16836 if( !j3valid[ij3] )
16837 {
16838  continue;
16839 }
16840 _ij3[0] = ij3; _ij3[1] = -1;
16841 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16842 {
16843 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16844 {
16845  j3valid[iij3]=false; _ij3[1] = iij3; break;
16846 }
16847 }
16848 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16849 {
16850 IkReal evalcond[8];
16851 IkReal x1341=IKsin(j3);
16852 IkReal x1342=IKcos(j3);
16853 IkReal x1343=((1.0)*gconst16);
16854 IkReal x1344=((-1.0)*gconst16);
16855 evalcond[0]=(new_r01*x1341);
16856 evalcond[1]=(new_r00*x1342);
16857 evalcond[2]=(x1341*x1344);
16858 evalcond[3]=(x1342*x1344);
16859 evalcond[4]=(gconst16+((new_r00*x1341)));
16860 evalcond[5]=(((gconst16*x1341))+new_r00);
16861 evalcond[6]=((((-1.0)*x1342*x1343))+new_r01);
16862 evalcond[7]=((((-1.0)*x1343))+((new_r01*x1342)));
16863 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16864 {
16865 continue;
16866 }
16867 }
16868 
16869 {
16870 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16871 vinfos[0].jointtype = 1;
16872 vinfos[0].foffset = j0;
16873 vinfos[0].indices[0] = _ij0[0];
16874 vinfos[0].indices[1] = _ij0[1];
16875 vinfos[0].maxsolutions = _nj0;
16876 vinfos[1].jointtype = 1;
16877 vinfos[1].foffset = j1;
16878 vinfos[1].indices[0] = _ij1[0];
16879 vinfos[1].indices[1] = _ij1[1];
16880 vinfos[1].maxsolutions = _nj1;
16881 vinfos[2].jointtype = 1;
16882 vinfos[2].foffset = j2;
16883 vinfos[2].indices[0] = _ij2[0];
16884 vinfos[2].indices[1] = _ij2[1];
16885 vinfos[2].maxsolutions = _nj2;
16886 vinfos[3].jointtype = 1;
16887 vinfos[3].foffset = j3;
16888 vinfos[3].indices[0] = _ij3[0];
16889 vinfos[3].indices[1] = _ij3[1];
16890 vinfos[3].maxsolutions = _nj3;
16891 vinfos[4].jointtype = 1;
16892 vinfos[4].foffset = j4;
16893 vinfos[4].indices[0] = _ij4[0];
16894 vinfos[4].indices[1] = _ij4[1];
16895 vinfos[4].maxsolutions = _nj4;
16896 vinfos[5].jointtype = 1;
16897 vinfos[5].foffset = j5;
16898 vinfos[5].indices[0] = _ij5[0];
16899 vinfos[5].indices[1] = _ij5[1];
16900 vinfos[5].maxsolutions = _nj5;
16901 std::vector<int> vfree(0);
16902 solutions.AddSolution(vinfos,vfree);
16903 }
16904 }
16905 }
16906 
16907 }
16908 
16909 }
16910 
16911 } else
16912 {
16913 {
16914 IkReal j3array[1], cj3array[1], sj3array[1];
16915 bool j3valid[1]={false};
16916 _nj3 = 1;
16917 CheckValue<IkReal> x1345=IKPowWithIntegerCheck(new_r00,-1);
16918 if(!x1345.valid){
16919 continue;
16920 }
16921 CheckValue<IkReal> x1346=IKPowWithIntegerCheck(gconst16,-1);
16922 if(!x1346.valid){
16923 continue;
16924 }
16925 if( IKabs(((-1.0)*gconst16*(x1345.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1346.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1345.value)))+IKsqr((new_r01*(x1346.value)))-1) <= IKFAST_SINCOS_THRESH )
16926  continue;
16927 j3array[0]=IKatan2(((-1.0)*gconst16*(x1345.value)), (new_r01*(x1346.value)));
16928 sj3array[0]=IKsin(j3array[0]);
16929 cj3array[0]=IKcos(j3array[0]);
16930 if( j3array[0] > IKPI )
16931 {
16932  j3array[0]-=IK2PI;
16933 }
16934 else if( j3array[0] < -IKPI )
16935 { j3array[0]+=IK2PI;
16936 }
16937 j3valid[0] = true;
16938 for(int ij3 = 0; ij3 < 1; ++ij3)
16939 {
16940 if( !j3valid[ij3] )
16941 {
16942  continue;
16943 }
16944 _ij3[0] = ij3; _ij3[1] = -1;
16945 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16946 {
16947 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16948 {
16949  j3valid[iij3]=false; _ij3[1] = iij3; break;
16950 }
16951 }
16952 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16953 {
16954 IkReal evalcond[8];
16955 IkReal x1347=IKsin(j3);
16956 IkReal x1348=IKcos(j3);
16957 IkReal x1349=((1.0)*gconst16);
16958 IkReal x1350=((-1.0)*gconst16);
16959 evalcond[0]=(new_r01*x1347);
16960 evalcond[1]=(new_r00*x1348);
16961 evalcond[2]=(x1347*x1350);
16962 evalcond[3]=(x1348*x1350);
16963 evalcond[4]=(gconst16+((new_r00*x1347)));
16964 evalcond[5]=(((gconst16*x1347))+new_r00);
16965 evalcond[6]=((((-1.0)*x1348*x1349))+new_r01);
16966 evalcond[7]=((((-1.0)*x1349))+((new_r01*x1348)));
16967 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16968 {
16969 continue;
16970 }
16971 }
16972 
16973 {
16974 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16975 vinfos[0].jointtype = 1;
16976 vinfos[0].foffset = j0;
16977 vinfos[0].indices[0] = _ij0[0];
16978 vinfos[0].indices[1] = _ij0[1];
16979 vinfos[0].maxsolutions = _nj0;
16980 vinfos[1].jointtype = 1;
16981 vinfos[1].foffset = j1;
16982 vinfos[1].indices[0] = _ij1[0];
16983 vinfos[1].indices[1] = _ij1[1];
16984 vinfos[1].maxsolutions = _nj1;
16985 vinfos[2].jointtype = 1;
16986 vinfos[2].foffset = j2;
16987 vinfos[2].indices[0] = _ij2[0];
16988 vinfos[2].indices[1] = _ij2[1];
16989 vinfos[2].maxsolutions = _nj2;
16990 vinfos[3].jointtype = 1;
16991 vinfos[3].foffset = j3;
16992 vinfos[3].indices[0] = _ij3[0];
16993 vinfos[3].indices[1] = _ij3[1];
16994 vinfos[3].maxsolutions = _nj3;
16995 vinfos[4].jointtype = 1;
16996 vinfos[4].foffset = j4;
16997 vinfos[4].indices[0] = _ij4[0];
16998 vinfos[4].indices[1] = _ij4[1];
16999 vinfos[4].maxsolutions = _nj4;
17000 vinfos[5].jointtype = 1;
17001 vinfos[5].foffset = j5;
17002 vinfos[5].indices[0] = _ij5[0];
17003 vinfos[5].indices[1] = _ij5[1];
17004 vinfos[5].maxsolutions = _nj5;
17005 std::vector<int> vfree(0);
17006 solutions.AddSolution(vinfos,vfree);
17007 }
17008 }
17009 }
17010 
17011 }
17012 
17013 }
17014 
17015 } else
17016 {
17017 {
17018 IkReal j3array[1], cj3array[1], sj3array[1];
17019 bool j3valid[1]={false};
17020 _nj3 = 1;
17021 CheckValue<IkReal> x1351 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17022 if(!x1351.valid){
17023 continue;
17024 }
17025 CheckValue<IkReal> x1352=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17026 if(!x1352.valid){
17027 continue;
17028 }
17029 j3array[0]=((-1.5707963267949)+(x1351.value)+(((1.5707963267949)*(x1352.value))));
17030 sj3array[0]=IKsin(j3array[0]);
17031 cj3array[0]=IKcos(j3array[0]);
17032 if( j3array[0] > IKPI )
17033 {
17034  j3array[0]-=IK2PI;
17035 }
17036 else if( j3array[0] < -IKPI )
17037 { j3array[0]+=IK2PI;
17038 }
17039 j3valid[0] = true;
17040 for(int ij3 = 0; ij3 < 1; ++ij3)
17041 {
17042 if( !j3valid[ij3] )
17043 {
17044  continue;
17045 }
17046 _ij3[0] = ij3; _ij3[1] = -1;
17047 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17048 {
17049 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17050 {
17051  j3valid[iij3]=false; _ij3[1] = iij3; break;
17052 }
17053 }
17054 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17055 {
17056 IkReal evalcond[8];
17057 IkReal x1353=IKsin(j3);
17058 IkReal x1354=IKcos(j3);
17059 IkReal x1355=((1.0)*gconst16);
17060 IkReal x1356=((-1.0)*gconst16);
17061 evalcond[0]=(new_r01*x1353);
17062 evalcond[1]=(new_r00*x1354);
17063 evalcond[2]=(x1353*x1356);
17064 evalcond[3]=(x1354*x1356);
17065 evalcond[4]=(gconst16+((new_r00*x1353)));
17066 evalcond[5]=(new_r00+((gconst16*x1353)));
17067 evalcond[6]=((((-1.0)*x1354*x1355))+new_r01);
17068 evalcond[7]=((((-1.0)*x1355))+((new_r01*x1354)));
17069 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17070 {
17071 continue;
17072 }
17073 }
17074 
17075 {
17076 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17077 vinfos[0].jointtype = 1;
17078 vinfos[0].foffset = j0;
17079 vinfos[0].indices[0] = _ij0[0];
17080 vinfos[0].indices[1] = _ij0[1];
17081 vinfos[0].maxsolutions = _nj0;
17082 vinfos[1].jointtype = 1;
17083 vinfos[1].foffset = j1;
17084 vinfos[1].indices[0] = _ij1[0];
17085 vinfos[1].indices[1] = _ij1[1];
17086 vinfos[1].maxsolutions = _nj1;
17087 vinfos[2].jointtype = 1;
17088 vinfos[2].foffset = j2;
17089 vinfos[2].indices[0] = _ij2[0];
17090 vinfos[2].indices[1] = _ij2[1];
17091 vinfos[2].maxsolutions = _nj2;
17092 vinfos[3].jointtype = 1;
17093 vinfos[3].foffset = j3;
17094 vinfos[3].indices[0] = _ij3[0];
17095 vinfos[3].indices[1] = _ij3[1];
17096 vinfos[3].maxsolutions = _nj3;
17097 vinfos[4].jointtype = 1;
17098 vinfos[4].foffset = j4;
17099 vinfos[4].indices[0] = _ij4[0];
17100 vinfos[4].indices[1] = _ij4[1];
17101 vinfos[4].maxsolutions = _nj4;
17102 vinfos[5].jointtype = 1;
17103 vinfos[5].foffset = j5;
17104 vinfos[5].indices[0] = _ij5[0];
17105 vinfos[5].indices[1] = _ij5[1];
17106 vinfos[5].maxsolutions = _nj5;
17107 std::vector<int> vfree(0);
17108 solutions.AddSolution(vinfos,vfree);
17109 }
17110 }
17111 }
17112 
17113 }
17114 
17115 }
17116 
17117 }
17118 } while(0);
17119 if( bgotonextstatement )
17120 {
17121 bool bgotonextstatement = true;
17122 do
17123 {
17124 evalcond[0]=IKabs(new_r10);
17125 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17126 {
17127 bgotonextstatement=false;
17128 {
17129 IkReal j3eval[1];
17130 IkReal x1357=((-1.0)*new_r00);
17131 CheckValue<IkReal> x1359 = IKatan2WithCheck(IkReal(x1357),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17132 if(!x1359.valid){
17133 continue;
17134 }
17135 IkReal x1358=((1.0)*(x1359.value));
17136 sj4=0;
17137 cj4=-1.0;
17138 j4=3.14159265358979;
17139 sj5=gconst16;
17140 cj5=gconst17;
17141 j5=((3.14159265)+(((-1.0)*x1358)));
17142 new_r10=0;
17143 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1358)));
17144 IkReal x1360 = new_r00*new_r00;
17145 if(IKabs(x1360)==0){
17146 continue;
17147 }
17148 IkReal gconst16=(x1357*(pow(x1360,-0.5)));
17149 IkReal gconst17=0;
17150 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
17151 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17152 {
17153 {
17154 IkReal j3eval[1];
17155 IkReal x1361=((-1.0)*new_r00);
17156 CheckValue<IkReal> x1363 = IKatan2WithCheck(IkReal(x1361),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17157 if(!x1363.valid){
17158 continue;
17159 }
17160 IkReal x1362=((1.0)*(x1363.value));
17161 sj4=0;
17162 cj4=-1.0;
17163 j4=3.14159265358979;
17164 sj5=gconst16;
17165 cj5=gconst17;
17166 j5=((3.14159265)+(((-1.0)*x1362)));
17167 new_r10=0;
17168 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1362)));
17169 IkReal x1364 = new_r00*new_r00;
17170 if(IKabs(x1364)==0){
17171 continue;
17172 }
17173 IkReal gconst16=(x1361*(pow(x1364,-0.5)));
17174 IkReal gconst17=0;
17175 j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
17176 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17177 {
17178 {
17179 IkReal j3eval[1];
17180 IkReal x1365=((-1.0)*new_r00);
17181 CheckValue<IkReal> x1367 = IKatan2WithCheck(IkReal(x1365),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17182 if(!x1367.valid){
17183 continue;
17184 }
17185 IkReal x1366=((1.0)*(x1367.value));
17186 sj4=0;
17187 cj4=-1.0;
17188 j4=3.14159265358979;
17189 sj5=gconst16;
17190 cj5=gconst17;
17191 j5=((3.14159265)+(((-1.0)*x1366)));
17192 new_r10=0;
17193 IkReal gconst15=((3.14159265358979)+(((-1.0)*x1366)));
17194 IkReal x1368 = new_r00*new_r00;
17195 if(IKabs(x1368)==0){
17196 continue;
17197 }
17198 IkReal gconst16=(x1365*(pow(x1368,-0.5)));
17199 IkReal gconst17=0;
17200 j3eval[0]=new_r00;
17201 if( IKabs(j3eval[0]) < 0.0000010000000000 )
17202 {
17203 continue; // 3 cases reached
17204 
17205 } else
17206 {
17207 {
17208 IkReal j3array[1], cj3array[1], sj3array[1];
17209 bool j3valid[1]={false};
17210 _nj3 = 1;
17211 CheckValue<IkReal> x1369=IKPowWithIntegerCheck(new_r00,-1);
17212 if(!x1369.valid){
17213 continue;
17214 }
17215 CheckValue<IkReal> x1370=IKPowWithIntegerCheck(gconst16,-1);
17216 if(!x1370.valid){
17217 continue;
17218 }
17219 if( IKabs(((-1.0)*gconst16*(x1369.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1370.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1369.value)))+IKsqr((new_r01*(x1370.value)))-1) <= IKFAST_SINCOS_THRESH )
17220  continue;
17221 j3array[0]=IKatan2(((-1.0)*gconst16*(x1369.value)), (new_r01*(x1370.value)));
17222 sj3array[0]=IKsin(j3array[0]);
17223 cj3array[0]=IKcos(j3array[0]);
17224 if( j3array[0] > IKPI )
17225 {
17226  j3array[0]-=IK2PI;
17227 }
17228 else if( j3array[0] < -IKPI )
17229 { j3array[0]+=IK2PI;
17230 }
17231 j3valid[0] = true;
17232 for(int ij3 = 0; ij3 < 1; ++ij3)
17233 {
17234 if( !j3valid[ij3] )
17235 {
17236  continue;
17237 }
17238 _ij3[0] = ij3; _ij3[1] = -1;
17239 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17240 {
17241 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17242 {
17243  j3valid[iij3]=false; _ij3[1] = iij3; break;
17244 }
17245 }
17246 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17247 {
17248 IkReal evalcond[8];
17249 IkReal x1371=IKcos(j3);
17250 IkReal x1372=IKsin(j3);
17251 IkReal x1373=((1.0)*gconst16);
17252 evalcond[0]=(new_r00*x1371);
17253 evalcond[1]=((-1.0)*gconst16*x1371);
17254 evalcond[2]=(gconst16+((new_r00*x1372)));
17255 evalcond[3]=(new_r00+((gconst16*x1372)));
17256 evalcond[4]=((((-1.0)*x1371*x1373))+new_r01);
17257 evalcond[5]=((((-1.0)*x1372*x1373))+new_r11);
17258 evalcond[6]=(((new_r01*x1372))+(((-1.0)*new_r11*x1371)));
17259 evalcond[7]=((((-1.0)*x1373))+((new_r11*x1372))+((new_r01*x1371)));
17260 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17261 {
17262 continue;
17263 }
17264 }
17265 
17266 {
17267 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17268 vinfos[0].jointtype = 1;
17269 vinfos[0].foffset = j0;
17270 vinfos[0].indices[0] = _ij0[0];
17271 vinfos[0].indices[1] = _ij0[1];
17272 vinfos[0].maxsolutions = _nj0;
17273 vinfos[1].jointtype = 1;
17274 vinfos[1].foffset = j1;
17275 vinfos[1].indices[0] = _ij1[0];
17276 vinfos[1].indices[1] = _ij1[1];
17277 vinfos[1].maxsolutions = _nj1;
17278 vinfos[2].jointtype = 1;
17279 vinfos[2].foffset = j2;
17280 vinfos[2].indices[0] = _ij2[0];
17281 vinfos[2].indices[1] = _ij2[1];
17282 vinfos[2].maxsolutions = _nj2;
17283 vinfos[3].jointtype = 1;
17284 vinfos[3].foffset = j3;
17285 vinfos[3].indices[0] = _ij3[0];
17286 vinfos[3].indices[1] = _ij3[1];
17287 vinfos[3].maxsolutions = _nj3;
17288 vinfos[4].jointtype = 1;
17289 vinfos[4].foffset = j4;
17290 vinfos[4].indices[0] = _ij4[0];
17291 vinfos[4].indices[1] = _ij4[1];
17292 vinfos[4].maxsolutions = _nj4;
17293 vinfos[5].jointtype = 1;
17294 vinfos[5].foffset = j5;
17295 vinfos[5].indices[0] = _ij5[0];
17296 vinfos[5].indices[1] = _ij5[1];
17297 vinfos[5].maxsolutions = _nj5;
17298 std::vector<int> vfree(0);
17299 solutions.AddSolution(vinfos,vfree);
17300 }
17301 }
17302 }
17303 
17304 }
17305 
17306 }
17307 
17308 } else
17309 {
17310 {
17311 IkReal j3array[1], cj3array[1], sj3array[1];
17312 bool j3valid[1]={false};
17313 _nj3 = 1;
17314 CheckValue<IkReal> x1374 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17315 if(!x1374.valid){
17316 continue;
17317 }
17318 CheckValue<IkReal> x1375=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17319 if(!x1375.valid){
17320 continue;
17321 }
17322 j3array[0]=((-1.5707963267949)+(x1374.value)+(((1.5707963267949)*(x1375.value))));
17323 sj3array[0]=IKsin(j3array[0]);
17324 cj3array[0]=IKcos(j3array[0]);
17325 if( j3array[0] > IKPI )
17326 {
17327  j3array[0]-=IK2PI;
17328 }
17329 else if( j3array[0] < -IKPI )
17330 { j3array[0]+=IK2PI;
17331 }
17332 j3valid[0] = true;
17333 for(int ij3 = 0; ij3 < 1; ++ij3)
17334 {
17335 if( !j3valid[ij3] )
17336 {
17337  continue;
17338 }
17339 _ij3[0] = ij3; _ij3[1] = -1;
17340 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17341 {
17342 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17343 {
17344  j3valid[iij3]=false; _ij3[1] = iij3; break;
17345 }
17346 }
17347 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17348 {
17349 IkReal evalcond[8];
17350 IkReal x1376=IKcos(j3);
17351 IkReal x1377=IKsin(j3);
17352 IkReal x1378=((1.0)*gconst16);
17353 evalcond[0]=(new_r00*x1376);
17354 evalcond[1]=((-1.0)*gconst16*x1376);
17355 evalcond[2]=(gconst16+((new_r00*x1377)));
17356 evalcond[3]=(new_r00+((gconst16*x1377)));
17357 evalcond[4]=((((-1.0)*x1376*x1378))+new_r01);
17358 evalcond[5]=((((-1.0)*x1377*x1378))+new_r11);
17359 evalcond[6]=(((new_r01*x1377))+(((-1.0)*new_r11*x1376)));
17360 evalcond[7]=((((-1.0)*x1378))+((new_r11*x1377))+((new_r01*x1376)));
17361 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17362 {
17363 continue;
17364 }
17365 }
17366 
17367 {
17368 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17369 vinfos[0].jointtype = 1;
17370 vinfos[0].foffset = j0;
17371 vinfos[0].indices[0] = _ij0[0];
17372 vinfos[0].indices[1] = _ij0[1];
17373 vinfos[0].maxsolutions = _nj0;
17374 vinfos[1].jointtype = 1;
17375 vinfos[1].foffset = j1;
17376 vinfos[1].indices[0] = _ij1[0];
17377 vinfos[1].indices[1] = _ij1[1];
17378 vinfos[1].maxsolutions = _nj1;
17379 vinfos[2].jointtype = 1;
17380 vinfos[2].foffset = j2;
17381 vinfos[2].indices[0] = _ij2[0];
17382 vinfos[2].indices[1] = _ij2[1];
17383 vinfos[2].maxsolutions = _nj2;
17384 vinfos[3].jointtype = 1;
17385 vinfos[3].foffset = j3;
17386 vinfos[3].indices[0] = _ij3[0];
17387 vinfos[3].indices[1] = _ij3[1];
17388 vinfos[3].maxsolutions = _nj3;
17389 vinfos[4].jointtype = 1;
17390 vinfos[4].foffset = j4;
17391 vinfos[4].indices[0] = _ij4[0];
17392 vinfos[4].indices[1] = _ij4[1];
17393 vinfos[4].maxsolutions = _nj4;
17394 vinfos[5].jointtype = 1;
17395 vinfos[5].foffset = j5;
17396 vinfos[5].indices[0] = _ij5[0];
17397 vinfos[5].indices[1] = _ij5[1];
17398 vinfos[5].maxsolutions = _nj5;
17399 std::vector<int> vfree(0);
17400 solutions.AddSolution(vinfos,vfree);
17401 }
17402 }
17403 }
17404 
17405 }
17406 
17407 }
17408 
17409 } else
17410 {
17411 {
17412 IkReal j3array[1], cj3array[1], sj3array[1];
17413 bool j3valid[1]={false};
17414 _nj3 = 1;
17415 CheckValue<IkReal> x1379=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17416 if(!x1379.valid){
17417 continue;
17418 }
17419 CheckValue<IkReal> x1380 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17420 if(!x1380.valid){
17421 continue;
17422 }
17423 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1379.value)))+(x1380.value));
17424 sj3array[0]=IKsin(j3array[0]);
17425 cj3array[0]=IKcos(j3array[0]);
17426 if( j3array[0] > IKPI )
17427 {
17428  j3array[0]-=IK2PI;
17429 }
17430 else if( j3array[0] < -IKPI )
17431 { j3array[0]+=IK2PI;
17432 }
17433 j3valid[0] = true;
17434 for(int ij3 = 0; ij3 < 1; ++ij3)
17435 {
17436 if( !j3valid[ij3] )
17437 {
17438  continue;
17439 }
17440 _ij3[0] = ij3; _ij3[1] = -1;
17441 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17442 {
17443 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17444 {
17445  j3valid[iij3]=false; _ij3[1] = iij3; break;
17446 }
17447 }
17448 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17449 {
17450 IkReal evalcond[8];
17451 IkReal x1381=IKcos(j3);
17452 IkReal x1382=IKsin(j3);
17453 IkReal x1383=((1.0)*gconst16);
17454 evalcond[0]=(new_r00*x1381);
17455 evalcond[1]=((-1.0)*gconst16*x1381);
17456 evalcond[2]=(gconst16+((new_r00*x1382)));
17457 evalcond[3]=(((gconst16*x1382))+new_r00);
17458 evalcond[4]=((((-1.0)*x1381*x1383))+new_r01);
17459 evalcond[5]=((((-1.0)*x1382*x1383))+new_r11);
17460 evalcond[6]=((((-1.0)*new_r11*x1381))+((new_r01*x1382)));
17461 evalcond[7]=((((-1.0)*x1383))+((new_r11*x1382))+((new_r01*x1381)));
17462 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17463 {
17464 continue;
17465 }
17466 }
17467 
17468 {
17469 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17470 vinfos[0].jointtype = 1;
17471 vinfos[0].foffset = j0;
17472 vinfos[0].indices[0] = _ij0[0];
17473 vinfos[0].indices[1] = _ij0[1];
17474 vinfos[0].maxsolutions = _nj0;
17475 vinfos[1].jointtype = 1;
17476 vinfos[1].foffset = j1;
17477 vinfos[1].indices[0] = _ij1[0];
17478 vinfos[1].indices[1] = _ij1[1];
17479 vinfos[1].maxsolutions = _nj1;
17480 vinfos[2].jointtype = 1;
17481 vinfos[2].foffset = j2;
17482 vinfos[2].indices[0] = _ij2[0];
17483 vinfos[2].indices[1] = _ij2[1];
17484 vinfos[2].maxsolutions = _nj2;
17485 vinfos[3].jointtype = 1;
17486 vinfos[3].foffset = j3;
17487 vinfos[3].indices[0] = _ij3[0];
17488 vinfos[3].indices[1] = _ij3[1];
17489 vinfos[3].maxsolutions = _nj3;
17490 vinfos[4].jointtype = 1;
17491 vinfos[4].foffset = j4;
17492 vinfos[4].indices[0] = _ij4[0];
17493 vinfos[4].indices[1] = _ij4[1];
17494 vinfos[4].maxsolutions = _nj4;
17495 vinfos[5].jointtype = 1;
17496 vinfos[5].foffset = j5;
17497 vinfos[5].indices[0] = _ij5[0];
17498 vinfos[5].indices[1] = _ij5[1];
17499 vinfos[5].maxsolutions = _nj5;
17500 std::vector<int> vfree(0);
17501 solutions.AddSolution(vinfos,vfree);
17502 }
17503 }
17504 }
17505 
17506 }
17507 
17508 }
17509 
17510 }
17511 } while(0);
17512 if( bgotonextstatement )
17513 {
17514 bool bgotonextstatement = true;
17515 do
17516 {
17517 if( 1 )
17518 {
17519 bgotonextstatement=false;
17520 continue; // branch miss [j3]
17521 
17522 }
17523 } while(0);
17524 if( bgotonextstatement )
17525 {
17526 }
17527 }
17528 }
17529 }
17530 }
17531 }
17532 }
17533 
17534 } else
17535 {
17536 {
17537 IkReal j3array[1], cj3array[1], sj3array[1];
17538 bool j3valid[1]={false};
17539 _nj3 = 1;
17540 IkReal x1384=((1.0)*gconst17);
17541 CheckValue<IkReal> x1385=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1384)))),-1);
17542 if(!x1385.valid){
17543 continue;
17544 }
17545 CheckValue<IkReal> x1386 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst16*x1384)))),IKFAST_ATAN2_MAGTHRESH);
17546 if(!x1386.valid){
17547 continue;
17548 }
17549 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.value)))+(x1386.value));
17550 sj3array[0]=IKsin(j3array[0]);
17551 cj3array[0]=IKcos(j3array[0]);
17552 if( j3array[0] > IKPI )
17553 {
17554  j3array[0]-=IK2PI;
17555 }
17556 else if( j3array[0] < -IKPI )
17557 { j3array[0]+=IK2PI;
17558 }
17559 j3valid[0] = true;
17560 for(int ij3 = 0; ij3 < 1; ++ij3)
17561 {
17562 if( !j3valid[ij3] )
17563 {
17564  continue;
17565 }
17566 _ij3[0] = ij3; _ij3[1] = -1;
17567 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17568 {
17569 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17570 {
17571  j3valid[iij3]=false; _ij3[1] = iij3; break;
17572 }
17573 }
17574 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17575 {
17576 IkReal evalcond[8];
17577 IkReal x1387=IKsin(j3);
17578 IkReal x1388=IKcos(j3);
17579 IkReal x1389=((1.0)*gconst16);
17580 IkReal x1390=(gconst17*x1387);
17581 IkReal x1391=(gconst17*x1388);
17582 IkReal x1392=((1.0)*x1388);
17583 IkReal x1393=(x1388*x1389);
17584 evalcond[0]=(((new_r10*x1387))+gconst17+((new_r00*x1388)));
17585 evalcond[1]=(x1391+((gconst16*x1387))+new_r00);
17586 evalcond[2]=(gconst16+((new_r00*x1387))+(((-1.0)*new_r10*x1392)));
17587 evalcond[3]=(gconst17+((new_r01*x1387))+(((-1.0)*new_r11*x1392)));
17588 evalcond[4]=(x1390+new_r01+(((-1.0)*x1393)));
17589 evalcond[5]=(x1390+new_r10+(((-1.0)*x1393)));
17590 evalcond[6]=((((-1.0)*x1389))+((new_r11*x1387))+((new_r01*x1388)));
17591 evalcond[7]=((((-1.0)*x1391))+new_r11+(((-1.0)*x1387*x1389)));
17592 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17593 {
17594 continue;
17595 }
17596 }
17597 
17598 {
17599 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17600 vinfos[0].jointtype = 1;
17601 vinfos[0].foffset = j0;
17602 vinfos[0].indices[0] = _ij0[0];
17603 vinfos[0].indices[1] = _ij0[1];
17604 vinfos[0].maxsolutions = _nj0;
17605 vinfos[1].jointtype = 1;
17606 vinfos[1].foffset = j1;
17607 vinfos[1].indices[0] = _ij1[0];
17608 vinfos[1].indices[1] = _ij1[1];
17609 vinfos[1].maxsolutions = _nj1;
17610 vinfos[2].jointtype = 1;
17611 vinfos[2].foffset = j2;
17612 vinfos[2].indices[0] = _ij2[0];
17613 vinfos[2].indices[1] = _ij2[1];
17614 vinfos[2].maxsolutions = _nj2;
17615 vinfos[3].jointtype = 1;
17616 vinfos[3].foffset = j3;
17617 vinfos[3].indices[0] = _ij3[0];
17618 vinfos[3].indices[1] = _ij3[1];
17619 vinfos[3].maxsolutions = _nj3;
17620 vinfos[4].jointtype = 1;
17621 vinfos[4].foffset = j4;
17622 vinfos[4].indices[0] = _ij4[0];
17623 vinfos[4].indices[1] = _ij4[1];
17624 vinfos[4].maxsolutions = _nj4;
17625 vinfos[5].jointtype = 1;
17626 vinfos[5].foffset = j5;
17627 vinfos[5].indices[0] = _ij5[0];
17628 vinfos[5].indices[1] = _ij5[1];
17629 vinfos[5].maxsolutions = _nj5;
17630 std::vector<int> vfree(0);
17631 solutions.AddSolution(vinfos,vfree);
17632 }
17633 }
17634 }
17635 
17636 }
17637 
17638 }
17639 
17640 } else
17641 {
17642 {
17643 IkReal j3array[1], cj3array[1], sj3array[1];
17644 bool j3valid[1]={false};
17645 _nj3 = 1;
17646 IkReal x1394=((1.0)*gconst17);
17647 CheckValue<IkReal> x1395=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r11))+(((-1.0)*new_r01*x1394)))),-1);
17648 if(!x1395.valid){
17649 continue;
17650 }
17651 CheckValue<IkReal> x1396 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1394))+((new_r00*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17652 if(!x1396.valid){
17653 continue;
17654 }
17655 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1395.value)))+(x1396.value));
17656 sj3array[0]=IKsin(j3array[0]);
17657 cj3array[0]=IKcos(j3array[0]);
17658 if( j3array[0] > IKPI )
17659 {
17660  j3array[0]-=IK2PI;
17661 }
17662 else if( j3array[0] < -IKPI )
17663 { j3array[0]+=IK2PI;
17664 }
17665 j3valid[0] = true;
17666 for(int ij3 = 0; ij3 < 1; ++ij3)
17667 {
17668 if( !j3valid[ij3] )
17669 {
17670  continue;
17671 }
17672 _ij3[0] = ij3; _ij3[1] = -1;
17673 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17674 {
17675 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17676 {
17677  j3valid[iij3]=false; _ij3[1] = iij3; break;
17678 }
17679 }
17680 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17681 {
17682 IkReal evalcond[8];
17683 IkReal x1397=IKsin(j3);
17684 IkReal x1398=IKcos(j3);
17685 IkReal x1399=((1.0)*gconst16);
17686 IkReal x1400=(gconst17*x1397);
17687 IkReal x1401=(gconst17*x1398);
17688 IkReal x1402=((1.0)*x1398);
17689 IkReal x1403=(x1398*x1399);
17690 evalcond[0]=(((new_r10*x1397))+gconst17+((new_r00*x1398)));
17691 evalcond[1]=(x1401+new_r00+((gconst16*x1397)));
17692 evalcond[2]=(gconst16+((new_r00*x1397))+(((-1.0)*new_r10*x1402)));
17693 evalcond[3]=(gconst17+((new_r01*x1397))+(((-1.0)*new_r11*x1402)));
17694 evalcond[4]=((((-1.0)*x1403))+x1400+new_r01);
17695 evalcond[5]=((((-1.0)*x1403))+x1400+new_r10);
17696 evalcond[6]=(((new_r11*x1397))+((new_r01*x1398))+(((-1.0)*x1399)));
17697 evalcond[7]=((((-1.0)*x1401))+(((-1.0)*x1397*x1399))+new_r11);
17698 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17699 {
17700 continue;
17701 }
17702 }
17703 
17704 {
17705 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17706 vinfos[0].jointtype = 1;
17707 vinfos[0].foffset = j0;
17708 vinfos[0].indices[0] = _ij0[0];
17709 vinfos[0].indices[1] = _ij0[1];
17710 vinfos[0].maxsolutions = _nj0;
17711 vinfos[1].jointtype = 1;
17712 vinfos[1].foffset = j1;
17713 vinfos[1].indices[0] = _ij1[0];
17714 vinfos[1].indices[1] = _ij1[1];
17715 vinfos[1].maxsolutions = _nj1;
17716 vinfos[2].jointtype = 1;
17717 vinfos[2].foffset = j2;
17718 vinfos[2].indices[0] = _ij2[0];
17719 vinfos[2].indices[1] = _ij2[1];
17720 vinfos[2].maxsolutions = _nj2;
17721 vinfos[3].jointtype = 1;
17722 vinfos[3].foffset = j3;
17723 vinfos[3].indices[0] = _ij3[0];
17724 vinfos[3].indices[1] = _ij3[1];
17725 vinfos[3].maxsolutions = _nj3;
17726 vinfos[4].jointtype = 1;
17727 vinfos[4].foffset = j4;
17728 vinfos[4].indices[0] = _ij4[0];
17729 vinfos[4].indices[1] = _ij4[1];
17730 vinfos[4].maxsolutions = _nj4;
17731 vinfos[5].jointtype = 1;
17732 vinfos[5].foffset = j5;
17733 vinfos[5].indices[0] = _ij5[0];
17734 vinfos[5].indices[1] = _ij5[1];
17735 vinfos[5].maxsolutions = _nj5;
17736 std::vector<int> vfree(0);
17737 solutions.AddSolution(vinfos,vfree);
17738 }
17739 }
17740 }
17741 
17742 }
17743 
17744 }
17745 
17746 } else
17747 {
17748 {
17749 IkReal j3array[1], cj3array[1], sj3array[1];
17750 bool j3valid[1]={false};
17751 _nj3 = 1;
17752 IkReal x1404=((1.0)*new_r10);
17753 CheckValue<IkReal> x1405 = IKatan2WithCheck(IkReal((((gconst17*new_r11))+((gconst17*new_r00)))),IkReal(((((-1.0)*gconst17*x1404))+((gconst17*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17754 if(!x1405.valid){
17755 continue;
17756 }
17757 CheckValue<IkReal> x1406=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1404)))),-1);
17758 if(!x1406.valid){
17759 continue;
17760 }
17761 j3array[0]=((-1.5707963267949)+(x1405.value)+(((1.5707963267949)*(x1406.value))));
17762 sj3array[0]=IKsin(j3array[0]);
17763 cj3array[0]=IKcos(j3array[0]);
17764 if( j3array[0] > IKPI )
17765 {
17766  j3array[0]-=IK2PI;
17767 }
17768 else if( j3array[0] < -IKPI )
17769 { j3array[0]+=IK2PI;
17770 }
17771 j3valid[0] = true;
17772 for(int ij3 = 0; ij3 < 1; ++ij3)
17773 {
17774 if( !j3valid[ij3] )
17775 {
17776  continue;
17777 }
17778 _ij3[0] = ij3; _ij3[1] = -1;
17779 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17780 {
17781 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17782 {
17783  j3valid[iij3]=false; _ij3[1] = iij3; break;
17784 }
17785 }
17786 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17787 {
17788 IkReal evalcond[8];
17789 IkReal x1407=IKsin(j3);
17790 IkReal x1408=IKcos(j3);
17791 IkReal x1409=((1.0)*gconst16);
17792 IkReal x1410=(gconst17*x1407);
17793 IkReal x1411=(gconst17*x1408);
17794 IkReal x1412=((1.0)*x1408);
17795 IkReal x1413=(x1408*x1409);
17796 evalcond[0]=(gconst17+((new_r10*x1407))+((new_r00*x1408)));
17797 evalcond[1]=(((gconst16*x1407))+x1411+new_r00);
17798 evalcond[2]=((((-1.0)*new_r10*x1412))+gconst16+((new_r00*x1407)));
17799 evalcond[3]=((((-1.0)*new_r11*x1412))+((new_r01*x1407))+gconst17);
17800 evalcond[4]=((((-1.0)*x1413))+x1410+new_r01);
17801 evalcond[5]=((((-1.0)*x1413))+x1410+new_r10);
17802 evalcond[6]=(((new_r01*x1408))+((new_r11*x1407))+(((-1.0)*x1409)));
17803 evalcond[7]=((((-1.0)*x1407*x1409))+new_r11+(((-1.0)*x1411)));
17804 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17805 {
17806 continue;
17807 }
17808 }
17809 
17810 {
17811 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17812 vinfos[0].jointtype = 1;
17813 vinfos[0].foffset = j0;
17814 vinfos[0].indices[0] = _ij0[0];
17815 vinfos[0].indices[1] = _ij0[1];
17816 vinfos[0].maxsolutions = _nj0;
17817 vinfos[1].jointtype = 1;
17818 vinfos[1].foffset = j1;
17819 vinfos[1].indices[0] = _ij1[0];
17820 vinfos[1].indices[1] = _ij1[1];
17821 vinfos[1].maxsolutions = _nj1;
17822 vinfos[2].jointtype = 1;
17823 vinfos[2].foffset = j2;
17824 vinfos[2].indices[0] = _ij2[0];
17825 vinfos[2].indices[1] = _ij2[1];
17826 vinfos[2].maxsolutions = _nj2;
17827 vinfos[3].jointtype = 1;
17828 vinfos[3].foffset = j3;
17829 vinfos[3].indices[0] = _ij3[0];
17830 vinfos[3].indices[1] = _ij3[1];
17831 vinfos[3].maxsolutions = _nj3;
17832 vinfos[4].jointtype = 1;
17833 vinfos[4].foffset = j4;
17834 vinfos[4].indices[0] = _ij4[0];
17835 vinfos[4].indices[1] = _ij4[1];
17836 vinfos[4].maxsolutions = _nj4;
17837 vinfos[5].jointtype = 1;
17838 vinfos[5].foffset = j5;
17839 vinfos[5].indices[0] = _ij5[0];
17840 vinfos[5].indices[1] = _ij5[1];
17841 vinfos[5].maxsolutions = _nj5;
17842 std::vector<int> vfree(0);
17843 solutions.AddSolution(vinfos,vfree);
17844 }
17845 }
17846 }
17847 
17848 }
17849 
17850 }
17851 
17852 }
17853 } while(0);
17854 if( bgotonextstatement )
17855 {
17856 bool bgotonextstatement = true;
17857 do
17858 {
17859 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17860 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17861 {
17862 bgotonextstatement=false;
17863 {
17864 IkReal j3array[1], cj3array[1], sj3array[1];
17865 bool j3valid[1]={false};
17866 _nj3 = 1;
17867 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 )
17868  continue;
17869 j3array[0]=IKatan2(((-1.0)*new_r00), new_r01);
17870 sj3array[0]=IKsin(j3array[0]);
17871 cj3array[0]=IKcos(j3array[0]);
17872 if( j3array[0] > IKPI )
17873 {
17874  j3array[0]-=IK2PI;
17875 }
17876 else if( j3array[0] < -IKPI )
17877 { j3array[0]+=IK2PI;
17878 }
17879 j3valid[0] = true;
17880 for(int ij3 = 0; ij3 < 1; ++ij3)
17881 {
17882 if( !j3valid[ij3] )
17883 {
17884  continue;
17885 }
17886 _ij3[0] = ij3; _ij3[1] = -1;
17887 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17888 {
17889 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17890 {
17891  j3valid[iij3]=false; _ij3[1] = iij3; break;
17892 }
17893 }
17894 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17895 {
17896 IkReal evalcond[8];
17897 IkReal x1414=IKsin(j3);
17898 IkReal x1415=IKcos(j3);
17899 IkReal x1416=((1.0)*x1415);
17900 evalcond[0]=(x1414+new_r00);
17901 evalcond[1]=((((-1.0)*x1416))+new_r01);
17902 evalcond[2]=(new_r11+(((-1.0)*x1414)));
17903 evalcond[3]=((((-1.0)*x1416))+new_r10);
17904 evalcond[4]=(((new_r00*x1415))+((new_r10*x1414)));
17905 evalcond[5]=((((-1.0)*new_r11*x1416))+((new_r01*x1414)));
17906 evalcond[6]=((-1.0)+((new_r01*x1415))+((new_r11*x1414)));
17907 evalcond[7]=((1.0)+(((-1.0)*new_r10*x1416))+((new_r00*x1414)));
17908 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17909 {
17910 continue;
17911 }
17912 }
17913 
17914 {
17915 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17916 vinfos[0].jointtype = 1;
17917 vinfos[0].foffset = j0;
17918 vinfos[0].indices[0] = _ij0[0];
17919 vinfos[0].indices[1] = _ij0[1];
17920 vinfos[0].maxsolutions = _nj0;
17921 vinfos[1].jointtype = 1;
17922 vinfos[1].foffset = j1;
17923 vinfos[1].indices[0] = _ij1[0];
17924 vinfos[1].indices[1] = _ij1[1];
17925 vinfos[1].maxsolutions = _nj1;
17926 vinfos[2].jointtype = 1;
17927 vinfos[2].foffset = j2;
17928 vinfos[2].indices[0] = _ij2[0];
17929 vinfos[2].indices[1] = _ij2[1];
17930 vinfos[2].maxsolutions = _nj2;
17931 vinfos[3].jointtype = 1;
17932 vinfos[3].foffset = j3;
17933 vinfos[3].indices[0] = _ij3[0];
17934 vinfos[3].indices[1] = _ij3[1];
17935 vinfos[3].maxsolutions = _nj3;
17936 vinfos[4].jointtype = 1;
17937 vinfos[4].foffset = j4;
17938 vinfos[4].indices[0] = _ij4[0];
17939 vinfos[4].indices[1] = _ij4[1];
17940 vinfos[4].maxsolutions = _nj4;
17941 vinfos[5].jointtype = 1;
17942 vinfos[5].foffset = j5;
17943 vinfos[5].indices[0] = _ij5[0];
17944 vinfos[5].indices[1] = _ij5[1];
17945 vinfos[5].maxsolutions = _nj5;
17946 std::vector<int> vfree(0);
17947 solutions.AddSolution(vinfos,vfree);
17948 }
17949 }
17950 }
17951 
17952 }
17953 } while(0);
17954 if( bgotonextstatement )
17955 {
17956 bool bgotonextstatement = true;
17957 do
17958 {
17959 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17960 if( IKabs(evalcond[0]) < 0.0000050000000000 )
17961 {
17962 bgotonextstatement=false;
17963 {
17964 IkReal j3array[1], cj3array[1], sj3array[1];
17965 bool j3valid[1]={false};
17966 _nj3 = 1;
17967 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 )
17968  continue;
17969 j3array[0]=IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17970 sj3array[0]=IKsin(j3array[0]);
17971 cj3array[0]=IKcos(j3array[0]);
17972 if( j3array[0] > IKPI )
17973 {
17974  j3array[0]-=IK2PI;
17975 }
17976 else if( j3array[0] < -IKPI )
17977 { j3array[0]+=IK2PI;
17978 }
17979 j3valid[0] = true;
17980 for(int ij3 = 0; ij3 < 1; ++ij3)
17981 {
17982 if( !j3valid[ij3] )
17983 {
17984  continue;
17985 }
17986 _ij3[0] = ij3; _ij3[1] = -1;
17987 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17988 {
17989 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17990 {
17991  j3valid[iij3]=false; _ij3[1] = iij3; break;
17992 }
17993 }
17994 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17995 {
17996 IkReal evalcond[8];
17997 IkReal x1417=IKcos(j3);
17998 IkReal x1418=IKsin(j3);
17999 IkReal x1419=((1.0)*x1417);
18000 evalcond[0]=(x1417+new_r01);
18001 evalcond[1]=(x1418+new_r11);
18002 evalcond[2]=(x1417+new_r10);
18003 evalcond[3]=(new_r00+(((-1.0)*x1418)));
18004 evalcond[4]=(((new_r00*x1417))+((new_r10*x1418)));
18005 evalcond[5]=((((-1.0)*new_r11*x1419))+((new_r01*x1418)));
18006 evalcond[6]=((1.0)+((new_r01*x1417))+((new_r11*x1418)));
18007 evalcond[7]=((-1.0)+(((-1.0)*new_r10*x1419))+((new_r00*x1418)));
18008 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18009 {
18010 continue;
18011 }
18012 }
18013 
18014 {
18015 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18016 vinfos[0].jointtype = 1;
18017 vinfos[0].foffset = j0;
18018 vinfos[0].indices[0] = _ij0[0];
18019 vinfos[0].indices[1] = _ij0[1];
18020 vinfos[0].maxsolutions = _nj0;
18021 vinfos[1].jointtype = 1;
18022 vinfos[1].foffset = j1;
18023 vinfos[1].indices[0] = _ij1[0];
18024 vinfos[1].indices[1] = _ij1[1];
18025 vinfos[1].maxsolutions = _nj1;
18026 vinfos[2].jointtype = 1;
18027 vinfos[2].foffset = j2;
18028 vinfos[2].indices[0] = _ij2[0];
18029 vinfos[2].indices[1] = _ij2[1];
18030 vinfos[2].maxsolutions = _nj2;
18031 vinfos[3].jointtype = 1;
18032 vinfos[3].foffset = j3;
18033 vinfos[3].indices[0] = _ij3[0];
18034 vinfos[3].indices[1] = _ij3[1];
18035 vinfos[3].maxsolutions = _nj3;
18036 vinfos[4].jointtype = 1;
18037 vinfos[4].foffset = j4;
18038 vinfos[4].indices[0] = _ij4[0];
18039 vinfos[4].indices[1] = _ij4[1];
18040 vinfos[4].maxsolutions = _nj4;
18041 vinfos[5].jointtype = 1;
18042 vinfos[5].foffset = j5;
18043 vinfos[5].indices[0] = _ij5[0];
18044 vinfos[5].indices[1] = _ij5[1];
18045 vinfos[5].maxsolutions = _nj5;
18046 std::vector<int> vfree(0);
18047 solutions.AddSolution(vinfos,vfree);
18048 }
18049 }
18050 }
18051 
18052 }
18053 } while(0);
18054 if( bgotonextstatement )
18055 {
18056 bool bgotonextstatement = true;
18057 do
18058 {
18059 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
18060 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18061 {
18062 bgotonextstatement=false;
18063 {
18064 IkReal j3eval[3];
18065 sj4=0;
18066 cj4=-1.0;
18067 j4=3.14159265358979;
18068 new_r11=0;
18069 new_r00=0;
18070 j3eval[0]=new_r10;
18071 j3eval[1]=IKsign(new_r10);
18072 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18073 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18074 {
18075 {
18076 IkReal j3eval[3];
18077 sj4=0;
18078 cj4=-1.0;
18079 j4=3.14159265358979;
18080 new_r11=0;
18081 new_r00=0;
18082 j3eval[0]=new_r01;
18083 j3eval[1]=IKsign(new_r01);
18084 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18085 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18086 {
18087 {
18088 IkReal j3eval[2];
18089 sj4=0;
18090 cj4=-1.0;
18091 j4=3.14159265358979;
18092 new_r11=0;
18093 new_r00=0;
18094 j3eval[0]=new_r01;
18095 j3eval[1]=new_r10;
18096 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18097 {
18098 continue; // no branches [j3]
18099 
18100 } else
18101 {
18102 {
18103 IkReal j3array[1], cj3array[1], sj3array[1];
18104 bool j3valid[1]={false};
18105 _nj3 = 1;
18106 CheckValue<IkReal> x1420=IKPowWithIntegerCheck(new_r01,-1);
18107 if(!x1420.valid){
18108 continue;
18109 }
18110 CheckValue<IkReal> x1421=IKPowWithIntegerCheck(new_r10,-1);
18111 if(!x1421.valid){
18112 continue;
18113 }
18114 if( IKabs(((-1.0)*cj5*(x1420.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x1421.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x1420.value)))+IKsqr((sj5*(x1421.value)))-1) <= IKFAST_SINCOS_THRESH )
18115  continue;
18116 j3array[0]=IKatan2(((-1.0)*cj5*(x1420.value)), (sj5*(x1421.value)));
18117 sj3array[0]=IKsin(j3array[0]);
18118 cj3array[0]=IKcos(j3array[0]);
18119 if( j3array[0] > IKPI )
18120 {
18121  j3array[0]-=IK2PI;
18122 }
18123 else if( j3array[0] < -IKPI )
18124 { j3array[0]+=IK2PI;
18125 }
18126 j3valid[0] = true;
18127 for(int ij3 = 0; ij3 < 1; ++ij3)
18128 {
18129 if( !j3valid[ij3] )
18130 {
18131  continue;
18132 }
18133 _ij3[0] = ij3; _ij3[1] = -1;
18134 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18135 {
18136 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18137 {
18138  j3valid[iij3]=false; _ij3[1] = iij3; break;
18139 }
18140 }
18141 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18142 {
18143 IkReal evalcond[7];
18144 IkReal x1422=IKsin(j3);
18145 IkReal x1423=IKcos(j3);
18146 IkReal x1424=((1.0)*sj5);
18147 IkReal x1425=(cj5*x1422);
18148 IkReal x1426=(x1423*x1424);
18149 evalcond[0]=(cj5+((new_r01*x1422)));
18150 evalcond[1]=(cj5+((new_r10*x1422)));
18151 evalcond[2]=(sj5+(((-1.0)*new_r10*x1423)));
18152 evalcond[3]=(((new_r01*x1423))+(((-1.0)*x1424)));
18153 evalcond[4]=(((sj5*x1422))+((cj5*x1423)));
18154 evalcond[5]=(x1425+new_r01+(((-1.0)*x1426)));
18155 evalcond[6]=(x1425+new_r10+(((-1.0)*x1426)));
18156 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18157 {
18158 continue;
18159 }
18160 }
18161 
18162 {
18163 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18164 vinfos[0].jointtype = 1;
18165 vinfos[0].foffset = j0;
18166 vinfos[0].indices[0] = _ij0[0];
18167 vinfos[0].indices[1] = _ij0[1];
18168 vinfos[0].maxsolutions = _nj0;
18169 vinfos[1].jointtype = 1;
18170 vinfos[1].foffset = j1;
18171 vinfos[1].indices[0] = _ij1[0];
18172 vinfos[1].indices[1] = _ij1[1];
18173 vinfos[1].maxsolutions = _nj1;
18174 vinfos[2].jointtype = 1;
18175 vinfos[2].foffset = j2;
18176 vinfos[2].indices[0] = _ij2[0];
18177 vinfos[2].indices[1] = _ij2[1];
18178 vinfos[2].maxsolutions = _nj2;
18179 vinfos[3].jointtype = 1;
18180 vinfos[3].foffset = j3;
18181 vinfos[3].indices[0] = _ij3[0];
18182 vinfos[3].indices[1] = _ij3[1];
18183 vinfos[3].maxsolutions = _nj3;
18184 vinfos[4].jointtype = 1;
18185 vinfos[4].foffset = j4;
18186 vinfos[4].indices[0] = _ij4[0];
18187 vinfos[4].indices[1] = _ij4[1];
18188 vinfos[4].maxsolutions = _nj4;
18189 vinfos[5].jointtype = 1;
18190 vinfos[5].foffset = j5;
18191 vinfos[5].indices[0] = _ij5[0];
18192 vinfos[5].indices[1] = _ij5[1];
18193 vinfos[5].maxsolutions = _nj5;
18194 std::vector<int> vfree(0);
18195 solutions.AddSolution(vinfos,vfree);
18196 }
18197 }
18198 }
18199 
18200 }
18201 
18202 }
18203 
18204 } else
18205 {
18206 {
18207 IkReal j3array[1], cj3array[1], sj3array[1];
18208 bool j3valid[1]={false};
18209 _nj3 = 1;
18210 CheckValue<IkReal> x1427=IKPowWithIntegerCheck(IKsign(new_r01),-1);
18211 if(!x1427.valid){
18212 continue;
18213 }
18214 CheckValue<IkReal> x1428 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18215 if(!x1428.valid){
18216 continue;
18217 }
18218 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1427.value)))+(x1428.value));
18219 sj3array[0]=IKsin(j3array[0]);
18220 cj3array[0]=IKcos(j3array[0]);
18221 if( j3array[0] > IKPI )
18222 {
18223  j3array[0]-=IK2PI;
18224 }
18225 else if( j3array[0] < -IKPI )
18226 { j3array[0]+=IK2PI;
18227 }
18228 j3valid[0] = true;
18229 for(int ij3 = 0; ij3 < 1; ++ij3)
18230 {
18231 if( !j3valid[ij3] )
18232 {
18233  continue;
18234 }
18235 _ij3[0] = ij3; _ij3[1] = -1;
18236 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18237 {
18238 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18239 {
18240  j3valid[iij3]=false; _ij3[1] = iij3; break;
18241 }
18242 }
18243 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18244 {
18245 IkReal evalcond[7];
18246 IkReal x1429=IKsin(j3);
18247 IkReal x1430=IKcos(j3);
18248 IkReal x1431=((1.0)*sj5);
18249 IkReal x1432=(cj5*x1429);
18250 IkReal x1433=(x1430*x1431);
18251 evalcond[0]=(cj5+((new_r01*x1429)));
18252 evalcond[1]=(cj5+((new_r10*x1429)));
18253 evalcond[2]=(sj5+(((-1.0)*new_r10*x1430)));
18254 evalcond[3]=(((new_r01*x1430))+(((-1.0)*x1431)));
18255 evalcond[4]=(((cj5*x1430))+((sj5*x1429)));
18256 evalcond[5]=((((-1.0)*x1433))+x1432+new_r01);
18257 evalcond[6]=((((-1.0)*x1433))+x1432+new_r10);
18258 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18259 {
18260 continue;
18261 }
18262 }
18263 
18264 {
18265 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18266 vinfos[0].jointtype = 1;
18267 vinfos[0].foffset = j0;
18268 vinfos[0].indices[0] = _ij0[0];
18269 vinfos[0].indices[1] = _ij0[1];
18270 vinfos[0].maxsolutions = _nj0;
18271 vinfos[1].jointtype = 1;
18272 vinfos[1].foffset = j1;
18273 vinfos[1].indices[0] = _ij1[0];
18274 vinfos[1].indices[1] = _ij1[1];
18275 vinfos[1].maxsolutions = _nj1;
18276 vinfos[2].jointtype = 1;
18277 vinfos[2].foffset = j2;
18278 vinfos[2].indices[0] = _ij2[0];
18279 vinfos[2].indices[1] = _ij2[1];
18280 vinfos[2].maxsolutions = _nj2;
18281 vinfos[3].jointtype = 1;
18282 vinfos[3].foffset = j3;
18283 vinfos[3].indices[0] = _ij3[0];
18284 vinfos[3].indices[1] = _ij3[1];
18285 vinfos[3].maxsolutions = _nj3;
18286 vinfos[4].jointtype = 1;
18287 vinfos[4].foffset = j4;
18288 vinfos[4].indices[0] = _ij4[0];
18289 vinfos[4].indices[1] = _ij4[1];
18290 vinfos[4].maxsolutions = _nj4;
18291 vinfos[5].jointtype = 1;
18292 vinfos[5].foffset = j5;
18293 vinfos[5].indices[0] = _ij5[0];
18294 vinfos[5].indices[1] = _ij5[1];
18295 vinfos[5].maxsolutions = _nj5;
18296 std::vector<int> vfree(0);
18297 solutions.AddSolution(vinfos,vfree);
18298 }
18299 }
18300 }
18301 
18302 }
18303 
18304 }
18305 
18306 } else
18307 {
18308 {
18309 IkReal j3array[1], cj3array[1], sj3array[1];
18310 bool j3valid[1]={false};
18311 _nj3 = 1;
18312 CheckValue<IkReal> x1434=IKPowWithIntegerCheck(IKsign(new_r10),-1);
18313 if(!x1434.valid){
18314 continue;
18315 }
18316 CheckValue<IkReal> x1435 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18317 if(!x1435.valid){
18318 continue;
18319 }
18320 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1434.value)))+(x1435.value));
18321 sj3array[0]=IKsin(j3array[0]);
18322 cj3array[0]=IKcos(j3array[0]);
18323 if( j3array[0] > IKPI )
18324 {
18325  j3array[0]-=IK2PI;
18326 }
18327 else if( j3array[0] < -IKPI )
18328 { j3array[0]+=IK2PI;
18329 }
18330 j3valid[0] = true;
18331 for(int ij3 = 0; ij3 < 1; ++ij3)
18332 {
18333 if( !j3valid[ij3] )
18334 {
18335  continue;
18336 }
18337 _ij3[0] = ij3; _ij3[1] = -1;
18338 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18339 {
18340 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18341 {
18342  j3valid[iij3]=false; _ij3[1] = iij3; break;
18343 }
18344 }
18345 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18346 {
18347 IkReal evalcond[7];
18348 IkReal x1436=IKsin(j3);
18349 IkReal x1437=IKcos(j3);
18350 IkReal x1438=((1.0)*sj5);
18351 IkReal x1439=(cj5*x1436);
18352 IkReal x1440=(x1437*x1438);
18353 evalcond[0]=(cj5+((new_r01*x1436)));
18354 evalcond[1]=(cj5+((new_r10*x1436)));
18355 evalcond[2]=(sj5+(((-1.0)*new_r10*x1437)));
18356 evalcond[3]=(((new_r01*x1437))+(((-1.0)*x1438)));
18357 evalcond[4]=(((sj5*x1436))+((cj5*x1437)));
18358 evalcond[5]=(x1439+(((-1.0)*x1440))+new_r01);
18359 evalcond[6]=(x1439+(((-1.0)*x1440))+new_r10);
18360 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18361 {
18362 continue;
18363 }
18364 }
18365 
18366 {
18367 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18368 vinfos[0].jointtype = 1;
18369 vinfos[0].foffset = j0;
18370 vinfos[0].indices[0] = _ij0[0];
18371 vinfos[0].indices[1] = _ij0[1];
18372 vinfos[0].maxsolutions = _nj0;
18373 vinfos[1].jointtype = 1;
18374 vinfos[1].foffset = j1;
18375 vinfos[1].indices[0] = _ij1[0];
18376 vinfos[1].indices[1] = _ij1[1];
18377 vinfos[1].maxsolutions = _nj1;
18378 vinfos[2].jointtype = 1;
18379 vinfos[2].foffset = j2;
18380 vinfos[2].indices[0] = _ij2[0];
18381 vinfos[2].indices[1] = _ij2[1];
18382 vinfos[2].maxsolutions = _nj2;
18383 vinfos[3].jointtype = 1;
18384 vinfos[3].foffset = j3;
18385 vinfos[3].indices[0] = _ij3[0];
18386 vinfos[3].indices[1] = _ij3[1];
18387 vinfos[3].maxsolutions = _nj3;
18388 vinfos[4].jointtype = 1;
18389 vinfos[4].foffset = j4;
18390 vinfos[4].indices[0] = _ij4[0];
18391 vinfos[4].indices[1] = _ij4[1];
18392 vinfos[4].maxsolutions = _nj4;
18393 vinfos[5].jointtype = 1;
18394 vinfos[5].foffset = j5;
18395 vinfos[5].indices[0] = _ij5[0];
18396 vinfos[5].indices[1] = _ij5[1];
18397 vinfos[5].maxsolutions = _nj5;
18398 std::vector<int> vfree(0);
18399 solutions.AddSolution(vinfos,vfree);
18400 }
18401 }
18402 }
18403 
18404 }
18405 
18406 }
18407 
18408 }
18409 } while(0);
18410 if( bgotonextstatement )
18411 {
18412 bool bgotonextstatement = true;
18413 do
18414 {
18415 evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18416 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18417 {
18418 bgotonextstatement=false;
18419 {
18420 IkReal j3eval[1];
18421 sj4=0;
18422 cj4=-1.0;
18423 j4=3.14159265358979;
18424 new_r11=0;
18425 new_r01=0;
18426 new_r22=0;
18427 new_r20=0;
18428 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18429 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18430 {
18431 continue; // no branches [j3]
18432 
18433 } else
18434 {
18435 {
18436 IkReal j3array[2], cj3array[2], sj3array[2];
18437 bool j3valid[2]={false};
18438 _nj3 = 2;
18439 CheckValue<IkReal> x1442 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
18440 if(!x1442.valid){
18441 continue;
18442 }
18443 IkReal x1441=x1442.value;
18444 j3array[0]=((-1.0)*x1441);
18445 sj3array[0]=IKsin(j3array[0]);
18446 cj3array[0]=IKcos(j3array[0]);
18447 j3array[1]=((3.14159265358979)+(((-1.0)*x1441)));
18448 sj3array[1]=IKsin(j3array[1]);
18449 cj3array[1]=IKcos(j3array[1]);
18450 if( j3array[0] > IKPI )
18451 {
18452  j3array[0]-=IK2PI;
18453 }
18454 else if( j3array[0] < -IKPI )
18455 { j3array[0]+=IK2PI;
18456 }
18457 j3valid[0] = true;
18458 if( j3array[1] > IKPI )
18459 {
18460  j3array[1]-=IK2PI;
18461 }
18462 else if( j3array[1] < -IKPI )
18463 { j3array[1]+=IK2PI;
18464 }
18465 j3valid[1] = true;
18466 for(int ij3 = 0; ij3 < 2; ++ij3)
18467 {
18468 if( !j3valid[ij3] )
18469 {
18470  continue;
18471 }
18472 _ij3[0] = ij3; _ij3[1] = -1;
18473 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18474 {
18475 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18476 {
18477  j3valid[iij3]=false; _ij3[1] = iij3; break;
18478 }
18479 }
18480 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18481 {
18482 IkReal evalcond[1];
18483 evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
18484 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18485 {
18486 continue;
18487 }
18488 }
18489 
18490 {
18491 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18492 vinfos[0].jointtype = 1;
18493 vinfos[0].foffset = j0;
18494 vinfos[0].indices[0] = _ij0[0];
18495 vinfos[0].indices[1] = _ij0[1];
18496 vinfos[0].maxsolutions = _nj0;
18497 vinfos[1].jointtype = 1;
18498 vinfos[1].foffset = j1;
18499 vinfos[1].indices[0] = _ij1[0];
18500 vinfos[1].indices[1] = _ij1[1];
18501 vinfos[1].maxsolutions = _nj1;
18502 vinfos[2].jointtype = 1;
18503 vinfos[2].foffset = j2;
18504 vinfos[2].indices[0] = _ij2[0];
18505 vinfos[2].indices[1] = _ij2[1];
18506 vinfos[2].maxsolutions = _nj2;
18507 vinfos[3].jointtype = 1;
18508 vinfos[3].foffset = j3;
18509 vinfos[3].indices[0] = _ij3[0];
18510 vinfos[3].indices[1] = _ij3[1];
18511 vinfos[3].maxsolutions = _nj3;
18512 vinfos[4].jointtype = 1;
18513 vinfos[4].foffset = j4;
18514 vinfos[4].indices[0] = _ij4[0];
18515 vinfos[4].indices[1] = _ij4[1];
18516 vinfos[4].maxsolutions = _nj4;
18517 vinfos[5].jointtype = 1;
18518 vinfos[5].foffset = j5;
18519 vinfos[5].indices[0] = _ij5[0];
18520 vinfos[5].indices[1] = _ij5[1];
18521 vinfos[5].maxsolutions = _nj5;
18522 std::vector<int> vfree(0);
18523 solutions.AddSolution(vinfos,vfree);
18524 }
18525 }
18526 }
18527 
18528 }
18529 
18530 }
18531 
18532 }
18533 } while(0);
18534 if( bgotonextstatement )
18535 {
18536 bool bgotonextstatement = true;
18537 do
18538 {
18539 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18540 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18541 {
18542 bgotonextstatement=false;
18543 {
18544 IkReal j3eval[1];
18545 sj4=0;
18546 cj4=-1.0;
18547 j4=3.14159265358979;
18548 new_r00=0;
18549 new_r10=0;
18550 new_r21=0;
18551 new_r22=0;
18552 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18553 if( IKabs(j3eval[0]) < 0.0000010000000000 )
18554 {
18555 continue; // no branches [j3]
18556 
18557 } else
18558 {
18559 {
18560 IkReal j3array[2], cj3array[2], sj3array[2];
18561 bool j3valid[2]={false};
18562 _nj3 = 2;
18563 CheckValue<IkReal> x1444 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
18564 if(!x1444.valid){
18565 continue;
18566 }
18567 IkReal x1443=x1444.value;
18568 j3array[0]=((-1.0)*x1443);
18569 sj3array[0]=IKsin(j3array[0]);
18570 cj3array[0]=IKcos(j3array[0]);
18571 j3array[1]=((3.14159265358979)+(((-1.0)*x1443)));
18572 sj3array[1]=IKsin(j3array[1]);
18573 cj3array[1]=IKcos(j3array[1]);
18574 if( j3array[0] > IKPI )
18575 {
18576  j3array[0]-=IK2PI;
18577 }
18578 else if( j3array[0] < -IKPI )
18579 { j3array[0]+=IK2PI;
18580 }
18581 j3valid[0] = true;
18582 if( j3array[1] > IKPI )
18583 {
18584  j3array[1]-=IK2PI;
18585 }
18586 else if( j3array[1] < -IKPI )
18587 { j3array[1]+=IK2PI;
18588 }
18589 j3valid[1] = true;
18590 for(int ij3 = 0; ij3 < 2; ++ij3)
18591 {
18592 if( !j3valid[ij3] )
18593 {
18594  continue;
18595 }
18596 _ij3[0] = ij3; _ij3[1] = -1;
18597 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18598 {
18599 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18600 {
18601  j3valid[iij3]=false; _ij3[1] = iij3; break;
18602 }
18603 }
18604 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18605 {
18606 IkReal evalcond[1];
18607 evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
18608 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18609 {
18610 continue;
18611 }
18612 }
18613 
18614 {
18615 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18616 vinfos[0].jointtype = 1;
18617 vinfos[0].foffset = j0;
18618 vinfos[0].indices[0] = _ij0[0];
18619 vinfos[0].indices[1] = _ij0[1];
18620 vinfos[0].maxsolutions = _nj0;
18621 vinfos[1].jointtype = 1;
18622 vinfos[1].foffset = j1;
18623 vinfos[1].indices[0] = _ij1[0];
18624 vinfos[1].indices[1] = _ij1[1];
18625 vinfos[1].maxsolutions = _nj1;
18626 vinfos[2].jointtype = 1;
18627 vinfos[2].foffset = j2;
18628 vinfos[2].indices[0] = _ij2[0];
18629 vinfos[2].indices[1] = _ij2[1];
18630 vinfos[2].maxsolutions = _nj2;
18631 vinfos[3].jointtype = 1;
18632 vinfos[3].foffset = j3;
18633 vinfos[3].indices[0] = _ij3[0];
18634 vinfos[3].indices[1] = _ij3[1];
18635 vinfos[3].maxsolutions = _nj3;
18636 vinfos[4].jointtype = 1;
18637 vinfos[4].foffset = j4;
18638 vinfos[4].indices[0] = _ij4[0];
18639 vinfos[4].indices[1] = _ij4[1];
18640 vinfos[4].maxsolutions = _nj4;
18641 vinfos[5].jointtype = 1;
18642 vinfos[5].foffset = j5;
18643 vinfos[5].indices[0] = _ij5[0];
18644 vinfos[5].indices[1] = _ij5[1];
18645 vinfos[5].maxsolutions = _nj5;
18646 std::vector<int> vfree(0);
18647 solutions.AddSolution(vinfos,vfree);
18648 }
18649 }
18650 }
18651 
18652 }
18653 
18654 }
18655 
18656 }
18657 } while(0);
18658 if( bgotonextstatement )
18659 {
18660 bool bgotonextstatement = true;
18661 do
18662 {
18663 evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
18664 if( IKabs(evalcond[0]) < 0.0000050000000000 )
18665 {
18666 bgotonextstatement=false;
18667 {
18668 IkReal j3eval[3];
18669 sj4=0;
18670 cj4=-1.0;
18671 j4=3.14159265358979;
18672 new_r01=0;
18673 new_r10=0;
18674 j3eval[0]=new_r00;
18675 j3eval[1]=IKsign(new_r00);
18676 j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18677 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18678 {
18679 {
18680 IkReal j3eval[2];
18681 sj4=0;
18682 cj4=-1.0;
18683 j4=3.14159265358979;
18684 new_r01=0;
18685 new_r10=0;
18686 j3eval[0]=new_r00;
18687 j3eval[1]=new_r11;
18688 if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18689 {
18690 continue; // no branches [j3]
18691 
18692 } else
18693 {
18694 {
18695 IkReal j3array[1], cj3array[1], sj3array[1];
18696 bool j3valid[1]={false};
18697 _nj3 = 1;
18698 CheckValue<IkReal> x1445=IKPowWithIntegerCheck(new_r00,-1);
18699 if(!x1445.valid){
18700 continue;
18701 }
18702 CheckValue<IkReal> x1446=IKPowWithIntegerCheck(new_r11,-1);
18703 if(!x1446.valid){
18704 continue;
18705 }
18706 if( IKabs(((-1.0)*sj5*(x1445.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x1446.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x1445.value)))+IKsqr((cj5*(x1446.value)))-1) <= IKFAST_SINCOS_THRESH )
18707  continue;
18708 j3array[0]=IKatan2(((-1.0)*sj5*(x1445.value)), (cj5*(x1446.value)));
18709 sj3array[0]=IKsin(j3array[0]);
18710 cj3array[0]=IKcos(j3array[0]);
18711 if( j3array[0] > IKPI )
18712 {
18713  j3array[0]-=IK2PI;
18714 }
18715 else if( j3array[0] < -IKPI )
18716 { j3array[0]+=IK2PI;
18717 }
18718 j3valid[0] = true;
18719 for(int ij3 = 0; ij3 < 1; ++ij3)
18720 {
18721 if( !j3valid[ij3] )
18722 {
18723  continue;
18724 }
18725 _ij3[0] = ij3; _ij3[1] = -1;
18726 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18727 {
18728 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18729 {
18730  j3valid[iij3]=false; _ij3[1] = iij3; break;
18731 }
18732 }
18733 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18734 {
18735 IkReal evalcond[7];
18736 IkReal x1447=IKcos(j3);
18737 IkReal x1448=IKsin(j3);
18738 IkReal x1449=((1.0)*sj5);
18739 IkReal x1450=(cj5*x1447);
18740 IkReal x1451=((1.0)*x1447);
18741 evalcond[0]=(sj5+((new_r00*x1448)));
18742 evalcond[1]=(cj5+((new_r00*x1447)));
18743 evalcond[2]=(cj5+(((-1.0)*new_r11*x1451)));
18744 evalcond[3]=(((new_r11*x1448))+(((-1.0)*x1449)));
18745 evalcond[4]=((((-1.0)*x1447*x1449))+((cj5*x1448)));
18746 evalcond[5]=(((sj5*x1448))+x1450+new_r00);
18747 evalcond[6]=((((-1.0)*x1450))+new_r11+(((-1.0)*x1448*x1449)));
18748 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18749 {
18750 continue;
18751 }
18752 }
18753 
18754 {
18755 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18756 vinfos[0].jointtype = 1;
18757 vinfos[0].foffset = j0;
18758 vinfos[0].indices[0] = _ij0[0];
18759 vinfos[0].indices[1] = _ij0[1];
18760 vinfos[0].maxsolutions = _nj0;
18761 vinfos[1].jointtype = 1;
18762 vinfos[1].foffset = j1;
18763 vinfos[1].indices[0] = _ij1[0];
18764 vinfos[1].indices[1] = _ij1[1];
18765 vinfos[1].maxsolutions = _nj1;
18766 vinfos[2].jointtype = 1;
18767 vinfos[2].foffset = j2;
18768 vinfos[2].indices[0] = _ij2[0];
18769 vinfos[2].indices[1] = _ij2[1];
18770 vinfos[2].maxsolutions = _nj2;
18771 vinfos[3].jointtype = 1;
18772 vinfos[3].foffset = j3;
18773 vinfos[3].indices[0] = _ij3[0];
18774 vinfos[3].indices[1] = _ij3[1];
18775 vinfos[3].maxsolutions = _nj3;
18776 vinfos[4].jointtype = 1;
18777 vinfos[4].foffset = j4;
18778 vinfos[4].indices[0] = _ij4[0];
18779 vinfos[4].indices[1] = _ij4[1];
18780 vinfos[4].maxsolutions = _nj4;
18781 vinfos[5].jointtype = 1;
18782 vinfos[5].foffset = j5;
18783 vinfos[5].indices[0] = _ij5[0];
18784 vinfos[5].indices[1] = _ij5[1];
18785 vinfos[5].maxsolutions = _nj5;
18786 std::vector<int> vfree(0);
18787 solutions.AddSolution(vinfos,vfree);
18788 }
18789 }
18790 }
18791 
18792 }
18793 
18794 }
18795 
18796 } else
18797 {
18798 {
18799 IkReal j3array[1], cj3array[1], sj3array[1];
18800 bool j3valid[1]={false};
18801 _nj3 = 1;
18802 CheckValue<IkReal> x1452 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(((-1.0)*cj5)),IKFAST_ATAN2_MAGTHRESH);
18803 if(!x1452.valid){
18804 continue;
18805 }
18806 CheckValue<IkReal> x1453=IKPowWithIntegerCheck(IKsign(new_r00),-1);
18807 if(!x1453.valid){
18808 continue;
18809 }
18810 j3array[0]=((-1.5707963267949)+(x1452.value)+(((1.5707963267949)*(x1453.value))));
18811 sj3array[0]=IKsin(j3array[0]);
18812 cj3array[0]=IKcos(j3array[0]);
18813 if( j3array[0] > IKPI )
18814 {
18815  j3array[0]-=IK2PI;
18816 }
18817 else if( j3array[0] < -IKPI )
18818 { j3array[0]+=IK2PI;
18819 }
18820 j3valid[0] = true;
18821 for(int ij3 = 0; ij3 < 1; ++ij3)
18822 {
18823 if( !j3valid[ij3] )
18824 {
18825  continue;
18826 }
18827 _ij3[0] = ij3; _ij3[1] = -1;
18828 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18829 {
18830 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18831 {
18832  j3valid[iij3]=false; _ij3[1] = iij3; break;
18833 }
18834 }
18835 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18836 {
18837 IkReal evalcond[7];
18838 IkReal x1454=IKcos(j3);
18839 IkReal x1455=IKsin(j3);
18840 IkReal x1456=((1.0)*sj5);
18841 IkReal x1457=(cj5*x1454);
18842 IkReal x1458=((1.0)*x1454);
18843 evalcond[0]=(sj5+((new_r00*x1455)));
18844 evalcond[1]=(cj5+((new_r00*x1454)));
18845 evalcond[2]=(cj5+(((-1.0)*new_r11*x1458)));
18846 evalcond[3]=((((-1.0)*x1456))+((new_r11*x1455)));
18847 evalcond[4]=(((cj5*x1455))+(((-1.0)*x1454*x1456)));
18848 evalcond[5]=(((sj5*x1455))+x1457+new_r00);
18849 evalcond[6]=((((-1.0)*x1457))+(((-1.0)*x1455*x1456))+new_r11);
18850 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18851 {
18852 continue;
18853 }
18854 }
18855 
18856 {
18857 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18858 vinfos[0].jointtype = 1;
18859 vinfos[0].foffset = j0;
18860 vinfos[0].indices[0] = _ij0[0];
18861 vinfos[0].indices[1] = _ij0[1];
18862 vinfos[0].maxsolutions = _nj0;
18863 vinfos[1].jointtype = 1;
18864 vinfos[1].foffset = j1;
18865 vinfos[1].indices[0] = _ij1[0];
18866 vinfos[1].indices[1] = _ij1[1];
18867 vinfos[1].maxsolutions = _nj1;
18868 vinfos[2].jointtype = 1;
18869 vinfos[2].foffset = j2;
18870 vinfos[2].indices[0] = _ij2[0];
18871 vinfos[2].indices[1] = _ij2[1];
18872 vinfos[2].maxsolutions = _nj2;
18873 vinfos[3].jointtype = 1;
18874 vinfos[3].foffset = j3;
18875 vinfos[3].indices[0] = _ij3[0];
18876 vinfos[3].indices[1] = _ij3[1];
18877 vinfos[3].maxsolutions = _nj3;
18878 vinfos[4].jointtype = 1;
18879 vinfos[4].foffset = j4;
18880 vinfos[4].indices[0] = _ij4[0];
18881 vinfos[4].indices[1] = _ij4[1];
18882 vinfos[4].maxsolutions = _nj4;
18883 vinfos[5].jointtype = 1;
18884 vinfos[5].foffset = j5;
18885 vinfos[5].indices[0] = _ij5[0];
18886 vinfos[5].indices[1] = _ij5[1];
18887 vinfos[5].maxsolutions = _nj5;
18888 std::vector<int> vfree(0);
18889 solutions.AddSolution(vinfos,vfree);
18890 }
18891 }
18892 }
18893 
18894 }
18895 
18896 }
18897 
18898 }
18899 } while(0);
18900 if( bgotonextstatement )
18901 {
18902 bool bgotonextstatement = true;
18903 do
18904 {
18905 if( 1 )
18906 {
18907 bgotonextstatement=false;
18908 continue; // branch miss [j3]
18909 
18910 }
18911 } while(0);
18912 if( bgotonextstatement )
18913 {
18914 }
18915 }
18916 }
18917 }
18918 }
18919 }
18920 }
18921 }
18922 }
18923 }
18924 }
18925 }
18926 
18927 } else
18928 {
18929 {
18930 IkReal j3array[1], cj3array[1], sj3array[1];
18931 bool j3valid[1]={false};
18932 _nj3 = 1;
18933 IkReal x1459=((1.0)*new_r00);
18934 CheckValue<IkReal> x1460 = IKatan2WithCheck(IkReal(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))),IkReal(((((-1.0)*new_r10*x1459))+((cj5*sj5)))),IKFAST_ATAN2_MAGTHRESH);
18935 if(!x1460.valid){
18936 continue;
18937 }
18938 CheckValue<IkReal> x1461=IKPowWithIntegerCheck(IKsign((((cj5*new_r10))+(((-1.0)*sj5*x1459)))),-1);
18939 if(!x1461.valid){
18940 continue;
18941 }
18942 j3array[0]=((-1.5707963267949)+(x1460.value)+(((1.5707963267949)*(x1461.value))));
18943 sj3array[0]=IKsin(j3array[0]);
18944 cj3array[0]=IKcos(j3array[0]);
18945 if( j3array[0] > IKPI )
18946 {
18947  j3array[0]-=IK2PI;
18948 }
18949 else if( j3array[0] < -IKPI )
18950 { j3array[0]+=IK2PI;
18951 }
18952 j3valid[0] = true;
18953 for(int ij3 = 0; ij3 < 1; ++ij3)
18954 {
18955 if( !j3valid[ij3] )
18956 {
18957  continue;
18958 }
18959 _ij3[0] = ij3; _ij3[1] = -1;
18960 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18961 {
18962 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18963 {
18964  j3valid[iij3]=false; _ij3[1] = iij3; break;
18965 }
18966 }
18967 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18968 {
18969 IkReal evalcond[8];
18970 IkReal x1462=IKsin(j3);
18971 IkReal x1463=IKcos(j3);
18972 IkReal x1464=((1.0)*sj5);
18973 IkReal x1465=(cj5*x1462);
18974 IkReal x1466=((1.0)*x1463);
18975 IkReal x1467=(x1463*x1464);
18976 evalcond[0]=(((new_r00*x1463))+cj5+((new_r10*x1462)));
18977 evalcond[1]=(((cj5*x1463))+new_r00+((sj5*x1462)));
18978 evalcond[2]=((((-1.0)*new_r10*x1466))+((new_r00*x1462))+sj5);
18979 evalcond[3]=(((new_r01*x1462))+cj5+(((-1.0)*new_r11*x1466)));
18980 evalcond[4]=((((-1.0)*x1467))+x1465+new_r01);
18981 evalcond[5]=((((-1.0)*x1467))+x1465+new_r10);
18982 evalcond[6]=(((new_r01*x1463))+(((-1.0)*x1464))+((new_r11*x1462)));
18983 evalcond[7]=((((-1.0)*cj5*x1466))+(((-1.0)*x1462*x1464))+new_r11);
18984 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18985 {
18986 continue;
18987 }
18988 }
18989 
18990 {
18991 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18992 vinfos[0].jointtype = 1;
18993 vinfos[0].foffset = j0;
18994 vinfos[0].indices[0] = _ij0[0];
18995 vinfos[0].indices[1] = _ij0[1];
18996 vinfos[0].maxsolutions = _nj0;
18997 vinfos[1].jointtype = 1;
18998 vinfos[1].foffset = j1;
18999 vinfos[1].indices[0] = _ij1[0];
19000 vinfos[1].indices[1] = _ij1[1];
19001 vinfos[1].maxsolutions = _nj1;
19002 vinfos[2].jointtype = 1;
19003 vinfos[2].foffset = j2;
19004 vinfos[2].indices[0] = _ij2[0];
19005 vinfos[2].indices[1] = _ij2[1];
19006 vinfos[2].maxsolutions = _nj2;
19007 vinfos[3].jointtype = 1;
19008 vinfos[3].foffset = j3;
19009 vinfos[3].indices[0] = _ij3[0];
19010 vinfos[3].indices[1] = _ij3[1];
19011 vinfos[3].maxsolutions = _nj3;
19012 vinfos[4].jointtype = 1;
19013 vinfos[4].foffset = j4;
19014 vinfos[4].indices[0] = _ij4[0];
19015 vinfos[4].indices[1] = _ij4[1];
19016 vinfos[4].maxsolutions = _nj4;
19017 vinfos[5].jointtype = 1;
19018 vinfos[5].foffset = j5;
19019 vinfos[5].indices[0] = _ij5[0];
19020 vinfos[5].indices[1] = _ij5[1];
19021 vinfos[5].maxsolutions = _nj5;
19022 std::vector<int> vfree(0);
19023 solutions.AddSolution(vinfos,vfree);
19024 }
19025 }
19026 }
19027 
19028 }
19029 
19030 }
19031 
19032 } else
19033 {
19034 {
19035 IkReal j3array[1], cj3array[1], sj3array[1];
19036 bool j3valid[1]={false};
19037 _nj3 = 1;
19038 IkReal x1468=((1.0)*new_r10);
19039 CheckValue<IkReal> x1469 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((((-1.0)*new_r01*x1468))+(cj5*cj5))),IKFAST_ATAN2_MAGTHRESH);
19040 if(!x1469.valid){
19041 continue;
19042 }
19043 CheckValue<IkReal> x1470=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x1468)))),-1);
19044 if(!x1470.valid){
19045 continue;
19046 }
19047 j3array[0]=((-1.5707963267949)+(x1469.value)+(((1.5707963267949)*(x1470.value))));
19048 sj3array[0]=IKsin(j3array[0]);
19049 cj3array[0]=IKcos(j3array[0]);
19050 if( j3array[0] > IKPI )
19051 {
19052  j3array[0]-=IK2PI;
19053 }
19054 else if( j3array[0] < -IKPI )
19055 { j3array[0]+=IK2PI;
19056 }
19057 j3valid[0] = true;
19058 for(int ij3 = 0; ij3 < 1; ++ij3)
19059 {
19060 if( !j3valid[ij3] )
19061 {
19062  continue;
19063 }
19064 _ij3[0] = ij3; _ij3[1] = -1;
19065 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19066 {
19067 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19068 {
19069  j3valid[iij3]=false; _ij3[1] = iij3; break;
19070 }
19071 }
19072 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19073 {
19074 IkReal evalcond[8];
19075 IkReal x1471=IKsin(j3);
19076 IkReal x1472=IKcos(j3);
19077 IkReal x1473=((1.0)*sj5);
19078 IkReal x1474=(cj5*x1471);
19079 IkReal x1475=((1.0)*x1472);
19080 IkReal x1476=(x1472*x1473);
19081 evalcond[0]=(((new_r10*x1471))+cj5+((new_r00*x1472)));
19082 evalcond[1]=(((cj5*x1472))+((sj5*x1471))+new_r00);
19083 evalcond[2]=(sj5+((new_r00*x1471))+(((-1.0)*new_r10*x1475)));
19084 evalcond[3]=(cj5+(((-1.0)*new_r11*x1475))+((new_r01*x1471)));
19085 evalcond[4]=(x1474+(((-1.0)*x1476))+new_r01);
19086 evalcond[5]=(x1474+(((-1.0)*x1476))+new_r10);
19087 evalcond[6]=(((new_r11*x1471))+((new_r01*x1472))+(((-1.0)*x1473)));
19088 evalcond[7]=((((-1.0)*x1471*x1473))+(((-1.0)*cj5*x1475))+new_r11);
19089 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19090 {
19091 continue;
19092 }
19093 }
19094 
19095 {
19096 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19097 vinfos[0].jointtype = 1;
19098 vinfos[0].foffset = j0;
19099 vinfos[0].indices[0] = _ij0[0];
19100 vinfos[0].indices[1] = _ij0[1];
19101 vinfos[0].maxsolutions = _nj0;
19102 vinfos[1].jointtype = 1;
19103 vinfos[1].foffset = j1;
19104 vinfos[1].indices[0] = _ij1[0];
19105 vinfos[1].indices[1] = _ij1[1];
19106 vinfos[1].maxsolutions = _nj1;
19107 vinfos[2].jointtype = 1;
19108 vinfos[2].foffset = j2;
19109 vinfos[2].indices[0] = _ij2[0];
19110 vinfos[2].indices[1] = _ij2[1];
19111 vinfos[2].maxsolutions = _nj2;
19112 vinfos[3].jointtype = 1;
19113 vinfos[3].foffset = j3;
19114 vinfos[3].indices[0] = _ij3[0];
19115 vinfos[3].indices[1] = _ij3[1];
19116 vinfos[3].maxsolutions = _nj3;
19117 vinfos[4].jointtype = 1;
19118 vinfos[4].foffset = j4;
19119 vinfos[4].indices[0] = _ij4[0];
19120 vinfos[4].indices[1] = _ij4[1];
19121 vinfos[4].maxsolutions = _nj4;
19122 vinfos[5].jointtype = 1;
19123 vinfos[5].foffset = j5;
19124 vinfos[5].indices[0] = _ij5[0];
19125 vinfos[5].indices[1] = _ij5[1];
19126 vinfos[5].maxsolutions = _nj5;
19127 std::vector<int> vfree(0);
19128 solutions.AddSolution(vinfos,vfree);
19129 }
19130 }
19131 }
19132 
19133 }
19134 
19135 }
19136 
19137 } else
19138 {
19139 {
19140 IkReal j3array[1], cj3array[1], sj3array[1];
19141 bool j3valid[1]={false};
19142 _nj3 = 1;
19143 IkReal x1477=((1.0)*new_r10);
19144 CheckValue<IkReal> x1478 = IKatan2WithCheck(IkReal((((cj5*new_r11))+((cj5*new_r00)))),IkReal((((cj5*new_r01))+(((-1.0)*cj5*x1477)))),IKFAST_ATAN2_MAGTHRESH);
19145 if(!x1478.valid){
19146 continue;
19147 }
19148 CheckValue<IkReal> x1479=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x1477))+(((-1.0)*new_r00*new_r01)))),-1);
19149 if(!x1479.valid){
19150 continue;
19151 }
19152 j3array[0]=((-1.5707963267949)+(x1478.value)+(((1.5707963267949)*(x1479.value))));
19153 sj3array[0]=IKsin(j3array[0]);
19154 cj3array[0]=IKcos(j3array[0]);
19155 if( j3array[0] > IKPI )
19156 {
19157  j3array[0]-=IK2PI;
19158 }
19159 else if( j3array[0] < -IKPI )
19160 { j3array[0]+=IK2PI;
19161 }
19162 j3valid[0] = true;
19163 for(int ij3 = 0; ij3 < 1; ++ij3)
19164 {
19165 if( !j3valid[ij3] )
19166 {
19167  continue;
19168 }
19169 _ij3[0] = ij3; _ij3[1] = -1;
19170 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19171 {
19172 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19173 {
19174  j3valid[iij3]=false; _ij3[1] = iij3; break;
19175 }
19176 }
19177 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19178 {
19179 IkReal evalcond[8];
19180 IkReal x1480=IKsin(j3);
19181 IkReal x1481=IKcos(j3);
19182 IkReal x1482=((1.0)*sj5);
19183 IkReal x1483=(cj5*x1480);
19184 IkReal x1484=((1.0)*x1481);
19185 IkReal x1485=(x1481*x1482);
19186 evalcond[0]=(cj5+((new_r00*x1481))+((new_r10*x1480)));
19187 evalcond[1]=(((cj5*x1481))+new_r00+((sj5*x1480)));
19188 evalcond[2]=(sj5+(((-1.0)*new_r10*x1484))+((new_r00*x1480)));
19189 evalcond[3]=(cj5+(((-1.0)*new_r11*x1484))+((new_r01*x1480)));
19190 evalcond[4]=((((-1.0)*x1485))+x1483+new_r01);
19191 evalcond[5]=((((-1.0)*x1485))+x1483+new_r10);
19192 evalcond[6]=((((-1.0)*x1482))+((new_r11*x1480))+((new_r01*x1481)));
19193 evalcond[7]=((((-1.0)*x1480*x1482))+new_r11+(((-1.0)*cj5*x1484)));
19194 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19195 {
19196 continue;
19197 }
19198 }
19199 
19200 {
19201 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19202 vinfos[0].jointtype = 1;
19203 vinfos[0].foffset = j0;
19204 vinfos[0].indices[0] = _ij0[0];
19205 vinfos[0].indices[1] = _ij0[1];
19206 vinfos[0].maxsolutions = _nj0;
19207 vinfos[1].jointtype = 1;
19208 vinfos[1].foffset = j1;
19209 vinfos[1].indices[0] = _ij1[0];
19210 vinfos[1].indices[1] = _ij1[1];
19211 vinfos[1].maxsolutions = _nj1;
19212 vinfos[2].jointtype = 1;
19213 vinfos[2].foffset = j2;
19214 vinfos[2].indices[0] = _ij2[0];
19215 vinfos[2].indices[1] = _ij2[1];
19216 vinfos[2].maxsolutions = _nj2;
19217 vinfos[3].jointtype = 1;
19218 vinfos[3].foffset = j3;
19219 vinfos[3].indices[0] = _ij3[0];
19220 vinfos[3].indices[1] = _ij3[1];
19221 vinfos[3].maxsolutions = _nj3;
19222 vinfos[4].jointtype = 1;
19223 vinfos[4].foffset = j4;
19224 vinfos[4].indices[0] = _ij4[0];
19225 vinfos[4].indices[1] = _ij4[1];
19226 vinfos[4].maxsolutions = _nj4;
19227 vinfos[5].jointtype = 1;
19228 vinfos[5].foffset = j5;
19229 vinfos[5].indices[0] = _ij5[0];
19230 vinfos[5].indices[1] = _ij5[1];
19231 vinfos[5].maxsolutions = _nj5;
19232 std::vector<int> vfree(0);
19233 solutions.AddSolution(vinfos,vfree);
19234 }
19235 }
19236 }
19237 
19238 }
19239 
19240 }
19241 
19242 }
19243 } while(0);
19244 if( bgotonextstatement )
19245 {
19246 bool bgotonextstatement = true;
19247 do
19248 {
19249 evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
19250 if( IKabs(evalcond[0]) < 0.0000050000000000 )
19251 {
19252 bgotonextstatement=false;
19253 {
19254 IkReal j3eval[1];
19255 new_r02=0;
19256 new_r12=0;
19257 new_r20=0;
19258 new_r21=0;
19259 j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
19260 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19261 {
19262 {
19263 IkReal j3eval[1];
19264 new_r02=0;
19265 new_r12=0;
19266 new_r20=0;
19267 new_r21=0;
19268 j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
19269 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19270 {
19271 {
19272 IkReal j3eval[1];
19273 new_r02=0;
19274 new_r12=0;
19275 new_r20=0;
19276 new_r21=0;
19277 j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
19278 if( IKabs(j3eval[0]) < 0.0000010000000000 )
19279 {
19280 continue; // no branches [j3]
19281 
19282 } else
19283 {
19284 {
19285 IkReal j3array[2], cj3array[2], sj3array[2];
19286 bool j3valid[2]={false};
19287 _nj3 = 2;
19288 IkReal x1486=((-1.0)*new_r22);
19289 CheckValue<IkReal> x1488 = IKatan2WithCheck(IkReal((new_r00*x1486)),IkReal((new_r10*x1486)),IKFAST_ATAN2_MAGTHRESH);
19290 if(!x1488.valid){
19291 continue;
19292 }
19293 IkReal x1487=x1488.value;
19294 j3array[0]=((-1.0)*x1487);
19295 sj3array[0]=IKsin(j3array[0]);
19296 cj3array[0]=IKcos(j3array[0]);
19297 j3array[1]=((3.14159265358979)+(((-1.0)*x1487)));
19298 sj3array[1]=IKsin(j3array[1]);
19299 cj3array[1]=IKcos(j3array[1]);
19300 if( j3array[0] > IKPI )
19301 {
19302  j3array[0]-=IK2PI;
19303 }
19304 else if( j3array[0] < -IKPI )
19305 { j3array[0]+=IK2PI;
19306 }
19307 j3valid[0] = true;
19308 if( j3array[1] > IKPI )
19309 {
19310  j3array[1]-=IK2PI;
19311 }
19312 else if( j3array[1] < -IKPI )
19313 { j3array[1]+=IK2PI;
19314 }
19315 j3valid[1] = true;
19316 for(int ij3 = 0; ij3 < 2; ++ij3)
19317 {
19318 if( !j3valid[ij3] )
19319 {
19320  continue;
19321 }
19322 _ij3[0] = ij3; _ij3[1] = -1;
19323 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19324 {
19325 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19326 {
19327  j3valid[iij3]=false; _ij3[1] = iij3; break;
19328 }
19329 }
19330 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19331 {
19332 IkReal evalcond[5];
19333 IkReal x1489=IKsin(j3);
19334 IkReal x1490=IKcos(j3);
19335 IkReal x1491=((1.0)*new_r11);
19336 IkReal x1492=(new_r01*x1490);
19337 evalcond[0]=(((new_r11*x1489))+x1492);
19338 evalcond[1]=(((new_r00*x1490))+((new_r10*x1489)));
19339 evalcond[2]=(((new_r00*x1489))+(((-1.0)*new_r10*x1490)));
19340 evalcond[3]=(((new_r01*x1489))+(((-1.0)*x1490*x1491)));
19341 evalcond[4]=((((-1.0)*new_r22*x1492))+(((-1.0)*new_r22*x1489*x1491)));
19342 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19343 {
19344 continue;
19345 }
19346 }
19347 
19348 {
19349 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19350 vinfos[0].jointtype = 1;
19351 vinfos[0].foffset = j0;
19352 vinfos[0].indices[0] = _ij0[0];
19353 vinfos[0].indices[1] = _ij0[1];
19354 vinfos[0].maxsolutions = _nj0;
19355 vinfos[1].jointtype = 1;
19356 vinfos[1].foffset = j1;
19357 vinfos[1].indices[0] = _ij1[0];
19358 vinfos[1].indices[1] = _ij1[1];
19359 vinfos[1].maxsolutions = _nj1;
19360 vinfos[2].jointtype = 1;
19361 vinfos[2].foffset = j2;
19362 vinfos[2].indices[0] = _ij2[0];
19363 vinfos[2].indices[1] = _ij2[1];
19364 vinfos[2].maxsolutions = _nj2;
19365 vinfos[3].jointtype = 1;
19366 vinfos[3].foffset = j3;
19367 vinfos[3].indices[0] = _ij3[0];
19368 vinfos[3].indices[1] = _ij3[1];
19369 vinfos[3].maxsolutions = _nj3;
19370 vinfos[4].jointtype = 1;
19371 vinfos[4].foffset = j4;
19372 vinfos[4].indices[0] = _ij4[0];
19373 vinfos[4].indices[1] = _ij4[1];
19374 vinfos[4].maxsolutions = _nj4;
19375 vinfos[5].jointtype = 1;
19376 vinfos[5].foffset = j5;
19377 vinfos[5].indices[0] = _ij5[0];
19378 vinfos[5].indices[1] = _ij5[1];
19379 vinfos[5].maxsolutions = _nj5;
19380 std::vector<int> vfree(0);
19381 solutions.AddSolution(vinfos,vfree);
19382 }
19383 }
19384 }
19385 
19386 }
19387 
19388 }
19389 
19390 } else
19391 {
19392 {
19393 IkReal j3array[2], cj3array[2], sj3array[2];
19394 bool j3valid[2]={false};
19395 _nj3 = 2;
19396 CheckValue<IkReal> x1494 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
19397 if(!x1494.valid){
19398 continue;
19399 }
19400 IkReal x1493=x1494.value;
19401 j3array[0]=((-1.0)*x1493);
19402 sj3array[0]=IKsin(j3array[0]);
19403 cj3array[0]=IKcos(j3array[0]);
19404 j3array[1]=((3.14159265358979)+(((-1.0)*x1493)));
19405 sj3array[1]=IKsin(j3array[1]);
19406 cj3array[1]=IKcos(j3array[1]);
19407 if( j3array[0] > IKPI )
19408 {
19409  j3array[0]-=IK2PI;
19410 }
19411 else if( j3array[0] < -IKPI )
19412 { j3array[0]+=IK2PI;
19413 }
19414 j3valid[0] = true;
19415 if( j3array[1] > IKPI )
19416 {
19417  j3array[1]-=IK2PI;
19418 }
19419 else if( j3array[1] < -IKPI )
19420 { j3array[1]+=IK2PI;
19421 }
19422 j3valid[1] = true;
19423 for(int ij3 = 0; ij3 < 2; ++ij3)
19424 {
19425 if( !j3valid[ij3] )
19426 {
19427  continue;
19428 }
19429 _ij3[0] = ij3; _ij3[1] = -1;
19430 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19431 {
19432 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19433 {
19434  j3valid[iij3]=false; _ij3[1] = iij3; break;
19435 }
19436 }
19437 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19438 {
19439 IkReal evalcond[5];
19440 IkReal x1495=IKsin(j3);
19441 IkReal x1496=IKcos(j3);
19442 IkReal x1497=((1.0)*x1496);
19443 IkReal x1498=((1.0)*new_r22*x1495);
19444 evalcond[0]=(((new_r01*x1496))+((new_r11*x1495)));
19445 evalcond[1]=((((-1.0)*new_r10*x1497))+((new_r00*x1495)));
19446 evalcond[2]=(((new_r01*x1495))+(((-1.0)*new_r11*x1497)));
19447 evalcond[3]=((((-1.0)*new_r10*x1498))+(((-1.0)*new_r00*new_r22*x1497)));
19448 evalcond[4]=((((-1.0)*new_r11*x1498))+(((-1.0)*new_r01*new_r22*x1497)));
19449 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19450 {
19451 continue;
19452 }
19453 }
19454 
19455 {
19456 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19457 vinfos[0].jointtype = 1;
19458 vinfos[0].foffset = j0;
19459 vinfos[0].indices[0] = _ij0[0];
19460 vinfos[0].indices[1] = _ij0[1];
19461 vinfos[0].maxsolutions = _nj0;
19462 vinfos[1].jointtype = 1;
19463 vinfos[1].foffset = j1;
19464 vinfos[1].indices[0] = _ij1[0];
19465 vinfos[1].indices[1] = _ij1[1];
19466 vinfos[1].maxsolutions = _nj1;
19467 vinfos[2].jointtype = 1;
19468 vinfos[2].foffset = j2;
19469 vinfos[2].indices[0] = _ij2[0];
19470 vinfos[2].indices[1] = _ij2[1];
19471 vinfos[2].maxsolutions = _nj2;
19472 vinfos[3].jointtype = 1;
19473 vinfos[3].foffset = j3;
19474 vinfos[3].indices[0] = _ij3[0];
19475 vinfos[3].indices[1] = _ij3[1];
19476 vinfos[3].maxsolutions = _nj3;
19477 vinfos[4].jointtype = 1;
19478 vinfos[4].foffset = j4;
19479 vinfos[4].indices[0] = _ij4[0];
19480 vinfos[4].indices[1] = _ij4[1];
19481 vinfos[4].maxsolutions = _nj4;
19482 vinfos[5].jointtype = 1;
19483 vinfos[5].foffset = j5;
19484 vinfos[5].indices[0] = _ij5[0];
19485 vinfos[5].indices[1] = _ij5[1];
19486 vinfos[5].maxsolutions = _nj5;
19487 std::vector<int> vfree(0);
19488 solutions.AddSolution(vinfos,vfree);
19489 }
19490 }
19491 }
19492 
19493 }
19494 
19495 }
19496 
19497 } else
19498 {
19499 {
19500 IkReal j3array[2], cj3array[2], sj3array[2];
19501 bool j3valid[2]={false};
19502 _nj3 = 2;
19503 CheckValue<IkReal> x1500 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
19504 if(!x1500.valid){
19505 continue;
19506 }
19507 IkReal x1499=x1500.value;
19508 j3array[0]=((-1.0)*x1499);
19509 sj3array[0]=IKsin(j3array[0]);
19510 cj3array[0]=IKcos(j3array[0]);
19511 j3array[1]=((3.14159265358979)+(((-1.0)*x1499)));
19512 sj3array[1]=IKsin(j3array[1]);
19513 cj3array[1]=IKcos(j3array[1]);
19514 if( j3array[0] > IKPI )
19515 {
19516  j3array[0]-=IK2PI;
19517 }
19518 else if( j3array[0] < -IKPI )
19519 { j3array[0]+=IK2PI;
19520 }
19521 j3valid[0] = true;
19522 if( j3array[1] > IKPI )
19523 {
19524  j3array[1]-=IK2PI;
19525 }
19526 else if( j3array[1] < -IKPI )
19527 { j3array[1]+=IK2PI;
19528 }
19529 j3valid[1] = true;
19530 for(int ij3 = 0; ij3 < 2; ++ij3)
19531 {
19532 if( !j3valid[ij3] )
19533 {
19534  continue;
19535 }
19536 _ij3[0] = ij3; _ij3[1] = -1;
19537 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19538 {
19539 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19540 {
19541  j3valid[iij3]=false; _ij3[1] = iij3; break;
19542 }
19543 }
19544 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19545 {
19546 IkReal evalcond[5];
19547 IkReal x1501=IKsin(j3);
19548 IkReal x1502=IKcos(j3);
19549 IkReal x1503=((1.0)*new_r22);
19550 IkReal x1504=(new_r10*x1501);
19551 IkReal x1505=((1.0)*x1502);
19552 IkReal x1506=(new_r00*x1502);
19553 evalcond[0]=(x1506+x1504);
19554 evalcond[1]=((((-1.0)*new_r10*x1505))+((new_r00*x1501)));
19555 evalcond[2]=((((-1.0)*new_r11*x1505))+((new_r01*x1501)));
19556 evalcond[3]=((((-1.0)*x1503*x1506))+(((-1.0)*x1503*x1504)));
19557 evalcond[4]=((((-1.0)*new_r11*x1501*x1503))+(((-1.0)*new_r01*x1502*x1503)));
19558 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19559 {
19560 continue;
19561 }
19562 }
19563 
19564 {
19565 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19566 vinfos[0].jointtype = 1;
19567 vinfos[0].foffset = j0;
19568 vinfos[0].indices[0] = _ij0[0];
19569 vinfos[0].indices[1] = _ij0[1];
19570 vinfos[0].maxsolutions = _nj0;
19571 vinfos[1].jointtype = 1;
19572 vinfos[1].foffset = j1;
19573 vinfos[1].indices[0] = _ij1[0];
19574 vinfos[1].indices[1] = _ij1[1];
19575 vinfos[1].maxsolutions = _nj1;
19576 vinfos[2].jointtype = 1;
19577 vinfos[2].foffset = j2;
19578 vinfos[2].indices[0] = _ij2[0];
19579 vinfos[2].indices[1] = _ij2[1];
19580 vinfos[2].maxsolutions = _nj2;
19581 vinfos[3].jointtype = 1;
19582 vinfos[3].foffset = j3;
19583 vinfos[3].indices[0] = _ij3[0];
19584 vinfos[3].indices[1] = _ij3[1];
19585 vinfos[3].maxsolutions = _nj3;
19586 vinfos[4].jointtype = 1;
19587 vinfos[4].foffset = j4;
19588 vinfos[4].indices[0] = _ij4[0];
19589 vinfos[4].indices[1] = _ij4[1];
19590 vinfos[4].maxsolutions = _nj4;
19591 vinfos[5].jointtype = 1;
19592 vinfos[5].foffset = j5;
19593 vinfos[5].indices[0] = _ij5[0];
19594 vinfos[5].indices[1] = _ij5[1];
19595 vinfos[5].maxsolutions = _nj5;
19596 std::vector<int> vfree(0);
19597 solutions.AddSolution(vinfos,vfree);
19598 }
19599 }
19600 }
19601 
19602 }
19603 
19604 }
19605 
19606 }
19607 } while(0);
19608 if( bgotonextstatement )
19609 {
19610 bool bgotonextstatement = true;
19611 do
19612 {
19613 if( 1 )
19614 {
19615 bgotonextstatement=false;
19616 continue; // branch miss [j3]
19617 
19618 }
19619 } while(0);
19620 if( bgotonextstatement )
19621 {
19622 }
19623 }
19624 }
19625 }
19626 }
19627 
19628 } else
19629 {
19630 {
19631 IkReal j3array[1], cj3array[1], sj3array[1];
19632 bool j3valid[1]={false};
19633 _nj3 = 1;
19634 CheckValue<IkReal> x1508=IKPowWithIntegerCheck(sj4,-1);
19635 if(!x1508.valid){
19636 continue;
19637 }
19638 IkReal x1507=x1508.value;
19639 CheckValue<IkReal> x1509=IKPowWithIntegerCheck(new_r12,-1);
19640 if(!x1509.valid){
19641 continue;
19642 }
19643 if( IKabs((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x1507)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x1507))-1) <= IKFAST_SINCOS_THRESH )
19644  continue;
19645 j3array[0]=IKatan2((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1507));
19646 sj3array[0]=IKsin(j3array[0]);
19647 cj3array[0]=IKcos(j3array[0]);
19648 if( j3array[0] > IKPI )
19649 {
19650  j3array[0]-=IK2PI;
19651 }
19652 else if( j3array[0] < -IKPI )
19653 { j3array[0]+=IK2PI;
19654 }
19655 j3valid[0] = true;
19656 for(int ij3 = 0; ij3 < 1; ++ij3)
19657 {
19658 if( !j3valid[ij3] )
19659 {
19660  continue;
19661 }
19662 _ij3[0] = ij3; _ij3[1] = -1;
19663 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19664 {
19665 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19666 {
19667  j3valid[iij3]=false; _ij3[1] = iij3; break;
19668 }
19669 }
19670 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19671 {
19672 IkReal evalcond[18];
19673 IkReal x1510=IKcos(j3);
19674 IkReal x1511=IKsin(j3);
19675 IkReal x1512=(cj4*cj5);
19676 IkReal x1513=(cj4*sj5);
19677 IkReal x1514=((1.0)*new_r20);
19678 IkReal x1515=((1.0)*cj4);
19679 IkReal x1516=((1.0)*sj4);
19680 IkReal x1517=(sj4*x1511);
19681 IkReal x1518=((1.0)*x1510);
19682 IkReal x1519=(sj4*x1510);
19683 IkReal x1520=(x1511*x1515);
19684 evalcond[0]=(x1519+new_r02);
19685 evalcond[1]=(x1517+new_r12);
19686 evalcond[2]=((((-1.0)*new_r02*x1511))+((new_r12*x1510)));
19687 evalcond[3]=(sj4+((new_r02*x1510))+((new_r12*x1511)));
19688 evalcond[4]=(sj5+(((-1.0)*new_r10*x1518))+((new_r00*x1511)));
19689 evalcond[5]=((((-1.0)*new_r11*x1518))+cj5+((new_r01*x1511)));
19690 evalcond[6]=(((x1510*x1513))+((cj5*x1511))+new_r01);
19691 evalcond[7]=(x1513+((new_r01*x1510))+((new_r11*x1511)));
19692 evalcond[8]=(((sj5*x1511))+(((-1.0)*x1512*x1518))+new_r00);
19693 evalcond[9]=((((-1.0)*cj5*x1518))+new_r11+((x1511*x1513)));
19694 evalcond[10]=(((new_r10*x1511))+(((-1.0)*x1512))+((new_r00*x1510)));
19695 evalcond[11]=((((-1.0)*x1511*x1512))+(((-1.0)*sj5*x1518))+new_r10);
19696 evalcond[12]=(((new_r10*x1517))+(((-1.0)*cj4*x1514))+((new_r00*x1519)));
19697 evalcond[13]=((((-1.0)*new_r21*x1515))+((new_r01*x1519))+((new_r11*x1517)));
19698 evalcond[14]=((1.0)+(((-1.0)*new_r22*x1515))+((new_r02*x1519))+((new_r12*x1517)));
19699 evalcond[15]=((((-1.0)*new_r22*x1516))+(((-1.0)*new_r12*x1520))+(((-1.0)*new_r02*x1510*x1515)));
19700 evalcond[16]=(cj5+(((-1.0)*new_r00*x1510*x1515))+(((-1.0)*sj4*x1514))+(((-1.0)*new_r10*x1520)));
19701 evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r11*x1520))+(((-1.0)*new_r21*x1516))+(((-1.0)*new_r01*x1510*x1515)));
19702 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
19703 {
19704 continue;
19705 }
19706 }
19707 
19708 {
19709 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19710 vinfos[0].jointtype = 1;
19711 vinfos[0].foffset = j0;
19712 vinfos[0].indices[0] = _ij0[0];
19713 vinfos[0].indices[1] = _ij0[1];
19714 vinfos[0].maxsolutions = _nj0;
19715 vinfos[1].jointtype = 1;
19716 vinfos[1].foffset = j1;
19717 vinfos[1].indices[0] = _ij1[0];
19718 vinfos[1].indices[1] = _ij1[1];
19719 vinfos[1].maxsolutions = _nj1;
19720 vinfos[2].jointtype = 1;
19721 vinfos[2].foffset = j2;
19722 vinfos[2].indices[0] = _ij2[0];
19723 vinfos[2].indices[1] = _ij2[1];
19724 vinfos[2].maxsolutions = _nj2;
19725 vinfos[3].jointtype = 1;
19726 vinfos[3].foffset = j3;
19727 vinfos[3].indices[0] = _ij3[0];
19728 vinfos[3].indices[1] = _ij3[1];
19729 vinfos[3].maxsolutions = _nj3;
19730 vinfos[4].jointtype = 1;
19731 vinfos[4].foffset = j4;
19732 vinfos[4].indices[0] = _ij4[0];
19733 vinfos[4].indices[1] = _ij4[1];
19734 vinfos[4].maxsolutions = _nj4;
19735 vinfos[5].jointtype = 1;
19736 vinfos[5].foffset = j5;
19737 vinfos[5].indices[0] = _ij5[0];
19738 vinfos[5].indices[1] = _ij5[1];
19739 vinfos[5].maxsolutions = _nj5;
19740 std::vector<int> vfree(0);
19741 solutions.AddSolution(vinfos,vfree);
19742 }
19743 }
19744 }
19745 
19746 }
19747 
19748 }
19749 
19750 } else
19751 {
19752 {
19753 IkReal j3array[1], cj3array[1], sj3array[1];
19754 bool j3valid[1]={false};
19755 _nj3 = 1;
19756 CheckValue<IkReal> x1521=IKPowWithIntegerCheck(IKsign(sj4),-1);
19757 if(!x1521.valid){
19758 continue;
19759 }
19760 CheckValue<IkReal> x1522 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
19761 if(!x1522.valid){
19762 continue;
19763 }
19764 j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1521.value)))+(x1522.value));
19765 sj3array[0]=IKsin(j3array[0]);
19766 cj3array[0]=IKcos(j3array[0]);
19767 if( j3array[0] > IKPI )
19768 {
19769  j3array[0]-=IK2PI;
19770 }
19771 else if( j3array[0] < -IKPI )
19772 { j3array[0]+=IK2PI;
19773 }
19774 j3valid[0] = true;
19775 for(int ij3 = 0; ij3 < 1; ++ij3)
19776 {
19777 if( !j3valid[ij3] )
19778 {
19779  continue;
19780 }
19781 _ij3[0] = ij3; _ij3[1] = -1;
19782 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19783 {
19784 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19785 {
19786  j3valid[iij3]=false; _ij3[1] = iij3; break;
19787 }
19788 }
19789 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19790 {
19791 IkReal evalcond[18];
19792 IkReal x1523=IKcos(j3);
19793 IkReal x1524=IKsin(j3);
19794 IkReal x1525=(cj4*cj5);
19795 IkReal x1526=(cj4*sj5);
19796 IkReal x1527=((1.0)*new_r20);
19797 IkReal x1528=((1.0)*cj4);
19798 IkReal x1529=((1.0)*sj4);
19799 IkReal x1530=(sj4*x1524);
19800 IkReal x1531=((1.0)*x1523);
19801 IkReal x1532=(sj4*x1523);
19802 IkReal x1533=(x1524*x1528);
19803 evalcond[0]=(x1532+new_r02);
19804 evalcond[1]=(x1530+new_r12);
19805 evalcond[2]=(((new_r12*x1523))+(((-1.0)*new_r02*x1524)));
19806 evalcond[3]=(((new_r12*x1524))+sj4+((new_r02*x1523)));
19807 evalcond[4]=((((-1.0)*new_r10*x1531))+sj5+((new_r00*x1524)));
19808 evalcond[5]=(cj5+((new_r01*x1524))+(((-1.0)*new_r11*x1531)));
19809 evalcond[6]=(((x1523*x1526))+((cj5*x1524))+new_r01);
19810 evalcond[7]=(((new_r01*x1523))+x1526+((new_r11*x1524)));
19811 evalcond[8]=(((sj5*x1524))+(((-1.0)*x1525*x1531))+new_r00);
19812 evalcond[9]=(((x1524*x1526))+(((-1.0)*cj5*x1531))+new_r11);
19813 evalcond[10]=(((new_r00*x1523))+((new_r10*x1524))+(((-1.0)*x1525)));
19814 evalcond[11]=((((-1.0)*x1524*x1525))+new_r10+(((-1.0)*sj5*x1531)));
19815 evalcond[12]=((((-1.0)*cj4*x1527))+((new_r00*x1532))+((new_r10*x1530)));
19816 evalcond[13]=((((-1.0)*new_r21*x1528))+((new_r01*x1532))+((new_r11*x1530)));
19817 evalcond[14]=((1.0)+((new_r02*x1532))+(((-1.0)*new_r22*x1528))+((new_r12*x1530)));
19818 evalcond[15]=((((-1.0)*new_r02*x1523*x1528))+(((-1.0)*new_r22*x1529))+(((-1.0)*new_r12*x1533)));
19819 evalcond[16]=((((-1.0)*new_r10*x1533))+(((-1.0)*sj4*x1527))+cj5+(((-1.0)*new_r00*x1523*x1528)));
19820 evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r21*x1529))+(((-1.0)*new_r01*x1523*x1528))+(((-1.0)*new_r11*x1533)));
19821 if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || 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 )
19822 {
19823 continue;
19824 }
19825 }
19826 
19827 {
19828 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19829 vinfos[0].jointtype = 1;
19830 vinfos[0].foffset = j0;
19831 vinfos[0].indices[0] = _ij0[0];
19832 vinfos[0].indices[1] = _ij0[1];
19833 vinfos[0].maxsolutions = _nj0;
19834 vinfos[1].jointtype = 1;
19835 vinfos[1].foffset = j1;
19836 vinfos[1].indices[0] = _ij1[0];
19837 vinfos[1].indices[1] = _ij1[1];
19838 vinfos[1].maxsolutions = _nj1;
19839 vinfos[2].jointtype = 1;
19840 vinfos[2].foffset = j2;
19841 vinfos[2].indices[0] = _ij2[0];
19842 vinfos[2].indices[1] = _ij2[1];
19843 vinfos[2].maxsolutions = _nj2;
19844 vinfos[3].jointtype = 1;
19845 vinfos[3].foffset = j3;
19846 vinfos[3].indices[0] = _ij3[0];
19847 vinfos[3].indices[1] = _ij3[1];
19848 vinfos[3].maxsolutions = _nj3;
19849 vinfos[4].jointtype = 1;
19850 vinfos[4].foffset = j4;
19851 vinfos[4].indices[0] = _ij4[0];
19852 vinfos[4].indices[1] = _ij4[1];
19853 vinfos[4].maxsolutions = _nj4;
19854 vinfos[5].jointtype = 1;
19855 vinfos[5].foffset = j5;
19856 vinfos[5].indices[0] = _ij5[0];
19857 vinfos[5].indices[1] = _ij5[1];
19858 vinfos[5].maxsolutions = _nj5;
19859 std::vector<int> vfree(0);
19860 solutions.AddSolution(vinfos,vfree);
19861 }
19862 }
19863 }
19864 
19865 }
19866 
19867 }
19868 }
19869 }
19870 
19871 }
19872 
19873 }
19874 }
19875 }
19876 }
19877 }static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
19878 {
19879  using std::complex;
19880  if( rawcoeffs[0] == 0 ) {
19881  // solve with one reduced degree
19882  polyroots2(&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[3];
19889  const int maxsteps = 110;
19890  for(int i = 0; i < 3; ++i) {
19891  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19892  }
19893  complex<IkReal> roots[3];
19894  IkReal err[3];
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 < 3; ++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 < 3; ++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 < 3; ++j) {
19911  x = roots[i] * x + coeffs[j];
19912  }
19913  for(int j = 0; j < 3; ++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[3] = {false};
19931  for(int i = 0; i < 3; ++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 < 3; ++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 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
19956  IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19957  if( det < 0 ) {
19958  numroots=0;
19959  }
19960  else if( det == 0 ) {
19961  rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19962  numroots = 1;
19963  }
19964  else {
19965  det = IKsqrt(det);
19966  rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19967  rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
19968  numroots = 2;
19969  }
19970 }
19971 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
19972 {
19973  using std::complex;
19974  if( rawcoeffs[0] == 0 ) {
19975  // solve with one reduced degree
19976  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19977  return;
19978  }
19979  IKFAST_ASSERT(rawcoeffs[0] != 0);
19980  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19981  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19982  complex<IkReal> coeffs[4];
19983  const int maxsteps = 110;
19984  for(int i = 0; i < 4; ++i) {
19985  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19986  }
19987  complex<IkReal> roots[4];
19988  IkReal err[4];
19989  roots[0] = complex<IkReal>(1,0);
19990  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19991  err[0] = 1.0;
19992  err[1] = 1.0;
19993  for(int i = 2; i < 4; ++i) {
19994  roots[i] = roots[i-1]*roots[1];
19995  err[i] = 1.0;
19996  }
19997  for(int step = 0; step < maxsteps; ++step) {
19998  bool changed = false;
19999  for(int i = 0; i < 4; ++i) {
20000  if ( err[i] >= tol ) {
20001  changed = true;
20002  // evaluate
20003  complex<IkReal> x = roots[i] + coeffs[0];
20004  for(int j = 1; j < 4; ++j) {
20005  x = roots[i] * x + coeffs[j];
20006  }
20007  for(int j = 0; j < 4; ++j) {
20008  if( i != j ) {
20009  if( roots[i] != roots[j] ) {
20010  x /= (roots[i] - roots[j]);
20011  }
20012  }
20013  }
20014  roots[i] -= x;
20015  err[i] = abs(x);
20016  }
20017  }
20018  if( !changed ) {
20019  break;
20020  }
20021  }
20022 
20023  numroots = 0;
20024  bool visited[4] = {false};
20025  for(int i = 0; i < 4; ++i) {
20026  if( !visited[i] ) {
20027  // might be a multiple root, in which case it will have more error than the other roots
20028  // find any neighboring roots, and take the average
20029  complex<IkReal> newroot=roots[i];
20030  int n = 1;
20031  for(int j = i+1; j < 4; ++j) {
20032  // care about error in real much more than imaginary
20033  if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
20034  newroot += roots[j];
20035  n += 1;
20036  visited[j] = true;
20037  }
20038  }
20039  if( n > 1 ) {
20040  newroot /= n;
20041  }
20042  // 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
20043  if( IKabs(imag(newroot)) < tolsqrt ) {
20044  rawroots[numroots++] = real(newroot);
20045  }
20046  }
20047  }
20048 }
20049 };
20050 
20051 
20054 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
20055 IKSolver solver;
20056 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20057 }
20058 
20059 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
20060 IKSolver solver;
20061 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20062 }
20063 
20064 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - prbt (11ad975bd60aa3f28e8d228465d0f213)>"; }
20065 
20066 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
20067 
20068 #ifdef IKFAST_NAMESPACE
20069 } // end namespace
20070 #endif
20071 
20072 #ifndef IKFAST_NO_MAIN
20073 #include <stdio.h>
20074 #include <stdlib.h>
20075 #ifdef IKFAST_NAMESPACE
20076 using namespace IKFAST_NAMESPACE;
20077 #endif
20078 int main(int argc, char** argv)
20079 {
20080  if( argc != 12+GetNumFreeParameters()+1 ) {
20081  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
20082  "Returns the ik solutions given the transformation of the end effector specified by\n"
20083  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
20084  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
20085  return 1;
20086  }
20087 
20088  IkSolutionList<IkReal> solutions;
20089  std::vector<IkReal> vfree(GetNumFreeParameters());
20090  IkReal eerot[9],eetrans[3];
20091  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
20092  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
20093  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20094  for(std::size_t i = 0; i < vfree.size(); ++i)
20095  vfree[i] = atof(argv[13+i]);
20096  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
20097 
20098  if( !bSuccess ) {
20099  fprintf(stderr,"Failed to get ik solution\n");
20100  return -1;
20101  }
20102 
20103  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
20104  std::vector<IkReal> solvalues(GetNumJoints());
20105  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
20106  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
20107  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
20108  std::vector<IkReal> vsolfree(sol.GetFree().size());
20109  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
20110  for( std::size_t j = 0; j < solvalues.size(); ++j)
20111  printf("%.15f, ", solvalues[j]);
20112  printf("\n");
20113  }
20114  return 0;
20115 }
20116 
20117 #endif
IKfmod
float IKfmod(float x, float y)
Definition: prbt_manipulator_ikfast_solver.cpp:131
ikfast
Definition: ikfast.h:45
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKsign
float IKsign(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:219
IKatan2
float IKatan2(float fy, float fx)
Definition: prbt_manipulator_ikfast_solver.cpp:173
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: prbt_manipulator_ikfast_solver.cpp:20066
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: prbt_manipulator_ikfast_solver.cpp:20064
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: prbt_manipulator_ikfast_solver.cpp:360
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: prbt_manipulator_ikfast_solver.cpp:20054
IKsqrt
float IKsqrt(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:168
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: prbt_manipulator_ikfast_solver.cpp:293
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:111
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: prbt_manipulator_ikfast_solver.cpp:867
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: prbt_manipulator_ikfast_solver.cpp:170
GetIkType
IKFAST_API int GetIkType()
Definition: prbt_manipulator_ikfast_solver.cpp:364
IKasin
float IKasin(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:115
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: prbt_manipulator_ikfast_solver.cpp:53
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)
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: prbt_manipulator_ikfast_solver.cpp:19877
IKabs
float IKabs(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:85
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: prbt_manipulator_ikfast_solver.cpp:358
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: prbt_manipulator_ikfast_solver.cpp:359
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: prbt_manipulator_ikfast_solver.cpp:101
IKsin
float IKsin(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:162
IKPI
#define IKPI
Definition: prbt_manipulator_ikfast_solver.cpp:64
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: prbt_manipulator_ikfast_solver.cpp:205
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:106
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: prbt_manipulator_ikfast_solver.cpp:373
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: prbt_manipulator_ikfast_solver.cpp:19971
IK2PI
#define IK2PI
Definition: prbt_manipulator_ikfast_solver.cpp:63
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: prbt_manipulator_ikfast_solver.cpp:29
step
unsigned int step
IKlog
float IKlog(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:91
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)
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: prbt_manipulator_ikfast_solver.cpp:240
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: prbt_manipulator_ikfast_solver.cpp:96
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: prbt_manipulator_ikfast_solver.cpp:20059
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:43
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: prbt_manipulator_ikfast_solver.cpp:362
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: prbt_manipulator_ikfast_solver.cpp:19955
IKacos
float IKacos(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:148
IKcos
float IKcos(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:164
x
double x
IKsqr
float IKsqr(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:88
main
int main(int argc, char **argv)
Definition: prbt_manipulator_ikfast_solver.cpp:20078
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:100
IKtan
float IKtan(float f)
Definition: prbt_manipulator_ikfast_solver.cpp:166
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
IKPI_2
#define IKPI_2
Definition: prbt_manipulator_ikfast_solver.cpp:65

prbt_ikfast_manipulator_plugin
Author(s):
autogenerated on Mon May 3 2021 02:19:27